Point Cloud Library (PCL)  1.7.1
sample_consensus_prerejective.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_
42 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_H_
43 
44 #include <pcl/registration/registration.h>
45 #include <pcl/registration/transformation_estimation_svd.h>
46 #include <pcl/registration/transformation_validation.h>
47 #include <pcl/registration/correspondence_rejection_poly.h>
48 
49 namespace pcl
50 {
51  /** \brief Pose estimation and alignment class using a prerejective RANSAC routine.
52  *
53  * This class inserts a simple, yet effective "prerejection" step into the standard
54  * RANSAC pose estimation loop in order to avoid verification of pose hypotheses
55  * that are likely to be wrong. This is achieved by local pose-invariant geometric
56  * constraints, as also implemented in the class
57  * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
58  *
59  * In order to robustly align partial/occluded models, this routine performs
60  * fit error evaluation using only inliers, i.e. points closer than a
61  * Euclidean threshold, which is specifiable using \ref setInlierFraction().
62  *
63  * The amount of prerejection or "greedyness" of the algorithm can be specified
64  * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled,
65  * and 1 is maximally rejective.
66  *
67  * If you use this in academic work, please cite:
68  *
69  * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
70  * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
71  * International Conference on Robotics and Automation (ICRA), 2013.
72  *
73  * \author Anders Glent Buch (andersgb1@gmail.com)
74  * \ingroup registration
75  */
76  template <typename PointSource, typename PointTarget, typename FeatureT>
77  class SampleConsensusPrerejective : public Registration<PointSource, PointTarget>
78  {
79  public:
81 
94 
96  typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
97  typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
98 
100 
103 
107 
108  typedef boost::shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > Ptr;
109  typedef boost::shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> > ConstPtr;
110 
112 
116 
117  /** \brief Constructor */
119  : input_features_ ()
120  , target_features_ ()
121  , nr_samples_(3)
122  , k_correspondences_ (2)
123  , feature_tree_ (new pcl::KdTreeFLANN<FeatureT>)
125  , inlier_fraction_ (0.0f)
126  {
127  reg_name_ = "SampleConsensusPrerejective";
128  correspondence_rejector_poly_->setSimilarityThreshold (0.6f);
129  max_iterations_ = 5000;
131  };
132 
133  /** \brief Destructor */
135  {
136  }
137 
138  /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors
139  * \param features the source point cloud's features
140  */
141  void
142  setSourceFeatures (const FeatureCloudConstPtr &features);
143 
144  /** \brief Get a pointer to the source point cloud's features */
145  inline const FeatureCloudConstPtr
147  {
148  return (input_features_);
149  }
150 
151  /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors
152  * \param features the target point cloud's features
153  */
154  void
155  setTargetFeatures (const FeatureCloudConstPtr &features);
156 
157  /** \brief Get a pointer to the target point cloud's features */
158  inline const FeatureCloudConstPtr
160  {
161  return (target_features_);
162  }
163 
164  /** \brief Set the number of samples to use during each iteration
165  * \param nr_samples the number of samples to use during each iteration
166  */
167  inline void
168  setNumberOfSamples (int nr_samples)
169  {
170  nr_samples_ = nr_samples;
171  }
172 
173  /** \brief Get the number of samples to use during each iteration, as set by the user */
174  inline int
176  {
177  return (nr_samples_);
178  }
179 
180  /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will
181  * add more randomness to the feature matching.
182  * \param k the number of neighbors to use when selecting a random feature correspondence.
183  */
184  inline void
186  {
187  k_correspondences_ = k;
188  }
189 
190  /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
191  inline int
193  {
194  return (k_correspondences_);
195  }
196 
197  /** \brief Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object,
198  * where 1 is a perfect match
199  * \param similarity_threshold edge length similarity threshold
200  */
201  inline void
202  setSimilarityThreshold (float similarity_threshold)
203  {
204  correspondence_rejector_poly_->setSimilarityThreshold (similarity_threshold);
205  }
206 
207  /** \brief Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object,
208  * \return edge length similarity threshold
209  */
210  inline float
212  {
213  return correspondence_rejector_poly_->getSimilarityThreshold ();
214  }
215 
216  /** \brief Set the required inlier fraction (of the input)
217  * \param inlier_fraction required inlier fraction, must be in [0,1]
218  */
219  inline void
220  setInlierFraction (float inlier_fraction)
221  {
222  inlier_fraction_ = inlier_fraction;
223  }
224 
225  /** \brief Get the required inlier fraction
226  * \return required inlier fraction in [0,1]
227  */
228  inline float
230  {
231  return inlier_fraction_;
232  }
233 
234  /** \brief Get the inlier indices of the source point cloud under the final transformation
235  * @return inlier indices
236  */
237  inline const std::vector<int>&
238  getInliers () const
239  {
240  return inliers_;
241  }
242 
243  protected:
244  /** \brief Choose a random index between 0 and n-1
245  * \param n the number of possible indices to choose from
246  */
247  inline int
248  getRandomIndex (int n) const
249  {
250  return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0))));
251  };
252 
253  /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are
254  * greater than a user-defined minimum distance, \a min_sample_distance.
255  * \param cloud the input point cloud
256  * \param nr_samples the number of samples to select
257  * \param sample_indices the resulting sample indices
258  */
259  void
260  selectSamples (const PointCloudSource &cloud, int nr_samples,
261  std::vector<int> &sample_indices);
262 
263  /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to
264  * the sample points' features. From these, select one randomly which will be considered that sample point's
265  * correspondence.
266  * \param input_features a cloud of feature descriptors
267  * \param sample_indices the indices of each sample point
268  * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
269  */
270  void
271  findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices,
272  std::vector<int> &corresponding_indices);
273 
274  /** \brief Rigid transformation computation method.
275  * \param output the transformed input point cloud dataset using the rigid transformation found
276  */
277  void
278  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess);
279 
280  /** \brief Obtain the fitness of a transformation
281  * The following metrics are calculated, based on
282  * \b final_transformation_ and \b corr_dist_threshold_:
283  * - Inliers: the number of transformed points which are closer than threshold to NN
284  * - Error score: the MSE of the inliers
285  * \param inliers indices of source point cloud inliers
286  * \param fitness_score output fitness score as RMSE
287  */
288  void
289  getFitness (std::vector<int>& inliers, float& fitness_score);
290 
291  /** \brief The source point cloud's feature descriptors. */
293 
294  /** \brief The target point cloud's feature descriptors. */
296 
297  /** \brief The number of samples to use during each iteration. */
299 
300  /** \brief The number of neighbors to use when selecting a random feature correspondence. */
302 
303  /** \brief The KdTree used to compare feature descriptors. */
305 
306  /** \brief The polygonal correspondence rejector used for prerejection */
308 
309  /** \brief The fraction [0,1] of inlier points required for accepting a transformation */
311 
312  /** \brief Inlier points of final transformation as indices into source */
313  std::vector<int> inliers_;
314  };
315 }
316 
317 #include <pcl/registration/impl/sample_consensus_prerejective.hpp>
318 
319 #endif
CorrespondenceRejectorPolyPtr correspondence_rejector_poly_
The polygonal correspondence rejector used for prerejection.
boost::shared_ptr< SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > Ptr
float getSimilarityThreshold() const
Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector...
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence...
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:62
const FeatureCloudConstPtr getTargetFeatures() const
Get a pointer to the target point cloud's features.
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:543
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< const CorrespondenceRejectorPoly > ConstPtr
Registration< PointSource, PointTarget >::Matrix4 Matrix4
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud's feature descriptors.
void setCorrespondenceRandomness(int k)
Set the number of neighbors to use when selecting a random feature correspondence.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
float inlier_fraction_
The fraction [0,1] of inlier points required for accepting a transformation.
KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
float getInlierFraction() const
Get the required inlier fraction.
int nr_samples_
The number of samples to use during each iteration.
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
std::vector< int > inliers_
Inlier points of final transformation as indices into source.
void setInlierFraction(float inlier_fraction)
Set the required inlier fraction (of the input)
CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr
int getRandomIndex(int n) const
Choose a random index between 0 and n-1.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:67
boost::shared_ptr< const SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > ConstPtr
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
boost::shared_ptr< CorrespondenceRejectorPoly > Ptr
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)
Rigid transformation computation method.
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void selectSamples(const PointCloudSource &cloud, int nr_samples, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
int getCorrespondenceRandomness() const
Get the number of neighbors used when selecting a random feature correspondence, as set by the user...
const FeatureCloudConstPtr getSourceFeatures() const
Get a pointer to the source point cloud's features.
PointCloudSource::ConstPtr PointCloudSourceConstPtr
void getFitness(std::vector< int > &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
std::string reg_name_
The registration method name.
Definition: registration.h:478
Pose estimation and alignment class using a prerejective RANSAC routine.
Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
int getNumberOfSamples() const
Get the number of samples to use during each iteration, as set by the user.
void findSimilarFeatures(const FeatureCloud &input_features, const std::vector< int > &sample_indices, std::vector< int > &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr
FeatureCloudConstPtr input_features_
The source point cloud's feature descriptors.
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
Definition: PointIndices.h:23
FeatureCloudConstPtr target_features_
The target point cloud's feature descriptors.
pcl::registration::CorrespondenceRejectorPoly< PointSource, PointTarget > CorrespondenceRejectorPoly
const std::vector< int > & getInliers() const
Get the inlier indices of the source point cloud under the final transformation.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:492
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud's feature descriptors.