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