Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
ia_ransac.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/memory.h>
46
47namespace pcl {
48/** \brief @b SampleConsensusInitialAlignment is an implementation of the initial
49 * alignment algorithm described in section IV of "Fast Point Feature Histograms (FPFH)
50 * for 3D Registration," Rusu et al. \author Michael Dixon, Radu B. Rusu
51 * \ingroup registration
52 */
53template <typename PointSource, typename PointTarget, typename FeatureT>
54class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget> {
55public:
56 using Registration<PointSource, PointTarget>::reg_name_;
57 using Registration<PointSource, PointTarget>::input_;
58 using Registration<PointSource, PointTarget>::indices_;
59 using Registration<PointSource, PointTarget>::target_;
60 using Registration<PointSource, PointTarget>::final_transformation_;
61 using Registration<PointSource, PointTarget>::transformation_;
62 using Registration<PointSource, PointTarget>::corr_dist_threshold_;
63 using Registration<PointSource, PointTarget>::min_number_correspondences_;
64 using Registration<PointSource, PointTarget>::max_iterations_;
65 using Registration<PointSource, PointTarget>::tree_;
66 using Registration<PointSource, PointTarget>::transformation_estimation_;
67 using Registration<PointSource, PointTarget>::converged_;
68 using Registration<PointSource, PointTarget>::getClassName;
69
72 using PointCloudSourcePtr = typename PointCloudSource::Ptr;
73 using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
74
77
80
84
85 using Ptr =
86 shared_ptr<SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>>;
87 using ConstPtr = shared_ptr<
89
91 public:
92 using Ptr = shared_ptr<ErrorFunctor>;
93 using ConstPtr = shared_ptr<const ErrorFunctor>;
94
95 virtual ~ErrorFunctor() = default;
96 virtual float
97 operator()(float d) const = 0;
98 };
99
100 class HuberPenalty : public ErrorFunctor {
101 private:
102 HuberPenalty() {}
103
104 public:
105 HuberPenalty(float threshold) : threshold_(threshold) {}
106 virtual float
107 operator()(float e) const
108 {
109 if (e <= threshold_)
110 return (0.5 * e * e);
111 return (0.5 * threshold_ * (2.0 * std::fabs(e) - threshold_));
112 }
113
114 protected:
116 };
117
119 private:
120 TruncatedError() {}
121
122 public:
124
125 TruncatedError(float threshold) : threshold_(threshold) {}
126 float
127 operator()(float e) const override
128 {
129 if (e <= threshold_)
130 return (e / threshold_);
131 return (1.0);
132 }
133
134 protected:
136 };
137
139
141 /** \brief Constructor. */
145 , nr_samples_(3)
148 , feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
150 {
151 reg_name_ = "SampleConsensusInitialAlignment";
152 max_iterations_ = 1000;
153
154 // Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_
155 // to make it play nicely with TruncatedError
156 corr_dist_threshold_ = 100.0f;
159 };
160
161 /** \brief Provide a shared pointer to the source point cloud's feature descriptors
162 * \param features the source point cloud's features
163 */
164 void
166
167 /** \brief Get a pointer to the source point cloud's features */
168 inline FeatureCloudConstPtr const
170 {
171 return (input_features_);
172 }
173
174 /** \brief Provide a shared pointer to the target point cloud's feature descriptors
175 * \param features the target point cloud's features
176 */
177 void
179
180 /** \brief Get a pointer to the target point cloud's features */
181 inline FeatureCloudConstPtr const
183 {
184 return (target_features_);
185 }
186
187 /** \brief Set the minimum distances between samples
188 * \param min_sample_distance the minimum distances between samples
189 */
190 void
191 setMinSampleDistance(float min_sample_distance)
192 {
193 min_sample_distance_ = min_sample_distance;
194 }
195
196 /** \brief Get the minimum distances between samples, as set by the user */
197 float
199 {
200 return (min_sample_distance_);
201 }
202
203 /** \brief Set the number of samples to use during each iteration
204 * \param nr_samples the number of samples to use during each iteration
205 */
206 void
207 setNumberOfSamples(int nr_samples)
208 {
209 nr_samples_ = nr_samples;
210 }
211
212 /** \brief Get the number of samples to use during each iteration, as set by the user
213 */
214 int
216 {
217 return (nr_samples_);
218 }
219
220 /** \brief Set the number of neighbors to use when selecting a random feature
221 * correspondence. A higher value will add more randomness to the feature matching.
222 * \param k the number of neighbors to use when selecting a random feature
223 * correspondence.
224 */
225 void
230
231 /** \brief Get the number of neighbors used when selecting a random feature
232 * correspondence, as set by the user */
233 int
238
239 /** \brief Specify the error function to minimize
240 * \note This call is optional. TruncatedError will be used by default
241 * \param[in] error_functor a shared pointer to a subclass of
242 * SampleConsensusInitialAlignment::ErrorFunctor
243 */
244 void
245 setErrorFunction(const ErrorFunctorPtr& error_functor)
246 {
247 error_functor_ = error_functor;
248 }
249
250 /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized
251 * \return A shared pointer to a subclass of
252 * SampleConsensusInitialAlignment::ErrorFunctor
253 */
256 {
257 return (error_functor_);
258 }
259
260protected:
261 /** \brief Choose a random index between 0 and n-1
262 * \param n the number of possible indices to choose from
263 */
264 inline pcl::index_t
266 {
267 return (static_cast<pcl::index_t>(n * (rand() / (RAND_MAX + 1.0))));
268 };
269
270 /** \brief Select \a nr_samples sample points from cloud while making sure that their
271 * pairwise distances are greater than a user-defined minimum distance, \a
272 * min_sample_distance. \param cloud the input point cloud \param nr_samples the
273 * number of samples to select \param min_sample_distance the minimum distance between
274 * any two samples \param sample_indices the resulting sample indices
275 */
276 void
277 selectSamples(const PointCloudSource& cloud,
278 unsigned int nr_samples,
279 float min_sample_distance,
280 pcl::Indices& sample_indices);
281
282 /** \brief For each of the sample points, find a list of points in the target cloud
283 * whose features are similar to the sample points' features. From these, select one
284 * randomly which will be considered that sample point's correspondence. \param
285 * input_features a cloud of feature descriptors \param sample_indices the indices of
286 * each sample point \param corresponding_indices the resulting indices of each
287 * sample's corresponding point in the target cloud
288 */
289 void
290 findSimilarFeatures(const FeatureCloud& input_features,
291 const pcl::Indices& sample_indices,
292 pcl::Indices& corresponding_indices);
293
294 /** \brief An error metric for that computes the quality of the alignment between the
295 * given cloud and the target. \param cloud the input cloud \param threshold distances
296 * greater than this value are capped
297 */
298 float
299 computeErrorMetric(const PointCloudSource& cloud, float threshold);
300
301 /** \brief Rigid transformation computation method.
302 * \param output the transformed input point cloud dataset using the rigid
303 * transformation found \param guess The computed transforamtion
304 */
305 void
307 const Eigen::Matrix4f& guess) override;
308
309 /** \brief The source point cloud's feature descriptors. */
311
312 /** \brief The target point cloud's feature descriptors. */
314
315 /** \brief The number of samples to use during each iteration. */
317
318 /** \brief The minimum distances between samples. */
320
321 /** \brief The number of neighbors to use when selecting a random feature
322 * correspondence. */
324
325 /** \brief The KdTree used to compare feature descriptors. */
327
329
330public:
332};
333} // namespace pcl
334
335#include <pcl/registration/impl/ia_ransac.hpp>
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
PointCloudConstPtr input_
Definition pcl_base.h:147
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< PointCloud< PointT > > Ptr
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.
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.
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
const std::string & getClassName() const
Abstract class get name method.
virtual float operator()(float d) const =0
shared_ptr< const ErrorFunctor > ConstPtr
Definition ia_ransac.h:93
SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in ...
Definition ia_ransac.h:54
FeatureCloudConstPtr const getTargetFeatures()
Get a pointer to the target point cloud's features.
Definition ia_ransac.h:182
PointIndices::ConstPtr PointIndicesConstPtr
Definition ia_ransac.h:79
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
shared_ptr< SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > Ptr
Definition ia_ransac.h:85
shared_ptr< const SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > ConstPtr
Definition ia_ransac.h:87
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition ia_ransac.h:70
int getNumberOfSamples()
Get the number of samples to use during each iteration, as set by the user.
Definition ia_ransac.h:215
float computeErrorMetric(const PointCloudSource &cloud, float threshold)
An error metric for that computes the quality of the alignment between the given cloud and the target...
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition ia_ransac.h:73
void setMinSampleDistance(float min_sample_distance)
Set the minimum distances between samples.
Definition ia_ransac.h:191
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
Definition ia_ransac.h:326
pcl::index_t getRandomIndex(int n)
Choose a random index between 0 and n-1.
Definition ia_ransac.h:265
pcl::PointCloud< FeatureT > FeatureCloud
Definition ia_ransac.h:81
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud's feature descriptors.
Definition ia_ransac.hpp:64
typename KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
Definition ia_ransac.h:140
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
Definition ia_ransac.h:207
FeatureCloudConstPtr target_features_
The target point cloud's feature descriptors.
Definition ia_ransac.h:313
float getMinSampleDistance()
Get the minimum distances between samples, as set by the user.
Definition ia_ransac.h:198
float min_sample_distance_
The minimum distances between samples.
Definition ia_ransac.h:319
typename ErrorFunctor::Ptr ErrorFunctorPtr
Definition ia_ransac.h:138
typename FeatureCloud::Ptr FeatureCloudPtr
Definition ia_ransac.h:82
void setErrorFunction(const ErrorFunctorPtr &error_functor)
Specify the error function to minimize.
Definition ia_ransac.h:245
FeatureCloudConstPtr input_features_
The source point cloud's feature descriptors.
Definition ia_ransac.h:310
int nr_samples_
The number of samples to use during each iteration.
Definition ia_ransac.h:316
void selectSamples(const PointCloudSource &cloud, unsigned int nr_samples, float min_sample_distance, pcl::Indices &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
Definition ia_ransac.hpp:79
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ia_ransac.h:72
int getCorrespondenceRandomness()
Get the number of neighbors used when selecting a random feature correspondence, as set by the user.
Definition ia_ransac.h:234
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
Definition ia_ransac.h:75
void findSimilarFeatures(const FeatureCloud &input_features, const pcl::Indices &sample_indices, 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 setCorrespondenceRandomness(int k)
Set the number of neighbors to use when selecting a random feature correspondence.
Definition ia_ransac.h:226
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition ia_ransac.h:83
SampleConsensusInitialAlignment()
Constructor.
Definition ia_ransac.h:142
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
Definition ia_ransac.h:323
ErrorFunctorPtr getErrorFunction()
Get a shared pointer to the ErrorFunctor that is to be minimized.
Definition ia_ransac.h:255
FeatureCloudConstPtr const getSourceFeatures()
Get a pointer to the source point cloud's features.
Definition ia_ransac.h:169
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud's feature descriptors.
Definition ia_ransac.hpp:50
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
Defines functions, macros and traits for allocating and using memory.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr