Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
48namespace 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 */
74template <typename PointSource, typename PointTarget, typename FeatureT>
75class SampleConsensusPrerejective : public Registration<PointSource, PointTarget> {
76public:
78
79 using Registration<PointSource, PointTarget>::reg_name_;
80 using Registration<PointSource, PointTarget>::getClassName;
81 using Registration<PointSource, PointTarget>::input_;
82 using Registration<PointSource, PointTarget>::target_;
83 using Registration<PointSource, PointTarget>::tree_;
84 using Registration<PointSource, PointTarget>::max_iterations_;
85 using Registration<PointSource, PointTarget>::corr_dist_threshold_;
86 using Registration<PointSource, PointTarget>::transformation_;
87 using Registration<PointSource, PointTarget>::final_transformation_;
88 using Registration<PointSource, PointTarget>::transformation_estimation_;
89 using Registration<PointSource, PointTarget>::getFitnessScore;
90 using Registration<PointSource, PointTarget>::converged_;
91
94 using PointCloudSourcePtr = typename PointCloudSource::Ptr;
95 using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
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 */
133
134 /** \brief Destructor */
135 ~SampleConsensusPrerejective() override = default;
136
137 /** \brief Provide a boost shared pointer to the source point cloud's feature
138 * descriptors \param features the source point cloud's features
139 */
140 void
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
151 * descriptors \param features the target point cloud's features
152 */
153 void
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 */
174 inline int
176 {
177 return (nr_samples_);
178 }
179
180 /** \brief Set the number of neighbors to use when selecting a random feature
181 * correspondence. A higher value will add more randomness to the feature matching.
182 * \param k the number of neighbors to use when selecting a random feature
183 * correspondence.
184 */
185 inline void
190
191 /** \brief Get the number of neighbors used when selecting a random feature
192 * correspondence, as set by the user */
193 inline int
195 {
196 return (k_correspondences_);
197 }
198
199 /** \brief Set the similarity threshold in [0,1[ between edge lengths of the
200 * underlying polygonal correspondence rejector object, where 1 is a perfect match
201 * \param similarity_threshold edge length similarity threshold
202 */
203 inline void
204 setSimilarityThreshold(float similarity_threshold)
205 {
206 correspondence_rejector_poly_->setSimilarityThreshold(similarity_threshold);
207 }
208
209 /** \brief Get the similarity threshold between edge lengths of the underlying
210 * polygonal correspondence rejector object, \return edge length similarity threshold
211 */
212 inline float
214 {
215 return correspondence_rejector_poly_->getSimilarityThreshold();
216 }
217
218 /** \brief Set the required inlier fraction (of the input)
219 * \param inlier_fraction required inlier fraction, must be in [0,1]
220 */
221 inline void
222 setInlierFraction(float inlier_fraction)
223 {
224 inlier_fraction_ = inlier_fraction;
225 }
226
227 /** \brief Get the required inlier fraction
228 * \return required inlier fraction in [0,1]
229 */
230 inline float
232 {
233 return inlier_fraction_;
234 }
235
236 /** \brief Get the inlier indices of the source point cloud under the final
237 * transformation
238 * @return inlier indices
239 */
240 inline const pcl::Indices&
242 {
243 return inliers_;
244 }
245
246protected:
247 /** \brief Choose a random index between 0 and n-1
248 * \param n the number of possible indices to choose from
249 */
250 inline int
251 getRandomIndex(int n) const
252 {
253 return (static_cast<int>(n * (rand() / (RAND_MAX + 1.0))));
254 };
255
256 /** \brief Select \a nr_samples sample points from cloud while making sure that their
257 * pairwise distances are greater than a user-defined minimum distance, \a
258 * min_sample_distance. \param cloud the input point cloud \param nr_samples the
259 * number of samples to select \param sample_indices the resulting sample indices
260 */
261 void
262 selectSamples(const PointCloudSource& cloud,
263 int nr_samples,
264 pcl::Indices& sample_indices);
265
266 /** \brief For each of the sample points, find a list of points in the target cloud
267 * whose features are similar to the sample points' features. From these, select one
268 * randomly which will be considered that sample point's correspondence. \param
269 * sample_indices the indices of each sample point \param similar_features
270 * correspondence cache, which is used to read/write already computed correspondences
271 * \param corresponding_indices the resulting indices of each sample's corresponding
272 * point in the target cloud
273 */
274 void
275 findSimilarFeatures(const pcl::Indices& sample_indices,
276 std::vector<pcl::Indices>& similar_features,
277 pcl::Indices& corresponding_indices);
278
279 /** \brief Rigid transformation computation method.
280 * \param output the transformed input point cloud dataset using the rigid
281 * transformation found \param guess The computed transformation
282 */
283 void
285 const Eigen::Matrix4f& guess) override;
286
287 /** \brief Obtain the fitness of a transformation
288 * The following metrics are calculated, based on
289 * \b final_transformation_ and \b corr_dist_threshold_:
290 * - Inliers: the number of transformed points which are closer than threshold to NN
291 * - Error score: the MSE of the inliers
292 * \param inliers indices of source point cloud inliers
293 * \param fitness_score output fitness score as RMSE
294 */
295 void
296 getFitness(pcl::Indices& inliers, float& fitness_score);
297
298 /** \brief The source point cloud's feature descriptors. */
300
301 /** \brief The target point cloud's feature descriptors. */
303
304 /** \brief The number of samples to use during each iteration. */
306
307 /** \brief The number of neighbors to use when selecting a random feature
308 * correspondence. */
310
311 /** \brief The KdTree used to compare feature descriptors. */
313
314 /** \brief The polygonal correspondence rejector used for prerejection */
316
317 /** \brief The fraction [0,1] of inlier points required for accepting a transformation
318 */
319 float inlier_fraction_{0.0f};
320
321 /** \brief Inlier points of final transformation as indices into source */
323};
324} // namespace pcl
325
326#include <pcl/registration/impl/sample_consensus_prerejective.hpp>
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
shared_ptr< PointCloud< FeatureT > > Ptr
shared_ptr< const PointCloud< FeatureT > > ConstPtr
Registration represents the base registration class for general purpose, ICP-like methods.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
std::string reg_name_
The registration method name.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
KdTreePtr tree_
A pointer to the spatial search object.
bool converged_
Holds internal convergence state, given user parameters.
Eigen::Matrix< Scalar, 4, 4 > Matrix4
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
double getFitnessScore(double max_range=std::numeric_limits< double >::max())
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
PointCloudTargetConstPtr target_
The input point cloud dataset target.
const std::string & getClassName() const
Abstract class get name method.
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...
shared_ptr< SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > Ptr
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
float inlier_fraction_
The fraction [0,1] of inlier points required for accepting a transformation.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
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
typename PointCloudSource::Ptr PointCloudSourcePtr
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.
const pcl::Indices & getInliers() const
Get the inlier indices of the source point cloud under the final transformation.
int nr_samples_
The number of samples to use during each iteration.
typename Registration< PointSource, PointTarget >::Matrix4 Matrix4
typename CorrespondenceRejectorPoly::Ptr CorrespondenceRejectorPolyPtr
shared_ptr< const SampleConsensusPrerejective< PointSource, PointTarget, FeatureT > > ConstPtr
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
shared_ptr< const ::pcl::PointIndices > ConstPtr