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  using Ptr = boost::shared_ptr<ErrorFunctor>;
93  using ConstPtr = boost::shared_ptr<const ErrorFunctor>;
94 
95  virtual ~ErrorFunctor () = default;
96  virtual float operator () (float d) const = 0;
97  };
98 
99  class HuberPenalty : public ErrorFunctor
100  {
101  private:
102  HuberPenalty () {}
103  public:
104  HuberPenalty (float threshold) : threshold_ (threshold) {}
105  virtual float operator () (float e) const
106  {
107  if (e <= threshold_)
108  return (0.5 * e*e);
109  return (0.5 * threshold_ * (2.0 * std::fabs (e) - threshold_));
110  }
111  protected:
112  float threshold_;
113  };
114 
116  {
117  private:
118  TruncatedError () {}
119  public:
121 
122  TruncatedError (float threshold) : threshold_ (threshold) {}
123  float operator () (float e) const override
124  {
125  if (e <= threshold_)
126  return (e / threshold_);
127  return (1.0);
128  }
129  protected:
130  float threshold_;
131  };
132 
134 
136  /** \brief Constructor. */
140  feature_tree_ (new pcl::KdTreeFLANN<FeatureT>),
141  error_functor_ ()
142  {
143  reg_name_ = "SampleConsensusInitialAlignment";
144  max_iterations_ = 1000;
145 
146  // Setting a non-std::numeric_limits<double>::max () value to corr_dist_threshold_ to make it play nicely with TruncatedError
147  corr_dist_threshold_ = 100.0f;
149  };
150 
151  /** \brief Provide a shared pointer to the source point cloud's feature descriptors
152  * \param features the source point cloud's features
153  */
154  void
155  setSourceFeatures (const FeatureCloudConstPtr &features);
156 
157  /** \brief Get a pointer to the source point cloud's features */
158  inline FeatureCloudConstPtr const
160 
161  /** \brief Provide a shared pointer to the target point cloud's feature descriptors
162  * \param features the target point cloud's features
163  */
164  void
165  setTargetFeatures (const FeatureCloudConstPtr &features);
166 
167  /** \brief Get a pointer to the target point cloud's features */
168  inline FeatureCloudConstPtr const
170 
171  /** \brief Set the minimum distances between samples
172  * \param min_sample_distance the minimum distances between samples
173  */
174  void
175  setMinSampleDistance (float min_sample_distance) { min_sample_distance_ = min_sample_distance; }
176 
177  /** \brief Get the minimum distances between samples, as set by the user */
178  float
180 
181  /** \brief Set the number of samples to use during each iteration
182  * \param nr_samples the number of samples to use during each iteration
183  */
184  void
185  setNumberOfSamples (int nr_samples) { nr_samples_ = nr_samples; }
186 
187  /** \brief Get the number of samples to use during each iteration, as set by the user */
188  int
190 
191  /** \brief Set the number of neighbors to use when selecting a random feature correspondence. A higher value will
192  * add more randomness to the feature matching.
193  * \param k the number of neighbors to use when selecting a random feature correspondence.
194  */
195  void
197 
198  /** \brief Get the number of neighbors used when selecting a random feature correspondence, as set by the user */
199  int
201 
202  /** \brief Specify the error function to minimize
203  * \note This call is optional. TruncatedError will be used by default
204  * \param[in] error_functor a shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor
205  */
206  void
207  setErrorFunction (const ErrorFunctorPtr & error_functor) { error_functor_ = error_functor; }
208 
209  /** \brief Get a shared pointer to the ErrorFunctor that is to be minimized
210  * \return A shared pointer to a subclass of SampleConsensusInitialAlignment::ErrorFunctor
211  */
214 
215  protected:
216  /** \brief Choose a random index between 0 and n-1
217  * \param n the number of possible indices to choose from
218  */
219  inline int
220  getRandomIndex (int n) { return (static_cast<int> (n * (rand () / (RAND_MAX + 1.0)))); };
221 
222  /** \brief Select \a nr_samples sample points from cloud while making sure that their pairwise distances are
223  * greater than a user-defined minimum distance, \a min_sample_distance.
224  * \param cloud the input point cloud
225  * \param nr_samples the number of samples to select
226  * \param min_sample_distance the minimum distance between any two samples
227  * \param sample_indices the resulting sample indices
228  */
229  void
230  selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
231  std::vector<int> &sample_indices);
232 
233  /** \brief For each of the sample points, find a list of points in the target cloud whose features are similar to
234  * the sample points' features. From these, select one randomly which will be considered that sample point's
235  * correspondence.
236  * \param input_features a cloud of feature descriptors
237  * \param sample_indices the indices of each sample point
238  * \param corresponding_indices the resulting indices of each sample's corresponding point in the target cloud
239  */
240  void
241  findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices,
242  std::vector<int> &corresponding_indices);
243 
244  /** \brief An error metric for that computes the quality of the alignment between the given cloud and the target.
245  * \param cloud the input cloud
246  * \param threshold distances greater than this value are capped
247  */
248  float
249  computeErrorMetric (const PointCloudSource &cloud, float threshold);
250 
251  /** \brief Rigid transformation computation method.
252  * \param output the transformed input point cloud dataset using the rigid transformation found
253  * \param guess The computed transforamtion
254  */
255  void
256  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) override;
257 
258  /** \brief The source point cloud's feature descriptors. */
260 
261  /** \brief The target point cloud's feature descriptors. */
263 
264  /** \brief The number of samples to use during each iteration. */
266 
267  /** \brief The minimum distances between samples. */
269 
270  /** \brief The number of neighbors to use when selecting a random feature correspondence. */
272 
273  /** \brief The KdTree used to compare feature descriptors. */
275 
277  public:
279  };
280 }
281 
282 #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:196
int nr_samples_
The number of samples to use during each iteration.
Definition: ia_ransac.h:265
boost::shared_ptr< const ErrorFunctor > ConstPtr
Definition: ia_ransac.h:93
typename KdTreeFLANN< FeatureT >::Ptr FeatureKdTreePtr
Definition: ia_ransac.h:135
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
FeatureKdTreePtr feature_tree_
The KdTree used to compare feature descriptors.
Definition: ia_ransac.h:274
void setNumberOfSamples(int nr_samples)
Set the number of samples to use during each iteration.
Definition: ia_ransac.h:185
boost::shared_ptr< const SampleConsensusInitialAlignment< PointSource, PointTarget, FeatureT > > ConstPtr
Definition: ia_ransac.h:86
boost::shared_ptr< ErrorFunctor > Ptr
Definition: ia_ransac.h:92
#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:137
FeatureCloudConstPtr target_features_
The target point cloud&#39;s feature descriptors.
Definition: ia_ransac.h:262
int k_correspondences_
The number of neighbors to use when selecting a random feature correspondence.
Definition: ia_ransac.h:271
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:503
typename ErrorFunctor::Ptr ErrorFunctorPtr
Definition: ia_ransac.h:133
FeatureCloudConstPtr const getTargetFeatures()
Get a pointer to the target point cloud&#39;s features.
Definition: ia_ransac.h:169
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:213
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:200
boost::shared_ptr< PointCloud< FeatureT > > Ptr
Definition: point_cloud.h:415
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:207
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: ia_ransac.h:72
boost::shared_ptr< const PointCloud< FeatureT > > ConstPtr
Definition: point_cloud.h:416
FeatureCloudConstPtr const getSourceFeatures()
Get a pointer to the source point cloud&#39;s features.
Definition: ia_ransac.h:159
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:259
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:189
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:268
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:179
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:175
int getRandomIndex(int n)
Choose a random index between 0 and n-1.
Definition: ia_ransac.h:220