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