561 lines
20 KiB
C++
561 lines
20 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2014-, Open Perception, Inc.
|
|
* Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel.
|
|
*
|
|
* All rights reserved
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/common/common.h>
|
|
#include <pcl/registration/matching_candidate.h>
|
|
#include <pcl/registration/registration.h>
|
|
|
|
namespace pcl {
|
|
/** \brief Compute the mean point density of a given point cloud.
|
|
* \param[in] cloud pointer to the input point cloud
|
|
* \param[in] max_dist maximum distance of a point to be considered as a neighbor
|
|
* \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag
|
|
* is set) \return the mean point density of a given point cloud
|
|
*/
|
|
template <typename PointT>
|
|
inline float
|
|
getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
|
|
float max_dist,
|
|
int nr_threads = 1);
|
|
|
|
/** \brief Compute the mean point density of a given point cloud.
|
|
* \param[in] cloud pointer to the input point cloud
|
|
* \param[in] indices the vector of point indices to use from \a cloud
|
|
* \param[in] max_dist maximum distance of a point to be considered as a neighbor
|
|
* \param[in] nr_threads number of threads to use (default = 1, only used if OpenMP flag
|
|
* is set) \return the mean point density of a given point cloud
|
|
*/
|
|
template <typename PointT>
|
|
inline float
|
|
getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
|
|
const pcl::Indices& indices,
|
|
float max_dist,
|
|
int nr_threads = 1);
|
|
|
|
namespace registration {
|
|
/** \brief FPCSInitialAlignment computes corresponding four point congruent sets as
|
|
* described in: "4-points congruent sets for robust pairwise surface registration",
|
|
* Dror Aiger, Niloy Mitra, Daniel Cohen-Or. ACM Transactions on Graphics, vol. 27(3),
|
|
* 2008 \author P.W.Theiler \ingroup registration
|
|
*/
|
|
template <typename PointSource,
|
|
typename PointTarget,
|
|
typename NormalT = pcl::Normal,
|
|
typename Scalar = float>
|
|
class FPCSInitialAlignment : public Registration<PointSource, PointTarget, Scalar> {
|
|
public:
|
|
/** \cond */
|
|
using Ptr =
|
|
shared_ptr<FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
|
|
using ConstPtr =
|
|
shared_ptr<const FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
|
|
|
|
using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
|
|
using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
|
|
|
|
using PointCloudTarget = pcl::PointCloud<PointTarget>;
|
|
using PointCloudSource = pcl::PointCloud<PointSource>;
|
|
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
|
|
using PointCloudSourceIterator = typename PointCloudSource::iterator;
|
|
|
|
using Normals = pcl::PointCloud<NormalT>;
|
|
using NormalsConstPtr = typename Normals::ConstPtr;
|
|
|
|
using MatchingCandidate = pcl::registration::MatchingCandidate;
|
|
using MatchingCandidates = pcl::registration::MatchingCandidates;
|
|
/** \endcond */
|
|
|
|
/** \brief Constructor.
|
|
* Resets the maximum number of iterations to 0 thus forcing an internal computation
|
|
* if not set by the user. Sets the number of RANSAC iterations to 1000 and the
|
|
* standard transformation estimation to TransformationEstimation3Point.
|
|
*/
|
|
FPCSInitialAlignment();
|
|
|
|
/** \brief Destructor. */
|
|
~FPCSInitialAlignment(){};
|
|
|
|
/** \brief Provide a pointer to the vector of target indices.
|
|
* \param[in] target_indices a pointer to the target indices
|
|
*/
|
|
inline void
|
|
setTargetIndices(const IndicesPtr& target_indices)
|
|
{
|
|
target_indices_ = target_indices;
|
|
};
|
|
|
|
/** \return a pointer to the vector of target indices. */
|
|
inline IndicesPtr
|
|
getTargetIndices() const
|
|
{
|
|
return (target_indices_);
|
|
};
|
|
|
|
/** \brief Provide a pointer to the normals of the source point cloud.
|
|
* \param[in] source_normals pointer to the normals of the source pointer cloud.
|
|
*/
|
|
inline void
|
|
setSourceNormals(const NormalsConstPtr& source_normals)
|
|
{
|
|
source_normals_ = source_normals;
|
|
};
|
|
|
|
/** \return the normals of the source point cloud. */
|
|
inline NormalsConstPtr
|
|
getSourceNormals() const
|
|
{
|
|
return (source_normals_);
|
|
};
|
|
|
|
/** \brief Provide a pointer to the normals of the target point cloud.
|
|
* \param[in] target_normals point to the normals of the target point cloud.
|
|
*/
|
|
inline void
|
|
setTargetNormals(const NormalsConstPtr& target_normals)
|
|
{
|
|
target_normals_ = target_normals;
|
|
};
|
|
|
|
/** \return the normals of the target point cloud. */
|
|
inline NormalsConstPtr
|
|
getTargetNormals() const
|
|
{
|
|
return (target_normals_);
|
|
};
|
|
|
|
/** \brief Set the number of used threads if OpenMP is activated.
|
|
* \param[in] nr_threads the number of used threads
|
|
*/
|
|
inline void
|
|
setNumberOfThreads(int nr_threads)
|
|
{
|
|
nr_threads_ = nr_threads;
|
|
};
|
|
|
|
/** \return the number of threads used if OpenMP is activated. */
|
|
inline int
|
|
getNumberOfThreads() const
|
|
{
|
|
return (nr_threads_);
|
|
};
|
|
|
|
/** \brief Set the constant factor delta which weights the internally calculated
|
|
* parameters. \param[in] delta the weight factor delta \param[in] normalize flag if
|
|
* delta should be normalized according to point cloud density
|
|
*/
|
|
inline void
|
|
setDelta(float delta, bool normalize = false)
|
|
{
|
|
delta_ = delta;
|
|
normalize_delta_ = normalize;
|
|
};
|
|
|
|
/** \return the constant factor delta which weights the internally calculated
|
|
* parameters. */
|
|
inline float
|
|
getDelta() const
|
|
{
|
|
return (delta_);
|
|
};
|
|
|
|
/** \brief Set the approximate overlap between source and target.
|
|
* \param[in] approx_overlap the estimated overlap
|
|
*/
|
|
inline void
|
|
setApproxOverlap(float approx_overlap)
|
|
{
|
|
approx_overlap_ = approx_overlap;
|
|
};
|
|
|
|
/** \return the approximated overlap between source and target. */
|
|
inline float
|
|
getApproxOverlap() const
|
|
{
|
|
return (approx_overlap_);
|
|
};
|
|
|
|
/** \brief Set the scoring threshold used for early finishing the method.
|
|
* \param[in] score_threshold early terminating score criteria
|
|
*/
|
|
inline void
|
|
setScoreThreshold(float score_threshold)
|
|
{
|
|
score_threshold_ = score_threshold;
|
|
};
|
|
|
|
/** \return the scoring threshold used for early finishing the method. */
|
|
inline float
|
|
getScoreThreshold() const
|
|
{
|
|
return (score_threshold_);
|
|
};
|
|
|
|
/** \brief Set the number of source samples to use during alignment.
|
|
* \param[in] nr_samples the number of source samples
|
|
*/
|
|
inline void
|
|
setNumberOfSamples(int nr_samples)
|
|
{
|
|
nr_samples_ = nr_samples;
|
|
};
|
|
|
|
/** \return the number of source samples to use during alignment. */
|
|
inline int
|
|
getNumberOfSamples() const
|
|
{
|
|
return (nr_samples_);
|
|
};
|
|
|
|
/** \brief Set the maximum normal difference between valid point correspondences in
|
|
* degree. \param[in] max_norm_diff the maximum difference in degree
|
|
*/
|
|
inline void
|
|
setMaxNormalDifference(float max_norm_diff)
|
|
{
|
|
max_norm_diff_ = max_norm_diff;
|
|
};
|
|
|
|
/** \return the maximum normal difference between valid point correspondences in
|
|
* degree. */
|
|
inline float
|
|
getMaxNormalDifference() const
|
|
{
|
|
return (max_norm_diff_);
|
|
};
|
|
|
|
/** \brief Set the maximum computation time in seconds.
|
|
* \param[in] max_runtime the maximum runtime of the method in seconds
|
|
*/
|
|
inline void
|
|
setMaxComputationTime(int max_runtime)
|
|
{
|
|
max_runtime_ = max_runtime;
|
|
};
|
|
|
|
/** \return the maximum computation time in seconds. */
|
|
inline int
|
|
getMaxComputationTime() const
|
|
{
|
|
return (max_runtime_);
|
|
};
|
|
|
|
/** \return the fitness score of the best scored four-point match. */
|
|
inline float
|
|
getFitnessScore() const
|
|
{
|
|
return (fitness_score_);
|
|
};
|
|
|
|
protected:
|
|
using PCLBase<PointSource>::deinitCompute;
|
|
using PCLBase<PointSource>::input_;
|
|
using PCLBase<PointSource>::indices_;
|
|
|
|
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
|
|
using Registration<PointSource, PointTarget, Scalar>::target_;
|
|
using Registration<PointSource, PointTarget, Scalar>::tree_;
|
|
using Registration<PointSource, PointTarget, Scalar>::correspondences_;
|
|
using Registration<PointSource, PointTarget, Scalar>::target_cloud_updated_;
|
|
using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
|
|
using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
|
|
using Registration<PointSource, PointTarget, Scalar>::ransac_iterations_;
|
|
using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
|
|
using Registration<PointSource, PointTarget, Scalar>::converged_;
|
|
|
|
/** \brief Rigid transformation computation method.
|
|
* \param output the transformed input point cloud dataset using the rigid
|
|
* transformation found \param guess The computed transforamtion
|
|
*/
|
|
void
|
|
computeTransformation(PointCloudSource& output,
|
|
const Eigen::Matrix4f& guess) override;
|
|
|
|
/** \brief Internal computation initialization. */
|
|
virtual bool
|
|
initCompute();
|
|
|
|
/** \brief Select an approximately coplanar set of four points from the source cloud.
|
|
* \param[out] base_indices selected source cloud indices, further used as base (B)
|
|
* \param[out] ratio the two diagonal intersection ratios (r1,r2) of the base points
|
|
* \return
|
|
* * < 0 no coplanar four point sets with large enough sampling distance was found
|
|
* * = 0 a set of four congruent points was selected
|
|
*/
|
|
int
|
|
selectBase(pcl::Indices& base_indices, float (&ratio)[2]);
|
|
|
|
/** \brief Select randomly a triplet of points with large point-to-point distances.
|
|
* The minimum point sampling distance is calculated based on the estimated point
|
|
* cloud overlap during initialization.
|
|
*
|
|
* \param[out] base_indices indices of base B
|
|
* \return
|
|
* * < 0 no triangle with large enough base lines could be selected
|
|
* * = 0 base triangle succesully selected
|
|
*/
|
|
int
|
|
selectBaseTriangle(pcl::Indices& base_indices);
|
|
|
|
/** \brief Setup the base (four coplanar points) by ordering the points and computing
|
|
* intersection ratios and segment to segment distances of base diagonal.
|
|
*
|
|
* \param[in,out] base_indices indices of base B (will be reordered)
|
|
* \param[out] ratio diagonal intersection ratios of base points
|
|
*/
|
|
void
|
|
setupBase(pcl::Indices& base_indices, float (&ratio)[2]);
|
|
|
|
/** \brief Calculate intersection ratios and segment to segment distances of base
|
|
* diagonals. \param[in] base_indices indices of base B \param[out] ratio diagonal
|
|
* intersection ratios of base points \return quality value of diagonal intersection
|
|
*/
|
|
float
|
|
segmentToSegmentDist(const pcl::Indices& base_indices, float (&ratio)[2]);
|
|
|
|
/** \brief Search for corresponding point pairs given the distance between two base
|
|
* points.
|
|
*
|
|
* \param[in] idx1 first index of current base segment (in source cloud)
|
|
* \param[in] idx2 second index of current base segment (in source cloud)
|
|
* \param[out] pairs resulting point pairs with point-to-point distance close to
|
|
* ref_dist \return
|
|
* * < 0 no corresponding point pair was found
|
|
* * = 0 at least one point pair candidate was found
|
|
*/
|
|
virtual int
|
|
bruteForceCorrespondences(int idx1, int idx2, pcl::Correspondences& pairs);
|
|
|
|
/** \brief Determine base matches by combining the point pair candidate and search for
|
|
* coinciding intersection points using the diagonal segment ratios of base B. The
|
|
* coincidation threshold is calculated during initialization (coincidation_limit_).
|
|
*
|
|
* \param[in] base_indices indices of base B
|
|
* \param[out] matches vector of candidate matches w.r.t the base B
|
|
* \param[in] pairs_a point pairs corresponding to points of 1st diagonal of base B
|
|
* \param[in] pairs_b point pairs corresponding to points of 2nd diagonal of base B
|
|
* \param[in] ratio diagonal intersection ratios of base points
|
|
* \return
|
|
* * < 0 no base match could be found
|
|
* * = 0 at least one base match was found
|
|
*/
|
|
virtual int
|
|
determineBaseMatches(const pcl::Indices& base_indices,
|
|
std::vector<pcl::Indices>& matches,
|
|
const pcl::Correspondences& pairs_a,
|
|
const pcl::Correspondences& pairs_b,
|
|
const float (&ratio)[2]);
|
|
|
|
/** \brief Check if outer rectangle distance of matched points fit with the base
|
|
* rectangle.
|
|
*
|
|
* \param[in] match_indices indices of match M
|
|
* \param[in] ds edge lengths of base B
|
|
* \return
|
|
* * < 0 at least one edge of the match M has no corresponding one in the base B
|
|
* * = 0 edges of match M fits to the ones of base B
|
|
*/
|
|
int
|
|
checkBaseMatch(const pcl::Indices& match_indices, const float (&ds)[4]);
|
|
|
|
/** \brief Method to handle current candidate matches. Here we validate and evaluate
|
|
* the matches w.r.t the base and store the best fitting match (together with its
|
|
* score and estimated transformation). \note For forwards compatibility the results
|
|
* are stored in 'vectors of size 1'.
|
|
*
|
|
* \param[in] base_indices indices of base B
|
|
* \param[in,out] matches vector of candidate matches w.r.t the base B. The candidate
|
|
* matches are reordered during this step. \param[out] candidates vector which
|
|
* contains the candidates matches M
|
|
*/
|
|
virtual void
|
|
handleMatches(const pcl::Indices& base_indices,
|
|
std::vector<pcl::Indices>& matches,
|
|
MatchingCandidates& candidates);
|
|
|
|
/** \brief Sets the correspondences between the base B and the match M by using the
|
|
* distance of each point to the centroid of the rectangle.
|
|
*
|
|
* \param[in] base_indices indices of base B
|
|
* \param[in] match_indices indices of match M
|
|
* \param[out] correspondences resulting correspondences
|
|
*/
|
|
virtual void
|
|
linkMatchWithBase(const pcl::Indices& base_indices,
|
|
pcl::Indices& match_indices,
|
|
pcl::Correspondences& correspondences);
|
|
|
|
/** \brief Validate the matching by computing the transformation between the source
|
|
* and target based on the four matched points and by comparing the mean square error
|
|
* (MSE) to a threshold. The MSE limit was calculated during initialization
|
|
* (max_mse_).
|
|
*
|
|
* \param[in] base_indices indices of base B
|
|
* \param[in] match_indices indices of match M
|
|
* \param[in] correspondences corresondences between source and target
|
|
* \param[out] transformation resulting transformation matrix
|
|
* \return
|
|
* * < 0 MSE bigger than max_mse_
|
|
* * = 0 MSE smaller than max_mse_
|
|
*/
|
|
virtual int
|
|
validateMatch(const pcl::Indices& base_indices,
|
|
const pcl::Indices& match_indices,
|
|
const pcl::Correspondences& correspondences,
|
|
Eigen::Matrix4f& transformation);
|
|
|
|
/** \brief Validate the transformation by calculating the number of inliers after
|
|
* transforming the source cloud. The resulting fitness score is later used as the
|
|
* decision criteria of the best fitting match.
|
|
*
|
|
* \param[out] transformation updated orientation matrix using all inliers
|
|
* \param[out] fitness_score current best fitness_score
|
|
* \note fitness score is only updated if the score of the current transformation
|
|
* exceeds the input one. \return
|
|
* * < 0 if previous result is better than the current one (score remains)
|
|
* * = 0 current result is better than the previous one (score updated)
|
|
*/
|
|
virtual int
|
|
validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score);
|
|
|
|
/** \brief Final computation of best match out of vector of best matches. To avoid
|
|
* cross thread dependencies during parallel running, a best match for each try was
|
|
* calculated. \note For forwards compatibility the candidates are stored in vectors
|
|
* of 'vectors of size 1'. \param[in] candidates vector of candidate matches
|
|
*/
|
|
virtual void
|
|
finalCompute(const std::vector<MatchingCandidates>& candidates);
|
|
|
|
/** \brief Normals of source point cloud. */
|
|
NormalsConstPtr source_normals_;
|
|
|
|
/** \brief Normals of target point cloud. */
|
|
NormalsConstPtr target_normals_;
|
|
|
|
/** \brief Number of threads for parallelization (standard = 1).
|
|
* \note Only used if run compiled with OpenMP.
|
|
*/
|
|
int nr_threads_;
|
|
|
|
/** \brief Estimated overlap between source and target (standard = 0.5). */
|
|
float approx_overlap_;
|
|
|
|
/** \brief Delta value of 4pcs algorithm (standard = 1.0).
|
|
* It can be used as:
|
|
* * absolute value (normalization = false), value should represent the point accuracy
|
|
* to ensure finding neighbors between source <-> target
|
|
* * relative value (normalization = true), to adjust the internally calculated point
|
|
* accuracy (= point density)
|
|
*/
|
|
float delta_;
|
|
|
|
/** \brief Score threshold to stop calculation with success.
|
|
* If not set by the user it depends on the size of the approximated overlap
|
|
*/
|
|
float score_threshold_;
|
|
|
|
/** \brief The number of points to uniformly sample the source point cloud. (standard
|
|
* = 0 => full cloud). */
|
|
int nr_samples_;
|
|
|
|
/** \brief Maximum normal difference of corresponding point pairs in degrees (standard
|
|
* = 90). */
|
|
float max_norm_diff_;
|
|
|
|
/** \brief Maximum allowed computation time in seconds (standard = 0 => ~unlimited).
|
|
*/
|
|
int max_runtime_;
|
|
|
|
/** \brief Resulting fitness score of the best match. */
|
|
float fitness_score_;
|
|
|
|
/** \brief Estimated diamter of the target point cloud. */
|
|
float diameter_;
|
|
|
|
/** \brief Estimated squared metric overlap between source and target.
|
|
* \note Internally calculated using the estimated overlap and the extent of the
|
|
* source cloud. It is used to derive the minimum sampling distance of the base points
|
|
* as well as to calculated the number of tries to reliably find a correct match.
|
|
*/
|
|
float max_base_diameter_sqr_;
|
|
|
|
/** \brief Use normals flag. */
|
|
bool use_normals_;
|
|
|
|
/** \brief Normalize delta flag. */
|
|
bool normalize_delta_;
|
|
|
|
/** \brief A pointer to the vector of source point indices to use after sampling. */
|
|
pcl::IndicesPtr source_indices_;
|
|
|
|
/** \brief A pointer to the vector of target point indices to use after sampling. */
|
|
pcl::IndicesPtr target_indices_;
|
|
|
|
/** \brief Maximal difference between corresponding point pairs in source and target.
|
|
* \note Internally calculated using an estimation of the point density.
|
|
*/
|
|
float max_pair_diff_;
|
|
|
|
/** \brief Maximal difference between the length of the base edges and valid match
|
|
* edges. \note Internally calculated using an estimation of the point density.
|
|
*/
|
|
float max_edge_diff_;
|
|
|
|
/** \brief Maximal distance between coinciding intersection points to find valid
|
|
* matches. \note Internally calculated using an estimation of the point density.
|
|
*/
|
|
float coincidation_limit_;
|
|
|
|
/** \brief Maximal mean squared errors of a transformation calculated from a candidate
|
|
* match. \note Internally calculated using an estimation of the point density.
|
|
*/
|
|
float max_mse_;
|
|
|
|
/** \brief Maximal squared point distance between source and target points to count as
|
|
* inlier. \note Internally calculated using an estimation of the point density.
|
|
*/
|
|
float max_inlier_dist_sqr_;
|
|
|
|
/** \brief Definition of a small error. */
|
|
const float small_error_;
|
|
};
|
|
}; // namespace registration
|
|
}; // namespace pcl
|
|
|
|
#include <pcl/registration/impl/ia_fpcs.hpp>
|