Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
46namespace 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 FLANN and Newton's method
53 * for optimization (call `useBFGS` to switch to BFGS optimizer, however Newton
54 * is usually faster and more accurate).
55 * Basic usage example:
56 * \code
57 * pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
58 * reg.setInputSource(src);
59 * reg.setInputTarget(tgt);
60 * // use default parameters or set them yourself, for example:
61 * // reg.setMaximumIterations(...);
62 * // reg.setTransformationEpsilon(...);
63 * // reg.setRotationEpsilon(...);
64 * // reg.setCorrespondenceRandomness(...);
65 * pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
66 * // supply a better guess, if possible:
67 * Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
68 * reg.align(*output, guess);
69 * std::cout << reg.getFinalTransformation() << std::endl;
70 * \endcode
71 * \author Nizar Sallem
72 * \ingroup registration
73 */
74template <typename PointSource, typename PointTarget, typename Scalar = float>
76: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
77public:
78 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
79 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::getClassName;
80 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::indices_;
81 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_;
82 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::input_;
83 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::tree_;
84 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::tree_reciprocal_;
85 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::nr_iterations_;
86 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::max_iterations_;
87 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
89 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
90 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
91 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
93 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::converged_;
94 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
95 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::inlier_threshold_;
96 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
98 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
99
103
107
110
112 std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>;
113 using MatricesVectorPtr = shared_ptr<MatricesVector>;
114 using MatricesVectorConstPtr = shared_ptr<const MatricesVector>;
115
119
120 using Ptr =
121 shared_ptr<GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
122 using ConstPtr = shared_ptr<
124
125 using Vector3 = typename Eigen::Matrix<Scalar, 3, 1>;
126 using Vector4 = typename Eigen::Matrix<Scalar, 4, 1>;
127 using Vector6d = Eigen::Matrix<double, 6, 1>;
128 using Matrix3 = typename Eigen::Matrix<Scalar, 3, 3>;
129 using Matrix4 =
131 using Matrix6d = Eigen::Matrix<double, 6, 6>;
132 using AngleAxis = typename Eigen::AngleAxis<Scalar>;
133
135
136 /** \brief Empty constructor. */
138 {
140 reg_name_ = "GeneralizedIterativeClosestPoint";
141 max_iterations_ = 200;
145 rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src,
146 const pcl::Indices& indices_src,
147 const PointCloudTarget& cloud_tgt,
148 const pcl::Indices& indices_tgt,
149 Matrix4& transformation_matrix) {
151 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
152 };
153 }
154
155 /** \brief Provide a pointer to the input dataset
156 * \param cloud the const boost shared pointer to a PointCloud message
157 */
158 inline void
160 {
161
162 if (cloud->points.empty()) {
163 PCL_ERROR(
164 "[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
165 getClassName().c_str());
166 return;
167 }
168 PointCloudSource input = *cloud;
169 // Set all the point.data[3] values to 1 to aid the rigid transformation
170 for (std::size_t i = 0; i < input.size(); ++i)
171 input[i].data[3] = 1.0;
172
174 input_covariances_.reset();
175 }
176
177 /** \brief Provide a pointer to the covariances of the input source (if computed
178 * externally!). If not set, GeneralizedIterativeClosestPoint will compute the
179 * covariances itself. Make sure to set the covariances AFTER setting the input source
180 * point cloud (setting the input source point cloud will reset the covariances).
181 * \param[in] covariances the input source covariances
182 */
183 inline void
185 {
186 input_covariances_ = covariances;
187 }
188
189 /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
190 * to align the input source to) \param[in] target the input point cloud target
191 */
192 inline void
199
200 /** \brief Provide a pointer to the covariances of the input target (if computed
201 * externally!). If not set, GeneralizedIterativeClosestPoint will compute the
202 * covariances itself. Make sure to set the covariances AFTER setting the input source
203 * point cloud (setting the input source point cloud will reset the covariances).
204 * \param[in] covariances the input target covariances
205 */
206 inline void
208 {
209 target_covariances_ = covariances;
210 }
211
212 /** \brief Estimate a rigid rotation transformation between a source and a target
213 * point cloud using an iterative non-linear BFGS approach.
214 * \param[in] cloud_src the source point cloud dataset
215 * \param[in] indices_src the vector of indices describing
216 * the points of interest in \a cloud_src
217 * \param[in] cloud_tgt the target point cloud dataset
218 * \param[in] indices_tgt the vector of indices describing
219 * the correspondences of the interest points from \a indices_src
220 * \param[in,out] transformation_matrix the resultant transformation matrix
221 */
222 void
224 const pcl::Indices& indices_src,
225 const PointCloudTarget& cloud_tgt,
226 const pcl::Indices& indices_tgt,
227 Matrix4& transformation_matrix);
228
229 /** \brief Estimate a rigid rotation transformation between a source and a target
230 * point cloud using an iterative non-linear Newton approach.
231 * \param[in] cloud_src the source point cloud dataset
232 * \param[in] indices_src the vector of indices describing
233 * the points of interest in \a cloud_src
234 * \param[in] cloud_tgt the target point cloud dataset
235 * \param[in] indices_tgt the vector of indices describing
236 * the correspondences of the interest points from \a indices_src
237 * \param[in,out] transformation_matrix the resultant transformation matrix
238 */
239 void
241 const pcl::Indices& indices_src,
242 const PointCloudTarget& cloud_tgt,
243 const pcl::Indices& indices_tgt,
244 Matrix4& transformation_matrix);
245
246 /** \brief \return Mahalanobis distance matrix for the given point index */
247 inline const Eigen::Matrix3d&
248 mahalanobis(std::size_t index) const
249 {
250 assert(index < mahalanobis_.size());
251 return mahalanobis_[index];
252 }
253
254 /** \brief Computes the derivative of the cost function w.r.t rotation angles.
255 * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
256 * \return d/d_Phi, d/d_Theta, d/d_Psi respectively in g[3], g[4] and g[5]
257 * \param[in] x array representing 3D transformation
258 * \param[in] dCost_dR_T the transpose of the derivative of the cost function w.r.t
259 * rotation matrix
260 * \param[out] g gradient vector
261 */
262 void
264 const Eigen::Matrix3d& dCost_dR_T,
265 Vector6d& g) const;
266
267 /** \brief Set the rotation epsilon (maximum allowable difference between two
268 * consecutive rotations) in order for an optimization to be considered as having
269 * converged to the final solution.
270 * \param epsilon the rotation epsilon
271 */
272 inline void
273 setRotationEpsilon(double epsilon)
274 {
275 rotation_epsilon_ = epsilon;
276 }
277
278 /** \brief Get the rotation epsilon (maximum allowable difference between two
279 * consecutive rotations) as set by the user.
280 */
281 inline double
283 {
284 return rotation_epsilon_;
285 }
286
287 /** \brief Set the number of neighbors used when selecting a point neighbourhood
288 * to compute covariances.
289 * A higher value will bring more accurate covariance matrix but will make
290 * covariances computation slower.
291 * \param k the number of neighbors to use when computing covariances
292 */
293 void
298
299 /** \brief Get the number of neighbors used when computing covariances as set by
300 * the user
301 */
302 int
304 {
305 return k_correspondences_;
306 }
307
308 /** \brief Use BFGS optimizer instead of default Newton optimizer
309 */
310 void
312 {
313 rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src,
314 const pcl::Indices& indices_src,
315 const PointCloudTarget& cloud_tgt,
316 const pcl::Indices& indices_tgt,
317 Matrix4& transformation_matrix) {
319 cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
320 };
321 }
322
323 /** \brief Set maximum number of iterations at the optimization step
324 * \param[in] max maximum number of iterations for the optimizer
325 */
326 void
331
332 /** \brief Return maximum number of iterations at the optimization step
333 */
334 int
339
340 /** \brief Set the minimal translation gradient threshold for early optimization stop
341 * \param[in] tolerance translation gradient threshold in meters
342 */
343 void
345 {
347 }
348
349 /** \brief Return the minimal translation gradient threshold for early optimization
350 * stop
351 */
352 double
357
358 /** \brief Set the minimal rotation gradient threshold for early optimization stop
359 * \param[in] tolerance rotation gradient threshold in radians
360 */
361 void
363 {
365 }
366
367 /** \brief Return the minimal rotation gradient threshold for early optimization stop
368 */
369 double
374
375 /** \brief Initialize the scheduler and set the number of threads to use.
376 * \param nr_threads the number of hardware threads to use (0 sets the value back to
377 * automatic)
378 */
379 void
380 setNumberOfThreads(unsigned int nr_threads = 0);
381
382protected:
383 /** \brief The number of neighbors used for covariances computation.
384 * default: 20
385 */
387
388 /** \brief The epsilon constant for gicp paper; this is NOT the convergence
389 * tolerance
390 * default: 0.001
391 */
392 double gicp_epsilon_{0.001};
393
394 /** The epsilon constant for rotation error. (In GICP the transformation epsilon
395 * is split in rotation part and translation part).
396 * default: 2e-3
397 */
398 double rotation_epsilon_{2e-3};
399
400 /** \brief base transformation */
402
403 /** \brief Temporary pointer to the source dataset. */
405
406 /** \brief Temporary pointer to the target dataset. */
408
409 /** \brief Temporary pointer to the source dataset indices. */
411
412 /** \brief Temporary pointer to the target dataset indices. */
414
415 /** \brief Input cloud points covariances. */
417
418 /** \brief Target cloud points covariances. */
420
421 /** \brief Mahalanobis matrices holder. */
422 std::vector<Eigen::Matrix3d> mahalanobis_;
423
424 /** \brief maximum number of optimizations */
426
427 /** \brief minimal translation gradient for early optimization stop */
429
430 /** \brief minimal rotation gradient for early optimization stop */
432
433 /** \brief compute points covariances matrices according to the K nearest
434 * neighbors. K is set via setCorrespondenceRandomness() method.
435 * \param[in] cloud pointer to point cloud
436 * \param[in] tree KD tree performer for nearest neighbors search
437 * \param[out] cloud_covariances covariances matrices for each point in the cloud
438 */
439 template <typename PointT>
440 void
442 const typename pcl::search::KdTree<PointT>::Ptr tree,
443 MatricesVector& cloud_covariances);
444
445 /** \return trace of mat1 . mat2
446 * \param mat1 matrix of dimension nxm
447 * \param mat2 matrix of dimension mxp
448 */
449 inline double
450 matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
451 {
452 if (mat1.cols() != mat2.rows()) {
453 PCL_THROW_EXCEPTION(PCLException,
454 "The two matrices' shapes don't match. "
455 "They are ("
456 << mat1.rows() << ", " << mat1.cols() << ") and ("
457 << mat2.rows() << ", " << mat2.cols() << ")");
458 }
459 return (mat1 * mat2).trace();
460 }
461
462 /** \brief Rigid transformation computation method with initial guess.
463 * \param output the transformed input point cloud dataset using the rigid
464 * transformation found \param guess the initial guess of the transformation to
465 * compute
466 */
467 void
468 computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
469
470 /** \brief Search for the closest nearest neighbor of a given point.
471 * \param query the point to search a nearest neighbour for
472 * \param index vector of size 1 to store the index of the nearest neighbour found
473 * \param distance vector of size 1 to store the distance to nearest neighbour found
474 */
475 inline bool
476 searchForNeighbors(const PointSource& query,
477 pcl::Indices& index,
478 std::vector<float>& distance)
479 {
480 int k = tree_->nearestKSearch(query, 1, index, distance);
481 if (k == 0)
482 return (false);
483 return (true);
484 }
485
486 /// \brief compute transformation matrix from transformation matrix
487 void
488 applyState(Matrix4& t, const Vector6d& x) const;
489
490 /// \brief optimization functor structure
495 double
496 operator()(const Vector6d& x) override;
497 void
498 df(const Vector6d& x, Vector6d& df) override;
499 void
500 fdf(const Vector6d& x, double& f, Vector6d& df) override;
501 void
502 dfddf(const Vector6d& x, Vector6d& df, Matrix6d& ddf);
504 checkGradient(const Vector6d& g) override;
505
507 };
508
509 std::function<void(const pcl::PointCloud<PointSource>& cloud_src,
510 const pcl::Indices& src_indices,
511 const pcl::PointCloud<PointTarget>& cloud_tgt,
512 const pcl::Indices& tgt_indices,
513 Matrix4& transformation_matrix)>
515
516private:
517 void
518 getRDerivatives(double phi,
519 double theta,
520 double psi,
521 Eigen::Matrix3d& dR_dPhi,
522 Eigen::Matrix3d& dR_dTheta,
523 Eigen::Matrix3d& dR_dPsi) const;
524
525 void
526 getR2ndDerivatives(double phi,
527 double theta,
528 double psi,
529 Eigen::Matrix3d& ddR_dPhi_dPhi,
530 Eigen::Matrix3d& ddR_dPhi_dTheta,
531 Eigen::Matrix3d& ddR_dPhi_dPsi,
532 Eigen::Matrix3d& ddR_dTheta_dTheta,
533 Eigen::Matrix3d& ddR_dTheta_dPsi,
534 Eigen::Matrix3d& ddR_dPsi_dPsi) const;
535
536 /** \brief The number of threads the scheduler should use. */
537 unsigned int threads_;
538};
539} // namespace pcl
540
541#include <pcl/registration/impl/gicp.hpp>
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
Definition gicp.h:76
double rotation_epsilon_
The epsilon constant for rotation error.
Definition gicp.h:398
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:297
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
Definition gicp.h:344
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
Definition gicp.h:303
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
Definition gicp.h:294
typename Registration< PointSource, PointTarget, Scalar >::KdTree InputKdTree
Definition gicp.h:116
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition gicp.h:130
void applyState(Matrix4 &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition gicp.hpp:926
pcl::PointCloud< PointTarget > PointCloudTarget
Definition gicp.h:104
const pcl::Indices * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Definition gicp.h:413
pcl::PointCloud< PointSource > PointCloudSource
Definition gicp.h:100
Matrix4 base_transformation_
base transformation
Definition gicp.h:401
PointIndices::ConstPtr PointIndicesConstPtr
Definition gicp.h:109
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition gicp.h:121
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
Definition gicp.h:428
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition gicp.h:131
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
Definition gicp.h:407
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
Definition gicp.h:248
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
Definition gicp.h:335
MatricesVectorPtr input_covariances_
Input cloud points covariances.
Definition gicp.h:416
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition gicp.h:102
typename Eigen::Matrix< Scalar, 4, 1 > Vector4
Definition gicp.h:126
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition gicp.h:125
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition gicp.h:159
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
Definition gicp.h:207
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition gicp.h:112
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
Definition gicp.h:184
shared_ptr< const MatricesVector > MatricesVectorConstPtr
Definition gicp.h:114
typename Registration< PointSource, PointTarget, Scalar >::KdTreePtr InputKdTreePtr
Definition gicp.h:118
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:73
void estimateRigidTransformationNewton(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:373
void useBFGS()
Use BFGS optimizer instead of default Newton optimizer.
Definition gicp.h:311
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition gicp.h:101
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition gicp.h:105
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
Definition gicp.h:404
int max_inner_iterations_
maximum number of optimizations
Definition gicp.h:425
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0....
Definition gicp.h:392
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
Definition gicp.h:282
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition gicp.h:123
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
Definition gicp.h:273
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
Definition gicp.h:450
PCL_MAKE_ALIGNED_OPERATOR_NEW GeneralizedIterativeClosestPoint()
Empty constructor.
Definition gicp.h:137
PointIndices::Ptr PointIndicesPtr
Definition gicp.h:108
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
Definition gicp.h:370
int k_correspondences_
The number of neighbors used for covariances computation.
Definition gicp.h:386
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition gicp.hpp:50
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Definition gicp.h:422
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
Definition gicp.h:327
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition gicp.h:106
MatricesVectorPtr target_covariances_
Target cloud points covariances.
Definition gicp.h:419
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:205
typename Eigen::AngleAxis< Scalar > AngleAxis
Definition gicp.h:132
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition gicp.hpp:770
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
Definition gicp.h:353
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:193
typename Eigen::Matrix< Scalar, 3, 3 > Matrix3
Definition gicp.h:128
shared_ptr< MatricesVector > MatricesVectorPtr
Definition gicp.h:113
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
Definition gicp.h:431
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:514
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
Definition gicp.h:362
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:476
Eigen::Matrix< double, 6, 1 > Vector6d
Definition gicp.h:127
const pcl::Indices * tmp_idx_src_
Temporary pointer to the source dataset indices.
Definition gicp.h:410
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition icp.h:98
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition icp.h:143
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:232
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:199
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:67
std::size_t size() const
shared_ptr< PointCloud< PointSource > > Ptr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
std::string reg_name_
The registration method name.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
typename KdTree::Ptr KdTreePtr
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
KdTreePtr tree_
A pointer to the spatial search object.
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
bool converged_
Holds internal convergence state, given user parameters.
int max_iterations_
The maximum number of iterations the internal optimization should run for.
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
double inlier_threshold_
The inlier distance threshold for the internal RANSAC outlier rejection loop.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
const std::string & getClassName() const
Abstract class get name method.
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
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:86
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
void dfddf(const Vector6d &x, Vector6d &df, Matrix6d &ddf)
Definition gicp.hpp:601
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
Definition gicp.h:492
void df(const Vector6d &x, Vector6d &df) override
Definition gicp.hpp:514
BFGSSpace::Status checkGradient(const Vector6d &g) override
Definition gicp.hpp:747
void fdf(const Vector6d &x, double &f, Vector6d &df) override
Definition gicp.hpp:557
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr