Point Cloud Library (PCL)  1.9.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/pcl_macros.h>
44 #include <pcl/registration/registration.h>
45 #include <pcl/registration/transformation_estimation_svd.h>
46 
47 namespace pcl
48 {
49  /** \brief @b SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in
50  * section IV of "Fast Point Feature Histograms (FPFH) for 3D Registration," Rusu et al.
51  * \author Michael Dixon, Radu B. Rusu
52  * \ingroup registration
53  */
54  template <typename PointSource, typename PointTarget, typename FeatureT>
55  class SampleConsensusInitialAlignment : public Registration<PointSource, PointTarget>
56  {
57  public:
71 
73  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
74  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
75 
77 
80 
84 
85  using Ptr = boost::shared_ptr<SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT> >;
86  using ConstPtr = boost::shared_ptr<const SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT> >;
87 
88 
90  {
91  public:
92  virtual ~ErrorFunctor () {}
93  virtual float operator () (float d) const = 0;
94  };
95 
96  class HuberPenalty : public ErrorFunctor
97  {
98  private:
99  HuberPenalty () {}
100  public:
101  HuberPenalty (float threshold) : threshold_ (threshold) {}
102  virtual float operator () (float e) const
103  {
104  if (e <= threshold_)
105  return (0.5 * e*e);
106  return (0.5 * threshold_ * (2.0 * std::fabs (e) - threshold_));
107  }
108  protected:
109  float threshold_;
110  };
111 
113  {
114  private:
115  TruncatedError () {}
116  public:
118 
119  TruncatedError (float threshold) : threshold_ (threshold) {}
120  float operator () (float e) const override
121  {
122  if (e <= threshold_)
123  return (e / threshold_);
124  return (1.0);
125  }
126  protected:
127  float threshold_;
128  };
129 
130  using ErrorFunctorPtr = boost::shared_ptr<ErrorFunctor>;
131 
133  /** \brief Constructor. */
137  feature_tree_ (new pcl::KdTreeFLANN<FeatureT>),
138  error_functor_ ()
139  {
140  reg_name_ = "SampleConsensusInitialAlignment";
141  max_iterations_ = 1000;
142 
143  // Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_ to make it play nicely with TruncatedError
144  corr_dist_threshold_ = 100.0f;
146  };
147 
148  /** \brief Provide a shared pointer to the source point cloud's feature descriptors
149  * \param features the source point cloud's features
150  */
151  void
152  setSourceFeatures (const FeatureCloudConstPtr &features);
153 
154  /** \brief Get a pointer to the source point cloud's features */
155  inline FeatureCloudConstPtr const
157 
158  /** \brief Provide a shared pointer to the target point cloud's feature descriptors
159  * \param features the target point cloud's features
160  */
161  void
162  setTargetFeatures (const FeatureCloudConstPtr &features);
163 
164  /** \brief Get a pointer to the target point cloud's features */
165  inline FeatureCloudConstPtr const
167 
168  /** \brief Set the minimum distances between samples
169  * \param min_sample_distance the minimum distances between samples
170  */
171  void
172  setMinSampleDistance (float min_sample_distance) { min_sample_distance_ = min_sample_distance; }
173 
174  /** \brief Get the minimum distances between samples, as set by the user */
175  float
177 
178  /** \brief Set the number of samples to use during each iteration
179  * \param nr_samples the number of samples to use during each iteration
180  */
181  void
182  setNumberOfSamples (int nr_samples) { nr_samples_ = nr_samples; }
183 
184  /** \brief Get the number of samples to use during each iteration, as set by the user */
185  int
187 
188  /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will
189  * add more randomness to the feature matching.
190  * \param k the number of neighbors to use when selecting a random feature correspondence.
191  */
192  void
194 
195  /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
196  int
198 
199  /** \brief Specify the error function to minimize
200  * \note This call is optional. TruncatedError will be used by default
201  * \param[in] error_functor a shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor
202  */
203  void
204  setErrorFunction (const ErrorFunctorPtr & error_functor) { error_functor_ = error_functor; }
205 
206  /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized
207  * \return A shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor
208  */
211 
212  protected:
213  /** \brief Choose a random index between 0 and n-1
214  * \param n the number of possible indices to choose from
215  */
216  inline int
217  getRandomIndex (int n) { return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0)))); };
218 
219  /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are
220  * greater than a user-defined minimum distance, \a min_sample_distance.
221  * \param cloud the input point cloud
222  * \param nr_samples the number of samples to select
223  * \param min_sample_distance the minimum distance between any two samples
224  * \param sample_indices the resulting sample indices
225  */
226  void
227  selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
228  std::vector<int> &sample_indices);
229 
230  /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to
231  * the sample points' features. From these, select one randomly which will be considered that sample point's
232  * correspondence.
233  * \param input_features a cloud of feature descriptors
234  * \param sample_indices the indices of each sample point
235  * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
236  */
237  void
238  findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices,
239  std::vector<int> &corresponding_indices);
240 
241  /** \brief An error metric for that computes the quality of the alignment between the given cloud and the target.
242  * \param cloud the input cloud
243  * \param threshold distances greater than this value are capped
244  */
245  float
246  computeErrorMetric (const PointCloudSource &cloud, float threshold);
247 
248  /** \brief Rigid transformation computation method.
249  * \param output the transformed input point cloud dataset using the rigid transformation found
250  * \param guess The computed transforamtion
251  */
252  void
253  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
254 
255  /** \brief The source point cloud's feature descriptors. */
257 
258  /** \brief The target point cloud's feature descriptors. */
260 
261  /** \brief The number of samples to use during each iteration. */
263 
264  /** \brief The minimum distances between samples. */
266 
267  /** \brief The number of neighbors to use when selecting a random feature correspondence. */
269 
270  /** \brief The KdTree used to compare feature descriptors. */
272 
274  public:
276  };
277 }
278 
279 #include <pcl/registration/impl/ia_ransac.hpp>
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:67
void setCorrespondenceRandomness(int k)
Set the number of neighbors to use when selecting a random feature correspondence.
Definition: ia_ransac.h:193
int nr_samples_
The number of samples to use during each iteration.
Definition: ia_ransac.h:262
typename KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
Definition: ia_ransac.h:132
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:559
PointIndices::ConstPtr PointIndicesConstPtr
Definition: ia_ransac.h:79
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::shared_ptr< ErrorFunctor > ErrorFunctorPtr
Definition: ia_ransac.h:130
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
Definition: ia_ransac.h:271
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
Definition: ia_ransac.h:182
boost::shared_ptr< const SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > ConstPtr
Definition: ia_ransac.h:86
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:359
virtual float operator()(float d) const =0
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud&#39;s feature descriptors.
Definition: ia_ransac.hpp:60
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud&#39;s feature descriptors.
Definition: ia_ransac.hpp:48
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:153
SampleConsensusInitialAlignment()
Constructor.
Definition: ia_ransac.h:134
FeatureCloudConstPtr target_features_
The target point cloud&#39;s feature descriptors.
Definition: ia_ransac.h:259
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
Definition: ia_ransac.h:268
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:503
FeatureCloudConstPtr const getTargetFeatures()
Get a pointer to the target point cloud&#39;s features.
Definition: ia_ransac.h:166
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
ErrorFunctorPtr getErrorFunction()
Get a shared pointer to the ErrorFunctor that is to be minimized.
Definition: ia_ransac.h:210
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: ia_ransac.h:74
int getCorrespondenceRandomness()
Get the number of neighbors used when selecting a random feature correspondence, as set by the user...
Definition: ia_ransac.h:197
boost::shared_ptr< PointCloud< FeatureT > > Ptr
Definition: point_cloud.h:411
typename FeatureCloud::Ptr FeatureCloudPtr
Definition: ia_ransac.h:82
boost::shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:23
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:60
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: ia_ransac.h:73
boost::shared_ptr< SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > Ptr
Definition: ia_ransac.h:85
void selectSamples(const PointCloudSource &cloud, int nr_samples, float min_sample_distance, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
Definition: ia_ransac.hpp:73
SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in ...
Definition: ia_ransac.h:55
void setErrorFunction(const ErrorFunctorPtr &error_functor)
Specify the error function to minimize.
Definition: ia_ransac.h:204
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: ia_ransac.h:72
boost::shared_ptr< const PointCloud< FeatureT > > ConstPtr
Definition: point_cloud.h:412
FeatureCloudConstPtr const getSourceFeatures()
Get a pointer to the source point cloud&#39;s features.
Definition: ia_ransac.h:156
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
Definition: ia_ransac.h:76
FeatureCloudConstPtr input_features_
The source point cloud&#39;s feature descriptors.
Definition: ia_ransac.h:256
void findSimilarFeatures(const FeatureCloud &input_features, const std::vector< int > &sample_indices, 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...
Definition: ia_ransac.hpp:132
int getNumberOfSamples()
Get the number of samples to use during each iteration, as set by the user.
Definition: ia_ransac.h:186
std::string reg_name_
The registration method name.
Definition: registration.h:489
PointIndices::Ptr PointIndicesPtr
Definition: ia_ransac.h:78
float min_sample_distance_
The minimum distances between samples.
Definition: ia_ransac.h:265
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:539
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
Definition: ia_ransac.hpp:175
float getMinSampleDistance()
Get the minimum distances between samples, as set by the user.
Definition: ia_ransac.h:176
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition: ia_ransac.h:83
Defines all the PCL and non-PCL macros used.
void setMinSampleDistance(float min_sample_distance)
Set the minimum distances between samples.
Definition: ia_ransac.h:172
int getRandomIndex(int n)
Choose a random index between 0 and n-1.
Definition: ia_ransac.h:217