Point Cloud Library (PCL)  1.12.1-dev
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 
47 namespace 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  */
53 template <typename PointSource, typename PointTarget, typename FeatureT>
54 class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget> {
55 public:
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 
90  class ErrorFunctor {
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() = default;
103 
104  public:
105  HuberPenalty(float threshold) : threshold_(threshold) {}
106  float
107  operator()(float e) const override
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:
115  float threshold_;
116  };
117 
118  class TruncatedError : public ErrorFunctor {
119  private:
120  TruncatedError() = default;
121 
122  public:
123  ~TruncatedError() = default;
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:
135  float threshold_;
136  };
137 
139 
141  /** \brief Constructor. */
143  : input_features_()
144  , target_features_()
145  , nr_samples_(3)
146  , min_sample_distance_(0.0f)
147  , k_correspondences_(10)
148  , feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
149  , error_functor_()
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
165  setSourceFeatures(const FeatureCloudConstPtr& features);
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
178  setTargetFeatures(const FeatureCloudConstPtr& features);
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
227  {
228  k_correspondences_ = k;
229  }
230 
231  /** \brief Get the number of neighbors used when selecting a random feature
232  * correspondence, as set by the user */
233  int
235  {
236  return (k_correspondences_);
237  }
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 
260 protected:
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 
330 public:
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.
Definition: kdtree_flann.h:132
shared_ptr< PointCloud< FeatureT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< FeatureT > > ConstPtr
Definition: point_cloud.h:414
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:615
std::string reg_name_
The registration method name.
Definition: registration.h:560
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
virtual float operator()(float d) const =0
shared_ptr< const ErrorFunctor > ConstPtr
Definition: ia_ransac.h:93
float operator()(float e) const override
Definition: ia_ransac.h:107
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.
Definition: ia_ransac.hpp:189
shared_ptr< const SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > ConstPtr
Definition: ia_ransac.h:88
int getNumberOfSamples()
Get the number of samples to use during each iteration, as set by the user.
Definition: ia_ransac.h:215
PointIndices::Ptr PointIndicesPtr
Definition: ia_ransac.h:78
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...
Definition: ia_ransac.hpp:166
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
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: ia_ransac.h:71
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
shared_ptr< SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > Ptr
Definition: ia_ransac.h:86
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
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...
Definition: ia_ransac.hpp:142
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
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
Definition: ia_ransac.h:76
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
Definition: PointIndices.h:13
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14