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