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