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. */
254 
255  /** \brief The target point cloud's feature descriptors. */
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. */
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_