Point Cloud Library (PCL)  1.7.1
gicp.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, 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 #ifndef PCL_GICP_H_
42 #define PCL_GICP_H_
43 
44 #include <pcl/registration/icp.h>
45 #include <pcl/registration/bfgs.h>
46 
47 namespace pcl
48 {
49  /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the
50  * generalized iterative closest point algorithm as described by Alex Segal et al. in
51  * http://www.stanford.edu/~avsegal/resources/papers/Generalized_ICP.pdf
52  * The approach is based on using anistropic cost functions to optimize the alignment
53  * after closest point assignments have been made.
54  * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
55  * FLANN.
56  * \author Nizar Sallem
57  * \ingroup registration
58  */
59  template <typename PointSource, typename PointTarget>
60  class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget>
61  {
62  public:
81 
85 
89 
92 
95 
96  typedef boost::shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> > Ptr;
97  typedef boost::shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> > ConstPtr;
98 
99 
100  typedef Eigen::Matrix<double, 6, 1> Vector6d;
101 
102  /** \brief Empty constructor. */
104  : k_correspondences_(20)
105  , gicp_epsilon_(0.001)
106  , rotation_epsilon_(2e-3)
107  , input_covariances_(0)
109  , mahalanobis_(0)
111  {
113  reg_name_ = "GeneralizedIterativeClosestPoint";
114  max_iterations_ = 200;
119  this, _1, _2, _3, _4, _5);
120  }
121 
122  /** \brief Provide a pointer to the input dataset
123  * \param cloud the const boost shared pointer to a PointCloud message
124  */
125  PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.");
126 
127  /** \brief Provide a pointer to the input dataset
128  * \param cloud the const boost shared pointer to a PointCloud message
129  */
130  inline void
132  {
133 
134  if (cloud->points.empty ())
135  {
136  PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
137  return;
138  }
139  PointCloudSource input = *cloud;
140  // Set all the point.data[3] values to 1 to aid the rigid transformation
141  for (size_t i = 0; i < input.size (); ++i)
142  input[i].data[3] = 1.0;
143 
145  input_covariances_.clear ();
146  input_covariances_.reserve (input_->size ());
147  }
148 
149  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
150  * \param[in] target the input point cloud target
151  */
152  inline void
154  {
156  target_covariances_.clear ();
157  target_covariances_.reserve (target_->size ());
158  }
159 
160  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative
161  * non-linear Levenberg-Marquardt approach.
162  * \param[in] cloud_src the source point cloud dataset
163  * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
164  * \param[in] cloud_tgt the target point cloud dataset
165  * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src
166  * \param[out] transformation_matrix the resultant transformation matrix
167  */
168  void
170  const std::vector<int> &indices_src,
171  const PointCloudTarget &cloud_tgt,
172  const std::vector<int> &indices_tgt,
173  Eigen::Matrix4f &transformation_matrix);
174 
175  /** \brief \return Mahalanobis distance matrix for the given point index */
176  inline const Eigen::Matrix3d& mahalanobis(size_t index) const
177  {
178  assert(index < mahalanobis_.size());
179  return mahalanobis_[index];
180  }
181 
182  /** \brief Computes rotation matrix derivative.
183  * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
184  * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5]
185  * param x array representing 3D transformation
186  * param R rotation matrix
187  * param g gradient vector
188  */
189  void
190  computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const;
191 
192  /** \brief Set the rotation epsilon (maximum allowable difference between two
193  * consecutive rotations) in order for an optimization to be considered as having
194  * converged to the final solution.
195  * \param epsilon the rotation epsilon
196  */
197  inline void
198  setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }
199 
200  /** \brief Get the rotation epsilon (maximum allowable difference between two
201  * consecutive rotations) as set by the user.
202  */
203  inline double
205 
206  /** \brief Set the number of neighbors used when selecting a point neighbourhood
207  * to compute covariances.
208  * A higher value will bring more accurate covariance matrix but will make
209  * covariances computation slower.
210  * \param k the number of neighbors to use when computing covariances
211  */
212  void
214 
215  /** \brief Get the number of neighbors used when computing covariances as set by
216  * the user
217  */
218  int
220 
221  /** set maximum number of iterations at the optimization step
222  * \param[in] max maximum number of iterations for the optimizer
223  */
224  void
226 
227  ///\return maximum number of iterations at the optimization step
228  int
230 
231  protected:
232 
233  /** \brief The number of neighbors used for covariances computation.
234  * default: 20
235  */
237 
238  /** \brief The epsilon constant for gicp paper; this is NOT the convergence
239  * tolerence
240  * default: 0.001
241  */
243 
244  /** The epsilon constant for rotation error. (In GICP the transformation epsilon
245  * is split in rotation part and translation part).
246  * default: 2e-3
247  */
249 
250  /** \brief base transformation */
251  Eigen::Matrix4f base_transformation_;
252 
253  /** \brief Temporary pointer to the source dataset. */
255 
256  /** \brief Temporary pointer to the target dataset. */
258 
259  /** \brief Temporary pointer to the source dataset indices. */
260  const std::vector<int> *tmp_idx_src_;
261 
262  /** \brief Temporary pointer to the target dataset indices. */
263  const std::vector<int> *tmp_idx_tgt_;
264 
265 
266  /** \brief Input cloud points covariances. */
267  std::vector<Eigen::Matrix3d> input_covariances_;
268 
269  /** \brief Target cloud points covariances. */
270  std::vector<Eigen::Matrix3d> target_covariances_;
271 
272  /** \brief Mahalanobis matrices holder. */
273  std::vector<Eigen::Matrix3d> mahalanobis_;
274 
275  /** \brief maximum number of optimizations */
277 
278  /** \brief compute points covariances matrices according to the K nearest
279  * neighbors. K is set via setCorrespondenceRandomness() methode.
280  * \param cloud pointer to point cloud
281  * \param tree KD tree performer for nearest neighbors search
282  * \return cloud_covariance covariances matrices for each point in the cloud
283  */
284  template<typename PointT>
286  const typename pcl::search::KdTree<PointT>::Ptr tree,
287  std::vector<Eigen::Matrix3d>& cloud_covariances);
288 
289  /** \return trace of mat1^t . mat2
290  * \param mat1 matrix of dimension nxm
291  * \param mat2 matrix of dimension nxp
292  */
293  inline double
294  matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
295  {
296  double r = 0.;
297  size_t n = mat1.rows();
298  // tr(mat1^t.mat2)
299  for(size_t i = 0; i < n; i++)
300  for(size_t j = 0; j < n; j++)
301  r += mat1 (j, i) * mat2 (i,j);
302  return r;
303  }
304 
305  /** \brief Rigid transformation computation method with initial guess.
306  * \param output the transformed input point cloud dataset using the rigid transformation found
307  * \param guess the initial guess of the transformation to compute
308  */
309  void
310  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);
311 
312  /** \brief Search for the closest nearest neighbor of a given point.
313  * \param query the point to search a nearest neighbour for
314  * \param index vector of size 1 to store the index of the nearest neighbour found
315  * \param distance vector of size 1 to store the distance to nearest neighbour found
316  */
317  inline bool
318  searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance)
319  {
320  int k = tree_->nearestKSearch (query, 1, index, distance);
321  if (k == 0)
322  return (false);
323  return (true);
324  }
325 
326  /// \brief compute transformation matrix from transformation matrix
327  void applyState(Eigen::Matrix4f &t, const Vector6d& x) const;
328 
329  /// \brief optimization functor structure
331  {
333  : BFGSDummyFunctor<double,6> (), gicp_(gicp) {}
334  double operator() (const Vector6d& x);
335  void df(const Vector6d &x, Vector6d &df);
336  void fdf(const Vector6d &x, double &f, Vector6d &df);
337 
339  };
340 
341  boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
342  const std::vector<int> &src_indices,
343  const pcl::PointCloud<PointTarget> &cloud_tgt,
344  const std::vector<int> &tgt_indices,
345  Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_;
346  };
347 }
348 
349 #include <pcl/registration/impl/gicp.hpp>
350 
351 #endif //#ifndef PCL_GICP_H_