Point Cloud Library (PCL) 1.12.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 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 */
57template <typename PointSource, typename PointTarget>
59: public IterativeClosestPoint<PointSource, PointTarget> {
60public:
61 using IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
62 using IterativeClosestPoint<PointSource, PointTarget>::getClassName;
63 using IterativeClosestPoint<PointSource, PointTarget>::indices_;
64 using IterativeClosestPoint<PointSource, PointTarget>::target_;
65 using IterativeClosestPoint<PointSource, PointTarget>::input_;
66 using IterativeClosestPoint<PointSource, PointTarget>::tree_;
67 using IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_;
68 using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
69 using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
70 using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
71 using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
72 using IterativeClosestPoint<PointSource, PointTarget>::transformation_;
73 using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
74 using IterativeClosestPoint<PointSource, PointTarget>::converged_;
75 using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
76 using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
77 using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
78 using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;
79
83
87
90
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. */
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 pcl::Indices& indices_src,
122 const PointCloudTarget& cloud_tgt,
123 const pcl::Indices& 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
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
190 * \param[in] cloud_tgt the target point cloud dataset
191 * \param[in] indices_tgt the vector of indices describing
192 * the correspondences of the interest points from \a indices_src
193 * \param[out] transformation_matrix the resultant transformation matrix
194 */
195 void
197 const pcl::Indices& indices_src,
198 const PointCloudTarget& cloud_tgt,
199 const pcl::Indices& indices_tgt,
200 Eigen::Matrix4f& transformation_matrix);
201
202 /** \brief \return Mahalanobis distance matrix for the given point index */
203 inline const Eigen::Matrix3d&
204 mahalanobis(std::size_t index) const
205 {
206 assert(index < mahalanobis_.size());
207 return mahalanobis_[index];
208 }
209
210 /** \brief Computes rotation matrix derivative.
211 * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
212 * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5]
213 * param x array representing 3D transformation
214 * param R rotation matrix
215 * param g gradient vector
216 */
217 void
218 computeRDerivative(const Vector6d& x, const Eigen::Matrix3d& R, Vector6d& g) const;
219
220 /** \brief Set the rotation epsilon (maximum allowable difference between two
221 * consecutive rotations) in order for an optimization to be considered as having
222 * converged to the final solution.
223 * \param epsilon the rotation epsilon
224 */
225 inline void
226 setRotationEpsilon(double epsilon)
227 {
228 rotation_epsilon_ = epsilon;
229 }
230
231 /** \brief Get the rotation epsilon (maximum allowable difference between two
232 * consecutive rotations) as set by the user.
233 */
234 inline double
236 {
237 return rotation_epsilon_;
238 }
239
240 /** \brief Set the number of neighbors used when selecting a point neighbourhood
241 * to compute covariances.
242 * A higher value will bring more accurate covariance matrix but will make
243 * covariances computation slower.
244 * \param k the number of neighbors to use when computing covariances
245 */
246 void
248 {
250 }
251
252 /** \brief Get the number of neighbors used when computing covariances as set by
253 * the user
254 */
255 int
257 {
258 return k_correspondences_;
259 }
260
261 /** \brief Set maximum number of iterations at the optimization step
262 * \param[in] max maximum number of iterations for the optimizer
263 */
264 void
266 {
268 }
269
270 /** \brief Return maximum number of iterations at the optimization step
271 */
272 int
274 {
276 }
277
278 /** \brief Set the minimal translation gradient threshold for early optimization stop
279 * \param[in] tolerance translation gradient threshold in meters
280 */
281 void
283 {
285 }
286
287 /** \brief Return the minimal translation gradient threshold for early optimization
288 * stop
289 */
290 double
292 {
294 }
295
296 /** \brief Set the minimal rotation gradient threshold for early optimization stop
297 * \param[in] tolerance rotation gradient threshold in radians
298 */
299 void
301 {
303 }
304
305 /** \brief Return the minimal rotation gradient threshold for early optimization stop
306 */
307 double
309 {
311 }
312
313protected:
314 /** \brief The number of neighbors used for covariances computation.
315 * default: 20
316 */
318
319 /** \brief The epsilon constant for gicp paper; this is NOT the convergence
320 * tolerance
321 * default: 0.001
322 */
324
325 /** The epsilon constant for rotation error. (In GICP the transformation epsilon
326 * is split in rotation part and translation part).
327 * default: 2e-3
328 */
330
331 /** \brief base transformation */
332 Eigen::Matrix4f base_transformation_;
333
334 /** \brief Temporary pointer to the source dataset. */
336
337 /** \brief Temporary pointer to the target dataset. */
339
340 /** \brief Temporary pointer to the source dataset indices. */
342
343 /** \brief Temporary pointer to the target dataset indices. */
345
346 /** \brief Input cloud points covariances. */
348
349 /** \brief Target cloud points covariances. */
351
352 /** \brief Mahalanobis matrices holder. */
353 std::vector<Eigen::Matrix3d> mahalanobis_;
354
355 /** \brief maximum number of optimizations */
357
358 /** \brief minimal translation gradient for early optimization stop */
360
361 /** \brief minimal rotation gradient for early optimization stop */
363
364 /** \brief compute points covariances matrices according to the K nearest
365 * neighbors. K is set via setCorrespondenceRandomness() method.
366 * \param cloud pointer to point cloud
367 * \param tree KD tree performer for nearest neighbors search
368 * \param[out] cloud_covariances covariances matrices for each point in the cloud
369 */
370 template <typename PointT>
371 void
373 const typename pcl::search::KdTree<PointT>::Ptr tree,
374 MatricesVector& cloud_covariances);
375
376 /** \return trace of mat1^t . mat2
377 * \param mat1 matrix of dimension nxm
378 * \param mat2 matrix of dimension nxp
379 */
380 inline double
381 matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
382 {
383 double r = 0.;
384 std::size_t n = mat1.rows();
385 // tr(mat1^t.mat2)
386 for (std::size_t i = 0; i < n; i++)
387 for (std::size_t j = 0; j < n; j++)
388 r += mat1(j, i) * mat2(i, j);
389 return r;
390 }
391
392 /** \brief Rigid transformation computation method with initial guess.
393 * \param output the transformed input point cloud dataset using the rigid
394 * transformation found \param guess the initial guess of the transformation to
395 * compute
396 */
397 void
399 const Eigen::Matrix4f& guess) override;
400
401 /** \brief Search for the closest nearest neighbor of a given point.
402 * \param query the point to search a nearest neighbour for
403 * \param index vector of size 1 to store the index of the nearest neighbour found
404 * \param distance vector of size 1 to store the distance to nearest neighbour found
405 */
406 inline bool
407 searchForNeighbors(const PointSource& query,
408 pcl::Indices& index,
409 std::vector<float>& distance)
410 {
411 int k = tree_->nearestKSearch(query, 1, index, distance);
412 if (k == 0)
413 return (false);
414 return (true);
415 }
416
417 /// \brief compute transformation matrix from transformation matrix
418 void
419 applyState(Eigen::Matrix4f& t, const Vector6d& x) const;
420
421 /// \brief optimization functor structure
424 : BFGSDummyFunctor<double, 6>(), gicp_(gicp)
425 {}
426 double
427 operator()(const Vector6d& x) override;
428 void
429 df(const Vector6d& x, Vector6d& df) override;
430 void
431 fdf(const Vector6d& x, double& f, Vector6d& df) override;
433 checkGradient(const Vector6d& g) override;
434
436 };
437
438 std::function<void(const pcl::PointCloud<PointSource>& cloud_src,
439 const pcl::Indices& src_indices,
440 const pcl::PointCloud<PointTarget>& cloud_tgt,
441 const pcl::Indices& tgt_indices,
442 Eigen::Matrix4f& transformation_matrix)>
444};
445} // namespace pcl
446
447#include <pcl/registration/impl/gicp.hpp>
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
Definition gicp.h:59
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition gicp.h:81
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition gicp.h:86
typename Registration< PointSource, PointTarget >::KdTreePtr InputKdTreePtr
Definition gicp.h:97
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition gicp.h:85
PointIndices::Ptr PointIndicesPtr
Definition gicp.h:88
const pcl::Indices * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Definition gicp.h:344
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
Definition gicp.h:335
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
Definition gicp.h:181
int max_inner_iterations_
maximum number of optimizations
Definition gicp.h:356
GeneralizedIterativeClosestPoint()
Empty constructor.
Definition gicp.h:106
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Definition gicp.h:353
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
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:291
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition gicp.hpp:518
int k_correspondences_
The number of neighbors used for covariances computation.
Definition gicp.h:317
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:407
PointIndices::ConstPtr PointIndicesConstPtr
Definition gicp.h:89
const pcl::Indices * tmp_idx_src_
Temporary pointer to the source dataset indices.
Definition gicp.h:341
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
Definition gicp.h:265
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
Definition gicp.h:256
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
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0....
Definition gicp.h:323
double rotation_epsilon_
The epsilon constant for rotation error.
Definition gicp.h:329
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
Definition gicp.h:362
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition gicp.h:82
shared_ptr< MatricesVector > MatricesVectorPtr
Definition gicp.h:93
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
Definition gicp.h:338
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition gicp.h:92
MatricesVectorPtr target_covariances_
Target cloud points covariances.
Definition gicp.h:350
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget > > ConstPtr
Definition gicp.h:101
pcl::PointCloud< PointTarget > PointCloudTarget
Definition gicp.h:84
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
Definition gicp.h:300
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition gicp.hpp:390
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
Definition gicp.h:282
pcl::PointCloud< PointSource > PointCloudSource
Definition gicp.h:80
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
Definition gicp.h:235
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:159
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
Definition gicp.h:247
MatricesVectorPtr input_covariances_
Input cloud points covariances.
Definition gicp.h:347
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
Definition gicp.h:381
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
Definition gicp.hpp:131
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &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:188
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
Definition gicp.h:226
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget > > Ptr
Definition gicp.h:99
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
Definition gicp.h:308
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, Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_
Definition gicp.h:443
Eigen::Matrix< double, 6, 1 > Vector6d
Definition gicp.h:103
typename Registration< PointSource, PointTarget >::KdTree InputKdTree
Definition gicp.h:96
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
Definition gicp.h:359
Eigen::Matrix4f base_transformation_
base transformation
Definition gicp.h:332
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
Definition gicp.h:204
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
Definition gicp.h:273
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition icp.h:97
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: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
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
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.
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...
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
BFGSSpace::Status checkGradient(const Vector6d &g) override
Definition gicp.hpp:368
void df(const Vector6d &x, Vector6d &df) override
Definition gicp.hpp:289
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
Definition gicp.h:423
void fdf(const Vector6d &x, double &f, Vector6d &df) override
Definition gicp.hpp:328
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr