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