Point Cloud Library (PCL)  1.11.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 #pragma once
42 
43 #include <pcl/registration/registration.h>
44 #include <pcl/registration/transformation_estimation_svd.h>
45 #include <pcl/registration/transformation_validation.h>
46 #include <pcl/registration/correspondence_rejection_poly.h>
47 
48 namespace pcl
49 {
50  /** \brief Pose estimation and alignment class using a prerejective RANSAC routine.
51  *
52  * This class inserts a simple, yet effective "prerejection" step into the standard
53  * RANSAC pose estimation loop in order to avoid verification of pose hypotheses
54  * that are likely to be wrong. This is achieved by local pose-invariant geometric
55  * constraints, as also implemented in the class
56  * \ref registration::CorrespondenceRejectorPoly "CorrespondenceRejectorPoly".
57  *
58  * In order to robustly align partial/occluded models, this routine performs
59  * fit error evaluation using only inliers, i.e. points closer than a
60  * Euclidean threshold, which is specifiable using \ref setInlierFraction().
61  *
62  * The amount of prerejection or "greedyness" of the algorithm can be specified
63  * using \ref setSimilarityThreshold() in [0,1[, where a value of 0 means disabled,
64  * and 1 is maximally rejective.
65  *
66  * If you use this in academic work, please cite:
67  *
68  * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
69  * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
70  * International Conference on Robotics and Automation (ICRA), 2013.
71  *
72  * \author Anders Glent Buch (andersgb1@gmail.com)
73  * \ingroup registration
74  */
75  template <typename PointSource, typename PointTarget, typename FeatureT>
76  class SampleConsensusPrerejective : public Registration<PointSource, PointTarget>
77  {
78  public:
80 
93 
95  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
96  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
97 
99 
102 
106 
107  using Ptr = shared_ptr<SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> >;
108  using ConstPtr = shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT> >;
109 
111 
115 
116  /** \brief Constructor */
118  : input_features_ ()
119  , target_features_ ()
120  , nr_samples_(3)
121  , k_correspondences_ (2)
122  , feature_tree_ (new pcl::KdTreeFLANN<FeatureT>)
124  , inlier_fraction_ (0.0f)
125  {
126  reg_name_ = "SampleConsensusPrerejective";
127  correspondence_rejector_poly_->setSimilarityThreshold (0.6f);
128  max_iterations_ = 5000;
130  };
131 
132  /** \brief Destructor */
134  {
135  }
136 
137  /** \brief Provide a boost shared pointer to the source point cloud's feature descriptors
138  * \param features the source point cloud's features
139  */
140  void
141  setSourceFeatures (const FeatureCloudConstPtr &features);
142 
143  /** \brief Get a pointer to the source point cloud's features */
144  inline const FeatureCloudConstPtr
146  {
147  return (input_features_);
148  }
149 
150  /** \brief Provide a boost shared pointer to the target point cloud's feature descriptors
151  * \param features the target point cloud's features
152  */
153  void
154  setTargetFeatures (const FeatureCloudConstPtr &features);
155 
156  /** \brief Get a pointer to the target point cloud's features */
157  inline const FeatureCloudConstPtr
159  {
160  return (target_features_);
161  }
162 
163  /** \brief Set the number of samples to use during each iteration
164  * \param nr_samples the number of samples to use during each iteration
165  */
166  inline void
167  setNumberOfSamples (int nr_samples)
168  {
169  nr_samples_ = nr_samples;
170  }
171 
172  /** \brief Get the number of samples to use during each iteration, as set by the user */
173  inline int
175  {
176  return (nr_samples_);
177  }
178 
179  /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will
180  * add more randomness to the feature matching.
181  * \param k the number of neighbors to use when selecting a random feature correspondence.
182  */
183  inline void
185  {
186  k_correspondences_ = k;
187  }
188 
189  /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
190  inline int
192  {
193  return (k_correspondences_);
194  }
195 
196  /** \brief Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence rejector object,
197  * where 1 is a perfect match
198  * \param similarity_threshold edge length similarity threshold
199  */
200  inline void
201  setSimilarityThreshold (float similarity_threshold)
202  {
203  correspondence_rejector_poly_->setSimilarityThreshold (similarity_threshold);
204  }
205 
206  /** \brief Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector object,
207  * \return edge length similarity threshold
208  */
209  inline float
211  {
212  return correspondence_rejector_poly_->getSimilarityThreshold ();
213  }
214 
215  /** \brief Set the required inlier fraction (of the input)
216  * \param inlier_fraction required inlier fraction, must be in [0,1]
217  */
218  inline void
219  setInlierFraction (float inlier_fraction)
220  {
221  inlier_fraction_ = inlier_fraction;
222  }
223 
224  /** \brief Get the required inlier fraction
225  * \return required inlier fraction in [0,1]
226  */
227  inline float
229  {
230  return inlier_fraction_;
231  }
232 
233  /** \brief Get the inlier indices of the source point cloud under the final transformation
234  * @return inlier indices
235  */
236  inline const std::vector<int>&
237  getInliers () const
238  {
239  return inliers_;
240  }
241 
242  protected:
243  /** \brief Choose a random index between 0 and n-1
244  * \param n the number of possible indices to choose from
245  */
246  inline int
247  getRandomIndex (int n) const
248  {
249  return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0))));
250  };
251 
252  /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are
253  * greater than a user-defined minimum distance, \a min_sample_distance.
254  * \param cloud the input point cloud
255  * \param nr_samples the number of samples to select
256  * \param sample_indices the resulting sample indices
257  */
258  void
259  selectSamples (const PointCloudSource &cloud, int nr_samples, std::vector<int> &sample_indices);
260 
261  /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to
262  * the sample points' features. From these, select one randomly which will be considered that sample point's
263  * correspondence.
264  * \param sample_indices the indices of each sample point
265  * \param similar_features correspondence cache, which is used to read/write already computed correspondences
266  * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
267  */
268  void
269  findSimilarFeatures (const std::vector<int> &sample_indices,
270  std::vector<std::vector<int> >& similar_features,
271  std::vector<int> &corresponding_indices);
272 
273  /** \brief Rigid transformation computation method.
274  * \param output the transformed input point cloud dataset using the rigid transformation found
275  * \param guess The computed transformation
276  */
277  void
278  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
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>
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:67
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
int nr_samples_
The number of samples to use during each iteration.
shared_ptr< PointCloud< FeatureT > > Ptr
Definition: point_cloud.h:429
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:560
FeatureCloudConstPtr input_features_
The source point cloud&#39;s feature descriptors.
void getFitness(std::vector< int > &inliers, float &fitness_score)
Obtain the fitness of a transformation The following metrics are calculated, based on final_transform...
shared_ptr< SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > Ptr
float getInlierFraction() const
Get the required inlier fraction.
int getCorrespondenceRandomness() const
Get the number of neighbors used when selecting a random feature correspondence, as set by the user...
typename Registration< PointSource, PointTarget >::Matrix4 Matrix4
const FeatureCloudConstPtr getSourceFeatures() const
Get a pointer to the source point cloud&#39;s features.
float getSimilarityThreshold() const
Get the similarity threshold between edge lengths of the underlying polygonal correspondence rejector...
int getRandomIndex(int n) const
Choose a random index between 0 and n-1.
float inlier_fraction_
The fraction [0,1] of inlier points required for accepting a transformation.
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
void setInlierFraction(float inlier_fraction)
Set the required inlier fraction (of the input)
typename CorrespondenceRejectorPoly::ConstPtr CorrespondenceRejectorPolyConstPtr
shared_ptr< CorrespondenceRejectorPoly< SourceT, TargetT > > Ptr
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:15
Pose estimation and alignment class using a prerejective RANSAC routine.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:504
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
typename PointCloudSource::Ptr PointCloudSourcePtr
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
typename CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr
void setCorrespondenceRandomness(int k)
Set the number of neighbors to use when selecting a random feature correspondence.
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the target point cloud&#39;s feature descriptors.
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:61
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
FeatureCloudConstPtr target_features_
The target point cloud&#39;s feature descriptors.
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
void findSimilarFeatures(const std::vector< int > &sample_indices, std::vector< std::vector< int > > &similar_features, 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...
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
shared_ptr< const SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > ConstPtr
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:16
const FeatureCloudConstPtr getTargetFeatures() const
Get a pointer to the target point cloud&#39;s features.
shared_ptr< const PointCloud< FeatureT > > ConstPtr
Definition: point_cloud.h:430
std::string reg_name_
The registration method name.
Definition: registration.h:490
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...
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a boost shared pointer to the source point cloud&#39;s feature descriptors.
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
std::vector< int > inliers_
Inlier points of final transformation as indices into source.
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths of the underlying polygonal correspondence...
typename KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
CorrespondenceRejectorPolyPtr correspondence_rejector_poly_
The polygonal correspondence rejector used for prerejection.
int getNumberOfSamples() const
Get the number of samples to use during each iteration, as set by the user.
const std::vector< int > & getInliers() const
Get the inlier indices of the source point cloud under the final transformation.
shared_ptr< const CorrespondenceRejectorPoly< SourceT, TargetT > > ConstPtr