Point Cloud Library (PCL)  1.11.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 #pragma once
42 
43 #include <pcl/registration/icp.h>
44 #include <pcl/registration/bfgs.h>
45 
46 namespace pcl
47 {
48  /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the
49  * generalized iterative closest point algorithm as described by Alex Segal et al. in
50  * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf
51  * The approach is based on using anistropic cost functions to optimize the alignment
52  * after closest point assignments have been made.
53  * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
54  * FLANN.
55  * \author Nizar Sallem
56  * \ingroup registration
57  */
58  template <typename PointSource, typename PointTarget>
59  class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget>
60  {
61  public:
80 
84 
88 
91 
92  using MatricesVector = std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> >;
93  using MatricesVectorPtr = shared_ptr<MatricesVector>;
94  using MatricesVectorConstPtr = shared_ptr<const MatricesVector>;
95 
98 
99  using Ptr = shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
100  using ConstPtr = shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> >;
101 
102 
103  using Vector6d = Eigen::Matrix<double, 6, 1>;
104 
105  /** \brief Empty constructor. */
107  : k_correspondences_(20)
108  , gicp_epsilon_(0.001)
109  , rotation_epsilon_(2e-3)
110  , mahalanobis_(0)
114  {
116  reg_name_ = "GeneralizedIterativeClosestPoint";
117  max_iterations_ = 200;
120  rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src,
121  const std::vector<int>& indices_src,
122  const PointCloudTarget& cloud_tgt,
123  const std::vector<int>& indices_tgt,
124  Eigen::Matrix4f& transformation_matrix)
125  {
126  estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
127  };
128  }
129 
130  /** \brief Provide a pointer to the input dataset
131  * \param cloud the const boost shared pointer to a PointCloud message
132  */
133  inline void
134  setInputSource (const PointCloudSourceConstPtr &cloud) override
135  {
136 
137  if (cloud->points.empty ())
138  {
139  PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
140  return;
141  }
142  PointCloudSource input = *cloud;
143  // Set all the point.data[3] values to 1 to aid the rigid transformation
144  for (std::size_t i = 0; i < input.size (); ++i)
145  input[i].data[3] = 1.0;
146 
148  input_covariances_.reset ();
149  }
150 
151  /** \brief Provide a pointer to the covariances of the input source (if computed externally!).
152  * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
153  * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
154  * \param[in] covariances the input source covariances
155  */
156  inline void
158  {
159  input_covariances_ = covariances;
160  }
161 
162  /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
163  * \param[in] target the input point cloud target
164  */
165  inline void
166  setInputTarget (const PointCloudTargetConstPtr &target) override
167  {
169  target_covariances_.reset ();
170  }
171 
172  /** \brief Provide a pointer to the covariances of the input target (if computed externally!).
173  * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
174  * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
175  * \param[in] covariances the input target covariances
176  */
177  inline void
179  {
180  target_covariances_ = covariances;
181  }
182 
183  /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative
184  * non-linear Levenberg-Marquardt approach.
185  * \param[in] cloud_src the source point cloud dataset
186  * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
187  * \param[in] cloud_tgt the target point cloud dataset
188  * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a indices_src
189  * \param[out] transformation_matrix the resultant transformation matrix
190  */
191  void
193  const std::vector<int> &indices_src,
194  const PointCloudTarget &cloud_tgt,
195  const std::vector<int> &indices_tgt,
196  Eigen::Matrix4f &transformation_matrix);
197 
198  /** \brief \return Mahalanobis distance matrix for the given point index */
199  inline const Eigen::Matrix3d& mahalanobis(std::size_t index) const
200  {
201  assert(index < mahalanobis_.size());
202  return mahalanobis_[index];
203  }
204 
205  /** \brief Computes rotation matrix derivative.
206  * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
207  * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5]
208  * param x array representing 3D transformation
209  * param R rotation matrix
210  * param g gradient vector
211  */
212  void
213  computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const;
214 
215  /** \brief Set the rotation epsilon (maximum allowable difference between two
216  * consecutive rotations) in order for an optimization to be considered as having
217  * converged to the final solution.
218  * \param epsilon the rotation epsilon
219  */
220  inline void
221  setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }
222 
223  /** \brief Get the rotation epsilon (maximum allowable difference between two
224  * consecutive rotations) as set by the user.
225  */
226  inline double
228 
229  /** \brief Set the number of neighbors used when selecting a point neighbourhood
230  * to compute covariances.
231  * A higher value will bring more accurate covariance matrix but will make
232  * covariances computation slower.
233  * \param k the number of neighbors to use when computing covariances
234  */
235  void
237 
238  /** \brief Get the number of neighbors used when computing covariances as set by
239  * the user
240  */
241  int
243 
244  /** \brief Set maximum number of iterations at the optimization step
245  * \param[in] max maximum number of iterations for the optimizer
246  */
247  void
249 
250  /** \brief Return maximum number of iterations at the optimization step
251  */
252  int
254 
255  /** \brief Set the minimal translation gradient threshold for early optimization stop
256  * \param[in] translation gradient threshold in meters
257  */
258  void
260 
261  /** \brief Return the minimal translation gradient threshold for early optimization stop
262  */
263  double
265 
266  /** \brief Set the minimal rotation gradient threshold for early optimization stop
267  * \param[in] rotation gradient threshold in radians
268  */
269  void
270  setRotationGradientTolerance (double tolerance) { rotation_gradient_tolerance_ = tolerance; }
271 
272  /** \brief Return the minimal rotation gradient threshold for early optimization stop
273  */
274  double
276 
277  protected:
278 
279  /** \brief The number of neighbors used for covariances computation.
280  * default: 20
281  */
283 
284  /** \brief The epsilon constant for gicp paper; this is NOT the convergence
285  * tolerance
286  * default: 0.001
287  */
289 
290  /** The epsilon constant for rotation error. (In GICP the transformation epsilon
291  * is split in rotation part and translation part).
292  * default: 2e-3
293  */
295 
296  /** \brief base transformation */
297  Eigen::Matrix4f base_transformation_;
298 
299  /** \brief Temporary pointer to the source dataset. */
301 
302  /** \brief Temporary pointer to the target dataset. */
304 
305  /** \brief Temporary pointer to the source dataset indices. */
306  const std::vector<int> *tmp_idx_src_;
307 
308  /** \brief Temporary pointer to the target dataset indices. */
309  const std::vector<int> *tmp_idx_tgt_;
310 
311  /** \brief Input cloud points covariances. */
313 
314  /** \brief Target cloud points covariances. */
316 
317  /** \brief Mahalanobis matrices holder. */
318  std::vector<Eigen::Matrix3d> mahalanobis_;
319 
320  /** \brief maximum number of optimizations */
322 
323  /** \brief minimal translation gradient for early optimization stop */
325 
326  /** \brief minimal rotation gradient for early optimization stop */
328 
329  /** \brief compute points covariances matrices according to the K nearest
330  * neighbors. K is set via setCorrespondenceRandomness() method.
331  * \param cloud pointer to point cloud
332  * \param tree KD tree performer for nearest neighbors search
333  * \param[out] cloud_covariances covariances matrices for each point in the cloud
334  */
335  template<typename PointT>
337  const typename pcl::search::KdTree<PointT>::Ptr tree,
338  MatricesVector& cloud_covariances);
339 
340  /** \return trace of mat1^t . mat2
341  * \param mat1 matrix of dimension nxm
342  * \param mat2 matrix of dimension nxp
343  */
344  inline double
345  matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
346  {
347  double r = 0.;
348  std::size_t n = mat1.rows();
349  // tr(mat1^t.mat2)
350  for(std::size_t i = 0; i < n; i++)
351  for(std::size_t j = 0; j < n; j++)
352  r += mat1 (j, i) * mat2 (i,j);
353  return r;
354  }
355 
356  /** \brief Rigid transformation computation method with initial guess.
357  * \param output the transformed input point cloud dataset using the rigid transformation found
358  * \param guess the initial guess of the transformation to compute
359  */
360  void
361  computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override;
362 
363  /** \brief Search for the closest nearest neighbor of a given point.
364  * \param query the point to search a nearest neighbour for
365  * \param index vector of size 1 to store the index of the nearest neighbour found
366  * \param distance vector of size 1 to store the distance to nearest neighbour found
367  */
368  inline bool
369  searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance)
370  {
371  int k = tree_->nearestKSearch (query, 1, index, distance);
372  if (k == 0)
373  return (false);
374  return (true);
375  }
376 
377  /// \brief compute transformation matrix from transformation matrix
378  void applyState(Eigen::Matrix4f &t, const Vector6d& x) const;
379 
380  /// \brief optimization functor structure
382  {
384  : BFGSDummyFunctor<double,6> (), gicp_(gicp) {}
385  double operator() (const Vector6d& x) override;
386  void df(const Vector6d &x, Vector6d &df) override;
387  void fdf(const Vector6d &x, double &f, Vector6d &df) override;
388  BFGSSpace::Status checkGradient(const Vector6d& g) override;
389 
391  };
392 
393  std::function<void(const pcl::PointCloud<PointSource> &cloud_src,
394  const std::vector<int> &src_indices,
395  const pcl::PointCloud<PointTarget> &cloud_tgt,
396  const std::vector<int> &tgt_indices,
397  Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_;
398  };
399 }
400 
401 #include <pcl/registration/impl/gicp.hpp>
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
Definition: gicp.h:60
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: gicp.h:82
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: gicp.h:87
typename Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
Definition: gicp.h:97
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: gicp.h:86
PointIndices::Ptr PointIndicesPtr
Definition: gicp.h:89
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
Definition: gicp.h:300
std::function< void(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &tgt_indices, Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_
Definition: gicp.h:397
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
Definition: gicp.h:178
int max_inner_iterations_
maximum number of optimizations
Definition: gicp.h:321
GeneralizedIterativeClosestPoint()
Empty constructor.
Definition: gicp.h:106
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Definition: gicp.h:318
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
Definition: gicp.hpp:53
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: gicp.h:134
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
Definition: gicp.h:264
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition: gicp.hpp:493
int k_correspondences_
The number of neighbors used for covariances computation.
Definition: gicp.h:282
PointIndices::ConstPtr PointIndicesConstPtr
Definition: gicp.h:90
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
Definition: gicp.h:248
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
Definition: gicp.h:242
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
Definition: gicp.h:99
void setInputTarget(const PointCloudTargetConstPtr &target) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition: gicp.h:166
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0....
Definition: gicp.h:288
double rotation_epsilon_
The epsilon constant for rotation error.
Definition: gicp.h:294
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
Definition: gicp.h:327
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: gicp.h:83
shared_ptr< MatricesVector > MatricesVectorPtr
Definition: gicp.h:93
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
Definition: gicp.h:303
MatricesVectorPtr target_covariances_
Target cloud points covariances.
Definition: gicp.h:315
pcl::PointCloud< PointTarget > PointCloudTarget
Definition: gicp.h:85
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
Definition: gicp.h:270
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition: gicp.hpp:370
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
Definition: gicp.h:259
pcl::PointCloud< PointSource > PointCloudSource
Definition: gicp.h:81
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
Definition: gicp.h:227
shared_ptr< const MatricesVector > MatricesVectorConstPtr
Definition: gicp.h:94
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
Definition: gicp.h:157
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
Definition: gicp.h:100
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
Definition: gicp.h:236
MatricesVectorPtr input_covariances_
Input cloud points covariances.
Definition: gicp.h:312
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
Definition: gicp.h:345
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
Definition: gicp.hpp:130
const std::vector< int > * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Definition: gicp.h:309
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
Definition: gicp.h:221
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
Definition: gicp.h:199
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
Definition: gicp.h:275
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition: gicp.h:92
bool searchForNeighbors(const PointSource &query, std::vector< int > &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
Definition: gicp.h:369
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: gicp.h:103
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const std::vector< int > &indices_src, const PointCloudTarget &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
Definition: gicp.hpp:185
typename Registration< PointSource, PointTarget >::KdTree InputKdTree
Definition: gicp.h:96
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
Definition: gicp.h:324
const std::vector< int > * tmp_idx_src_
Temporary pointer to the source dataset indices.
Definition: gicp.h:306
Eigen::Matrix4f base_transformation_
base transformation
Definition: gicp.h:297
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
Definition: gicp.h:253
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:95
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: icp.h:212
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: icp.h:178
std::size_t size() const
Definition: point_cloud.h:459
shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition: point_cloud.h:430
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:540
std::string reg_name_
The registration method name.
Definition: registration.h:490
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:430
typename KdTree::Ptr KdTreePtr
Definition: registration.h:76
KdTreePtr tree_
A pointer to the spatial search object.
Definition: registration.h:493
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:504
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:524
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
Definition: registration.h:554
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
Status
Definition: bfgs.h:73
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
BFGSSpace::Status checkGradient(const Vector6d &g) override
Definition: gicp.hpp:349
void df(const Vector6d &x, Vector6d &df) override
Definition: gicp.hpp:277
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
Definition: gicp.h:383
const GeneralizedIterativeClosestPoint * gicp_
Definition: gicp.h:390
void fdf(const Vector6d &x, double &f, Vector6d &df) override
Definition: gicp.hpp:313
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:15
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:16