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