Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
gicp.hpp
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#ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
42#define PCL_REGISTRATION_IMPL_GICP_HPP_
43
44#include <pcl/registration/exceptions.h>
45
46namespace pcl {
47
48template <typename PointSource, typename PointTarget, typename Scalar>
49void
51 unsigned int nr_threads)
52{
53#ifdef _OPENMP
54 if (nr_threads == 0)
55 threads_ = omp_get_num_procs();
56 else
57 threads_ = nr_threads;
58 PCL_DEBUG("[pcl::GeneralizedIterativeClosestPoint::setNumberOfThreads] Setting "
59 "number of threads to %u.\n",
60 threads_);
61#else
62 threads_ = 1;
63 if (nr_threads != 1)
64 PCL_WARN("[pcl::GeneralizedIterativeClosestPoint::setNumberOfThreads] "
65 "Parallelization is requested, but OpenMP is not available! Continuing "
66 "without parallelization.\n");
67#endif // _OPENMP
68}
69
70template <typename PointSource, typename PointTarget, typename Scalar>
71template <typename PointT>
72void
75 const typename pcl::search::KdTree<PointT>::Ptr kdtree,
76 MatricesVector& cloud_covariances)
77{
78 if (k_correspondences_ > static_cast<int>(cloud->size())) {
79 PCL_ERROR("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
80 "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
81 cloud->size(),
82 k_correspondences_);
83 return;
84 }
85
86 Eigen::Vector3d mean;
87 Eigen::Matrix3d cov;
88 pcl::Indices nn_indices(k_correspondences_);
89 std::vector<float> nn_dist_sq(k_correspondences_);
90
91 // We should never get there but who knows
92 if (cloud_covariances.size() < cloud->size())
93 cloud_covariances.resize(cloud->size());
94
95#pragma omp parallel for num_threads(threads_) schedule(dynamic, 32) \
96 shared(cloud, cloud_covariances) firstprivate(mean, cov, nn_indices, nn_dist_sq)
97 for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(cloud->size()); ++i) {
98 const PointT& query_point = (*cloud)[i];
99 // Zero out the cov and mean
100 cov.setZero();
101 mean.setZero();
102
103 // Search for the K nearest neighbours
104 kdtree->nearestKSearch(query_point, k_correspondences_, nn_indices, nn_dist_sq);
105
106 // Find the covariance matrix
107 for (int j = 0; j < k_correspondences_; j++) {
108 // de-mean neighbourhood to avoid inaccuracies when far away from origin
109 const double ptx = (*cloud)[nn_indices[j]].x - query_point.x,
110 pty = (*cloud)[nn_indices[j]].y - query_point.y,
111 ptz = (*cloud)[nn_indices[j]].z - query_point.z;
112
113 mean[0] += ptx;
114 mean[1] += pty;
115 mean[2] += ptz;
116
117 cov(0, 0) += ptx * ptx;
118
119 cov(1, 0) += pty * ptx;
120 cov(1, 1) += pty * pty;
121
122 cov(2, 0) += ptz * ptx;
123 cov(2, 1) += ptz * pty;
124 cov(2, 2) += ptz * ptz;
125 }
126
127 mean /= static_cast<double>(k_correspondences_);
128 // Get the actual covariance
129 for (int k = 0; k < 3; k++)
130 for (int l = 0; l <= k; l++) {
131 cov(k, l) /= static_cast<double>(k_correspondences_);
132 cov(k, l) -= mean[k] * mean[l];
133 cov(l, k) = cov(k, l);
134 }
135
136 // Compute the SVD (covariance matrix is symmetric so U = V')
137 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
138 cov.setZero();
139 Eigen::Matrix3d U = svd.matrixU();
140 // Reconstitute the covariance matrix with modified singular values using the column
141 // // vectors in V.
142 for (int k = 0; k < 3; k++) {
143 Eigen::Vector3d col = U.col(k);
144 double v = 1.; // biggest 2 singular values replaced by 1
145 if (k == 2) // smallest singular value replaced by gicp_epsilon
146 v = gicp_epsilon_;
147 cov += v * col * col.transpose();
148 }
149 cloud_covariances[i] = cov;
150 }
151}
152
153template <typename PointSource, typename PointTarget, typename Scalar>
154void
156 double phi,
157 double theta,
158 double psi,
159 Eigen::Matrix3d& dR_dPhi,
160 Eigen::Matrix3d& dR_dTheta,
161 Eigen::Matrix3d& dR_dPsi) const
162{
163 const double cphi = std::cos(phi), sphi = std::sin(phi);
164 const double ctheta = std::cos(theta), stheta = std::sin(theta);
165 const double cpsi = std::cos(psi), spsi = std::sin(psi);
166 dR_dPhi(0, 0) = 0.;
167 dR_dPhi(1, 0) = 0.;
168 dR_dPhi(2, 0) = 0.;
169
170 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
171 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
172 dR_dPhi(2, 1) = cphi * ctheta;
173
174 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
175 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
176 dR_dPhi(2, 2) = -ctheta * sphi;
177
178 dR_dTheta(0, 0) = -cpsi * stheta;
179 dR_dTheta(1, 0) = -spsi * stheta;
180 dR_dTheta(2, 0) = -ctheta;
181
182 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
183 dR_dTheta(1, 1) = ctheta * sphi * spsi;
184 dR_dTheta(2, 1) = -sphi * stheta;
185
186 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
187 dR_dTheta(1, 2) = cphi * ctheta * spsi;
188 dR_dTheta(2, 2) = -cphi * stheta;
189
190 dR_dPsi(0, 0) = -ctheta * spsi;
191 dR_dPsi(1, 0) = cpsi * ctheta;
192 dR_dPsi(2, 0) = 0.;
193
194 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
195 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
196 dR_dPsi(2, 1) = 0.;
197
198 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
199 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
200 dR_dPsi(2, 2) = 0.;
201}
202
203template <typename PointSource, typename PointTarget, typename Scalar>
204void
206 const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const
207{
208 Eigen::Matrix3d dR_dPhi;
209 Eigen::Matrix3d dR_dTheta;
210 Eigen::Matrix3d dR_dPsi;
211 getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
212
213 g[3] = (dR_dPhi * dCost_dR_T).trace();
214 g[4] = (dR_dTheta * dCost_dR_T).trace();
215 g[5] = (dR_dPsi * dCost_dR_T).trace();
216}
217
218template <typename PointSource, typename PointTarget, typename Scalar>
219void
221 double phi,
222 double theta,
223 double psi,
224 Eigen::Matrix3d& ddR_dPhi_dPhi,
225 Eigen::Matrix3d& ddR_dPhi_dTheta,
226 Eigen::Matrix3d& ddR_dPhi_dPsi,
227 Eigen::Matrix3d& ddR_dTheta_dTheta,
228 Eigen::Matrix3d& ddR_dTheta_dPsi,
229 Eigen::Matrix3d& ddR_dPsi_dPsi) const
230{
231 const double sphi = std::sin(phi), stheta = std::sin(theta), spsi = std::sin(psi);
232 const double cphi = std::cos(phi), ctheta = std::cos(theta), cpsi = std::cos(psi);
233 ddR_dPhi_dPhi(0, 0) = 0.0;
234 ddR_dPhi_dPhi(1, 0) = 0.0;
235 ddR_dPhi_dPhi(2, 0) = 0.0;
236 ddR_dPhi_dPhi(0, 1) = -cpsi * stheta * sphi + spsi * cphi;
237 ddR_dPhi_dPhi(1, 1) = -cpsi * cphi - spsi * stheta * sphi;
238 ddR_dPhi_dPhi(2, 1) = -ctheta * sphi;
239 ddR_dPhi_dPhi(0, 2) = -spsi * sphi - cpsi * stheta * cphi;
240 ddR_dPhi_dPhi(1, 2) = -spsi * stheta * cphi + cpsi * sphi;
241 ddR_dPhi_dPhi(2, 2) = -ctheta * cphi;
242
243 ddR_dPhi_dTheta(0, 0) = 0.0;
244 ddR_dPhi_dTheta(1, 0) = 0.0;
245 ddR_dPhi_dTheta(2, 0) = 0.0;
246 ddR_dPhi_dTheta(0, 1) = cpsi * ctheta * cphi;
247 ddR_dPhi_dTheta(1, 1) = spsi * ctheta * cphi;
248 ddR_dPhi_dTheta(2, 1) = -stheta * cphi;
249 ddR_dPhi_dTheta(0, 2) = -cpsi * ctheta * sphi;
250 ddR_dPhi_dTheta(1, 2) = -spsi * ctheta * sphi;
251 ddR_dPhi_dTheta(2, 2) = stheta * sphi;
252
253 ddR_dPhi_dPsi(0, 0) = 0.0;
254 ddR_dPhi_dPsi(1, 0) = 0.0;
255 ddR_dPhi_dPsi(2, 0) = 0.0;
256 ddR_dPhi_dPsi(0, 1) = -spsi * stheta * cphi + cpsi * sphi;
257 ddR_dPhi_dPsi(1, 1) = spsi * sphi + cpsi * stheta * cphi;
258 ddR_dPhi_dPsi(2, 1) = 0.0;
259 ddR_dPhi_dPsi(0, 2) = cpsi * cphi + spsi * stheta * sphi;
260 ddR_dPhi_dPsi(1, 2) = -cpsi * stheta * sphi + spsi * cphi;
261 ddR_dPhi_dPsi(2, 2) = 0.0;
262
263 ddR_dTheta_dTheta(0, 0) = -cpsi * ctheta;
264 ddR_dTheta_dTheta(1, 0) = -spsi * ctheta;
265 ddR_dTheta_dTheta(2, 0) = stheta;
266 ddR_dTheta_dTheta(0, 1) = -cpsi * stheta * sphi;
267 ddR_dTheta_dTheta(1, 1) = -spsi * stheta * sphi;
268 ddR_dTheta_dTheta(2, 1) = -ctheta * sphi;
269 ddR_dTheta_dTheta(0, 2) = -cpsi * stheta * cphi;
270 ddR_dTheta_dTheta(1, 2) = -spsi * stheta * cphi;
271 ddR_dTheta_dTheta(2, 2) = -ctheta * cphi;
272
273 ddR_dTheta_dPsi(0, 0) = spsi * stheta;
274 ddR_dTheta_dPsi(1, 0) = -cpsi * stheta;
275 ddR_dTheta_dPsi(2, 0) = 0.0;
276 ddR_dTheta_dPsi(0, 1) = -spsi * ctheta * sphi;
277 ddR_dTheta_dPsi(1, 1) = cpsi * ctheta * sphi;
278 ddR_dTheta_dPsi(2, 1) = 0.0;
279 ddR_dTheta_dPsi(0, 2) = -spsi * ctheta * cphi;
280 ddR_dTheta_dPsi(1, 2) = cpsi * ctheta * cphi;
281 ddR_dTheta_dPsi(2, 2) = 0.0;
282
283 ddR_dPsi_dPsi(0, 0) = -cpsi * ctheta;
284 ddR_dPsi_dPsi(1, 0) = -spsi * ctheta;
285 ddR_dPsi_dPsi(2, 0) = 0.0;
286 ddR_dPsi_dPsi(0, 1) = -cpsi * stheta * sphi + spsi * cphi;
287 ddR_dPsi_dPsi(1, 1) = -cpsi * cphi - spsi * stheta * sphi;
288 ddR_dPsi_dPsi(2, 1) = 0.0;
289 ddR_dPsi_dPsi(0, 2) = -spsi * sphi - cpsi * stheta * cphi;
290 ddR_dPsi_dPsi(1, 2) = -spsi * stheta * cphi + cpsi * sphi;
291 ddR_dPsi_dPsi(2, 2) = 0.0;
292}
293
294template <typename PointSource, typename PointTarget, typename Scalar>
295void
298 const pcl::Indices& indices_src,
299 const PointCloudTarget& cloud_tgt,
300 const pcl::Indices& indices_tgt,
301 Matrix4& transformation_matrix)
302{
303 // need at least min_number_correspondences_ samples
304 if (indices_src.size() < min_number_correspondences_) {
305 PCL_THROW_EXCEPTION(
307 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
308 "at least "
309 << min_number_correspondences_
310 << " points to estimate a transform! "
311 "Source and target have "
312 << indices_src.size() << " points!");
313 return;
314 }
315 // Set the initial solution
316 Vector6d x = Vector6d::Zero();
317 // translation part
318 x[0] = transformation_matrix(0, 3);
319 x[1] = transformation_matrix(1, 3);
320 x[2] = transformation_matrix(2, 3);
321 // rotation part (Z Y X euler angles convention)
322 // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
323 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
324 x[4] = asin(-transformation_matrix(2, 0));
325 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
326
327 // Set temporary pointers
328 tmp_src_ = &cloud_src;
329 tmp_tgt_ = &cloud_tgt;
330 tmp_idx_src_ = &indices_src;
331 tmp_idx_tgt_ = &indices_tgt;
332
333 // Optimize using BFGS
334 OptimizationFunctorWithIndices functor(this);
336 bfgs.parameters.sigma = 0.01;
337 bfgs.parameters.rho = 0.01;
338 bfgs.parameters.tau1 = 9;
339 bfgs.parameters.tau2 = 0.05;
340 bfgs.parameters.tau3 = 0.5;
341 bfgs.parameters.order = 3;
342
343 int inner_iterations_ = 0;
344 int result = bfgs.minimizeInit(x);
345 result = BFGSSpace::Running;
346 do {
347 inner_iterations_++;
348 result = bfgs.minimizeOneStep(x);
349 if (result) {
350 break;
351 }
352 result = bfgs.testGradient();
353 } while (result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
354 if (result == BFGSSpace::NoProgress || result == BFGSSpace::Success ||
355 inner_iterations_ == max_inner_iterations_) {
356 PCL_DEBUG("[pcl::registration::TransformationEstimationBFGS::"
357 "estimateRigidTransformation]");
358 PCL_DEBUG("BFGS solver finished with exit code %i \n", result);
359 transformation_matrix.setIdentity();
360 applyState(transformation_matrix, x);
361 }
362 else
363 PCL_THROW_EXCEPTION(
365 "[pcl::" << getClassName()
366 << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
367 "solver didn't converge!");
368}
369
370template <typename PointSource, typename PointTarget, typename Scalar>
371void
374 const pcl::Indices& indices_src,
375 const PointCloudTarget& cloud_tgt,
376 const pcl::Indices& indices_tgt,
377 Matrix4& transformation_matrix)
378{
379 // need at least min_number_correspondences_ samples
380 if (indices_src.size() < min_number_correspondences_) {
381 PCL_THROW_EXCEPTION(NotEnoughPointsException,
382 "[pcl::GeneralizedIterativeClosestPoint::"
383 "estimateRigidTransformationNewton] Need "
384 "at least "
385 << min_number_correspondences_
386 << " points to estimate a transform! "
387 "Source and target have "
388 << indices_src.size() << " points!");
389 return;
390 }
391 // Set the initial solution
392 Vector6d x = Vector6d::Zero();
393 // translation part
394 x[0] = transformation_matrix(0, 3);
395 x[1] = transformation_matrix(1, 3);
396 x[2] = transformation_matrix(2, 3);
397 // rotation part (Z Y X euler angles convention)
398 // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
399 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
400 x[4] = std::asin(
401 std::min<double>(1.0, std::max<double>(-1.0, -transformation_matrix(2, 0))));
402 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
403
404 // Set temporary pointers
405 tmp_src_ = &cloud_src;
406 tmp_tgt_ = &cloud_tgt;
407 tmp_idx_src_ = &indices_src;
408 tmp_idx_tgt_ = &indices_tgt;
409
410 // Optimize using Newton
411 OptimizationFunctorWithIndices functor(this);
412 Eigen::Matrix<double, 6, 6> hessian;
413 Eigen::Matrix<double, 6, 1> gradient;
414 double current_x_value = functor(x);
415 functor.dfddf(x, gradient, hessian);
416 Eigen::Matrix<double, 6, 1> delta;
417 int inner_iterations_ = 0;
418 do {
419 ++inner_iterations_;
420 // compute descent direction from hessian and gradient. Take special measures if
421 // hessian is not positive-definite (positive Eigenvalues)
422 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> eigensolver(hessian);
423 Eigen::Matrix<double, 6, 6> inverted_eigenvalues =
424 Eigen::Matrix<double, 6, 6>::Zero();
425 for (int i = 0; i < 6; ++i) {
426 const double ev = eigensolver.eigenvalues()[i];
427 if (ev < 0)
428 inverted_eigenvalues(i, i) = 1.0 / eigensolver.eigenvalues()[5];
429 else
430 inverted_eigenvalues(i, i) = 1.0 / ev;
431 }
432 delta = eigensolver.eigenvectors() * inverted_eigenvalues *
433 eigensolver.eigenvectors().transpose() * gradient;
434
435 // simple line search to guarantee a decrease in the function value
436 double alpha = 1.0;
437 double candidate_x_value;
438 bool improvement_found = false;
439 for (int i = 0; i < 10; ++i, alpha /= 2) {
440 Vector6d candidate_x = x - alpha * delta;
441 candidate_x_value = functor(candidate_x);
442 if (candidate_x_value < current_x_value) {
443 PCL_DEBUG("[estimateRigidTransformationNewton] Using stepsize=%g, function "
444 "value previously: %g, now: %g, "
445 "improvement: %g\n",
446 alpha,
447 current_x_value,
448 candidate_x_value,
449 current_x_value - candidate_x_value);
450 x = candidate_x;
451 current_x_value = candidate_x_value;
452 improvement_found = true;
453 break;
454 }
455 }
456 if (!improvement_found) {
457 PCL_DEBUG("[estimateRigidTransformationNewton] finishing because no progress\n");
458 break;
459 }
460 functor.dfddf(x, gradient, hessian);
461 if (gradient.head<3>().norm() < translation_gradient_tolerance_ &&
462 gradient.tail<3>().norm() < rotation_gradient_tolerance_) {
463 PCL_DEBUG("[estimateRigidTransformationNewton] finishing because gradient below "
464 "threshold: translation: %g<%g, rotation: %g<%g\n",
465 gradient.head<3>().norm(),
466 translation_gradient_tolerance_,
467 gradient.tail<3>().norm(),
468 rotation_gradient_tolerance_);
469 break;
470 }
471 } while (inner_iterations_ < max_inner_iterations_);
472 PCL_DEBUG("[estimateRigidTransformationNewton] solver finished after %i iterations "
473 "(of max %i)\n",
474 inner_iterations_,
475 max_inner_iterations_);
476 transformation_matrix.setIdentity();
477 applyState(transformation_matrix, x);
478}
479
480template <typename PointSource, typename PointTarget, typename Scalar>
481inline double
484{
485 Matrix4 transformation_matrix = gicp_->base_transformation_;
486 gicp_->applyState(transformation_matrix, x);
487 double f = 0;
488 int m = static_cast<int>(gicp_->tmp_idx_src_->size());
489 for (int i = 0; i < m; ++i) {
490 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
491 Vector4fMapConst p_src =
492 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
493 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
494 Vector4fMapConst p_tgt =
495 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
496 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
497 // Estimate the distance (cost function)
498 // The last coordinate is still guaranteed to be set to 1.0
499 // The d here is the negative of the d in the paper
500 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
501 p_trans_src[1] - p_tgt[1],
502 p_trans_src[2] - p_tgt[2]);
503 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
504 // increment= d'*Md/num_matches = d'*M*d/num_matches (we postpone
505 // 1/num_matches after the loop closes)
506 f += static_cast<double>(d.transpose() * Md);
507 }
508 return f / m;
509}
510
511template <typename PointSource, typename PointTarget, typename Scalar>
512inline void
515{
516 Matrix4 transformation_matrix = gicp_->base_transformation_;
517 gicp_->applyState(transformation_matrix, x);
518 // Zero out g
519 g.setZero();
520 // Eigen::Vector3d g_t = g.head<3> ();
521 // the transpose of the derivative of the cost function w.r.t rotation matrix
522 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
523 int m = static_cast<int>(gicp_->tmp_idx_src_->size());
524 for (int i = 0; i < m; ++i) {
525 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
526 Vector4fMapConst p_src =
527 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
528 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
529 Vector4fMapConst p_tgt =
530 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
531
532 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
533 // The last coordinate is still guaranteed to be set to 1.0
534 // The d here is the negative of the d in the paper
535 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
536 p_trans_src[1] - p_tgt[1],
537 p_trans_src[2] - p_tgt[2]);
538 // Md = M*d
539 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
540 // Increment translation gradient
541 // g.head<3> ()+= 2*M*d/num_matches (we postpone 2/num_matches after the loop
542 // closes)
543 g.head<3>() += Md;
544 // Increment rotation gradient
545 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
546 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
547 dCost_dR_T += p_base_src * Md.transpose();
548 }
549 g.head<3>() *= 2.0 / m;
550 dCost_dR_T *= 2.0 / m;
551 gicp_->computeRDerivative(x, dCost_dR_T, g);
552}
553
554template <typename PointSource, typename PointTarget, typename Scalar>
555inline void
558{
559 Matrix4 transformation_matrix = gicp_->base_transformation_;
560 gicp_->applyState(transformation_matrix, x);
561 f = 0;
562 g.setZero();
563 // the transpose of the derivative of the cost function w.r.t rotation matrix
564 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
565 const int m = static_cast<int>(gicp_->tmp_idx_src_->size());
566 for (int i = 0; i < m; ++i) {
567 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
568 Vector4fMapConst p_src =
569 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
570 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
571 Vector4fMapConst p_tgt =
572 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
573 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
574 // The last coordinate is still guaranteed to be set to 1.0
575 // The d here is the negative of the d in the paper
576 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
577 p_trans_src[1] - p_tgt[1],
578 p_trans_src[2] - p_tgt[2]);
579 // Md = M*d
580 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
581 // Increment total error
582 f += static_cast<double>(d.transpose() * Md);
583 // Increment translation gradient
584 // g.head<3> ()+= 2*M*d/num_matches (we postpone 2/num_matches after the loop
585 // closes)
586 g.head<3>() += Md;
587 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
588 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
589 // Increment rotation gradient
590 dCost_dR_T += p_base_src * Md.transpose();
591 }
592 f /= static_cast<double>(m);
593 g.head<3>() *= (2.0 / m);
594 dCost_dR_T *= 2.0 / m;
595 gicp_->computeRDerivative(x, dCost_dR_T, g);
596}
597
598template <typename PointSource, typename PointTarget, typename Scalar>
599inline void
602 Vector6d& gradient,
603 Matrix6d& hessian)
604{
605 Matrix4 transformation_matrix = gicp_->base_transformation_;
606 gicp_->applyState(transformation_matrix, x);
607 const Eigen::Matrix4f transformation_matrix_float =
608 transformation_matrix.template cast<float>();
609 const Eigen::Matrix4f base_transformation_float =
610 gicp_->base_transformation_.template cast<float>();
611 // Zero out gradient and hessian
612 gradient.setZero();
613 hessian.setZero();
614 // Helper matrices
615 Eigen::Matrix3d dR_dPhi;
616 Eigen::Matrix3d dR_dTheta;
617 Eigen::Matrix3d dR_dPsi;
618 gicp_->getRDerivatives(x[3], x[4], x[5], dR_dPhi, dR_dTheta, dR_dPsi);
619 Eigen::Matrix3d ddR_dPhi_dPhi;
620 Eigen::Matrix3d ddR_dPhi_dTheta;
621 Eigen::Matrix3d ddR_dPhi_dPsi;
622 Eigen::Matrix3d ddR_dTheta_dTheta;
623 Eigen::Matrix3d ddR_dTheta_dPsi;
624 Eigen::Matrix3d ddR_dPsi_dPsi;
625 gicp_->getR2ndDerivatives(x[3],
626 x[4],
627 x[5],
628 ddR_dPhi_dPhi,
629 ddR_dPhi_dTheta,
630 ddR_dPhi_dPsi,
631 ddR_dTheta_dTheta,
632 ddR_dTheta_dPsi,
633 ddR_dPsi_dPsi);
634 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
635 Eigen::Matrix3d dCost_dR_T1 = Eigen::Matrix3d::Zero();
636 Eigen::Matrix3d dCost_dR_T2 = Eigen::Matrix3d::Zero();
637 Eigen::Matrix3d dCost_dR_T3 = Eigen::Matrix3d::Zero();
638 Eigen::Matrix3d dCost_dR_T1b = Eigen::Matrix3d::Zero();
639 Eigen::Matrix3d dCost_dR_T2b = Eigen::Matrix3d::Zero();
640 Eigen::Matrix3d dCost_dR_T3b = Eigen::Matrix3d::Zero();
641 Eigen::Matrix3d hessian_rot_phi = Eigen::Matrix3d::Zero();
642 Eigen::Matrix3d hessian_rot_theta = Eigen::Matrix3d::Zero();
643 Eigen::Matrix3d hessian_rot_psi = Eigen::Matrix3d::Zero();
644 Eigen::Matrix<double, 9, 6> hessian_rot_tmp = Eigen::Matrix<double, 9, 6>::Zero();
645
646 int m = static_cast<int>(gicp_->tmp_idx_src_->size());
647 for (int i = 0; i < m; ++i) {
648 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
649 const auto& src_idx = (*gicp_->tmp_idx_src_)[i];
650 Vector4fMapConst p_src = (*gicp_->tmp_src_)[src_idx].getVector4fMap();
651 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
652 Vector4fMapConst p_tgt =
653 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
654 Eigen::Vector4f p_trans_src(transformation_matrix_float * p_src);
655 // The last coordinate is still guaranteed to be set to 1.0
656 // The d here is the negative of the d in the paper
657 const Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
658 p_trans_src[1] - p_tgt[1],
659 p_trans_src[2] - p_tgt[2]);
660 const Eigen::Matrix3d& M = gicp_->mahalanobis(src_idx);
661 const Eigen::Vector3d Md(M * d); // Md = M*d
662 gradient.head<3>() += Md; // translation gradient
663 hessian.topLeftCorner<3, 3>() += M; // translation-translation hessian
664 p_trans_src = base_transformation_float * p_src;
665 const Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
666 dCost_dR_T.noalias() += p_base_src * Md.transpose();
667 dCost_dR_T1b += p_base_src[0] * M;
668 dCost_dR_T2b += p_base_src[1] * M;
669 dCost_dR_T3b += p_base_src[2] * M;
670 hessian_rot_tmp.noalias() +=
671 Eigen::Map<const Eigen::Matrix<double, 9, 1>>{M.data()} *
672 (Eigen::Matrix<double, 1, 6>() << p_base_src[0] * p_base_src[0],
673 p_base_src[0] * p_base_src[1],
674 p_base_src[0] * p_base_src[2],
675 p_base_src[1] * p_base_src[1],
676 p_base_src[1] * p_base_src[2],
677 p_base_src[2] * p_base_src[2])
678 .finished();
679 }
680 gradient.head<3>() *= 2.0 / m; // translation gradient
681 dCost_dR_T *= 2.0 / m;
682 gicp_->computeRDerivative(x, dCost_dR_T, gradient); // rotation gradient
683 hessian.topLeftCorner<3, 3>() *= 2.0 / m; // translation-translation hessian
684 // translation-rotation hessian
685 dCost_dR_T1.row(0) = dCost_dR_T1b.col(0);
686 dCost_dR_T1.row(1) = dCost_dR_T2b.col(0);
687 dCost_dR_T1.row(2) = dCost_dR_T3b.col(0);
688 dCost_dR_T2.row(0) = dCost_dR_T1b.col(1);
689 dCost_dR_T2.row(1) = dCost_dR_T2b.col(1);
690 dCost_dR_T2.row(2) = dCost_dR_T3b.col(1);
691 dCost_dR_T3.row(0) = dCost_dR_T1b.col(2);
692 dCost_dR_T3.row(1) = dCost_dR_T2b.col(2);
693 dCost_dR_T3.row(2) = dCost_dR_T3b.col(2);
694 dCost_dR_T1 *= 2.0 / m;
695 dCost_dR_T2 *= 2.0 / m;
696 dCost_dR_T3 *= 2.0 / m;
697 hessian(3, 0) = (dR_dPhi * dCost_dR_T1).trace();
698 hessian(4, 0) = (dR_dTheta * dCost_dR_T1).trace();
699 hessian(5, 0) = (dR_dPsi * dCost_dR_T1).trace();
700 hessian(3, 1) = (dR_dPhi * dCost_dR_T2).trace();
701 hessian(4, 1) = (dR_dTheta * dCost_dR_T2).trace();
702 hessian(5, 1) = (dR_dPsi * dCost_dR_T2).trace();
703 hessian(3, 2) = (dR_dPhi * dCost_dR_T3).trace();
704 hessian(4, 2) = (dR_dTheta * dCost_dR_T3).trace();
705 hessian(5, 2) = (dR_dPsi * dCost_dR_T3).trace();
706 hessian.block<3, 3>(0, 3) = hessian.block<3, 3>(3, 0).transpose();
707 // rotation-rotation hessian
708 int lookup[3][3] = {{0, 1, 2}, {1, 3, 4}, {2, 4, 5}};
709 for (int l = 0; l < 3; ++l) {
710 for (int i = 0; i < 3; ++i) {
711 double phi_tmp = 0.0, theta_tmp = 0.0, psi_tmp = 0.0;
712 for (int j = 0; j < 3; ++j) {
713 for (int k = 0; k < 3; ++k) {
714 phi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPhi(j, k);
715 theta_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dTheta(j, k);
716 psi_tmp += hessian_rot_tmp(3 * j + i, lookup[l][k]) * dR_dPsi(j, k);
717 }
718 }
719 hessian_rot_phi(i, l) = phi_tmp;
720 hessian_rot_theta(i, l) = theta_tmp;
721 hessian_rot_psi(i, l) = psi_tmp;
722 }
723 }
724 hessian_rot_phi *= 2.0 / m;
725 hessian_rot_theta *= 2.0 / m;
726 hessian_rot_psi *= 2.0 / m;
727 hessian(3, 3) = (dR_dPhi.transpose() * hessian_rot_phi).trace() +
728 (ddR_dPhi_dPhi * dCost_dR_T).trace();
729 hessian(3, 4) = (dR_dPhi.transpose() * hessian_rot_theta).trace() +
730 (ddR_dPhi_dTheta * dCost_dR_T).trace();
731 hessian(3, 5) = (dR_dPhi.transpose() * hessian_rot_psi).trace() +
732 (ddR_dPhi_dPsi * dCost_dR_T).trace();
733 hessian(4, 4) = (dR_dTheta.transpose() * hessian_rot_theta).trace() +
734 (ddR_dTheta_dTheta * dCost_dR_T).trace();
735 hessian(4, 5) = (dR_dTheta.transpose() * hessian_rot_psi).trace() +
736 (ddR_dTheta_dPsi * dCost_dR_T).trace();
737 hessian(5, 5) = (dR_dPsi.transpose() * hessian_rot_psi).trace() +
738 (ddR_dPsi_dPsi * dCost_dR_T).trace();
739 hessian(4, 3) = hessian(3, 4);
740 hessian(5, 3) = hessian(3, 5);
741 hessian(5, 4) = hessian(4, 5);
742}
743
744template <typename PointSource, typename PointTarget, typename Scalar>
748{
749 auto translation_epsilon = gicp_->translation_gradient_tolerance_;
750 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
751
752 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
754
755 // express translation gradient as norm of translation parameters
756 auto translation_grad = g.head<3>().norm();
757
758 // express rotation gradient as a norm of rotation parameters
759 auto rotation_grad = g.tail<3>().norm();
760
761 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
762 return BFGSSpace::Success;
763
764 return BFGSSpace::Running;
765}
766
767template <typename PointSource, typename PointTarget, typename Scalar>
768inline void
771{
773 // Difference between consecutive transforms
774 double delta = 0;
775 // Get the size of the source point cloud
776 const std::size_t N = indices_->size();
777 // Set the mahalanobis matrices to identity
778 mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
779 // Compute target cloud covariance matrices
780 if ((!target_covariances_) || (target_covariances_->empty())) {
781 target_covariances_.reset(new MatricesVector);
782 computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
783 }
784 // Compute input cloud covariance matrices
785 if ((!input_covariances_) || (input_covariances_->empty())) {
786 input_covariances_.reset(new MatricesVector);
787 computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
788 }
789
790 base_transformation_ = Matrix4::Identity();
791 nr_iterations_ = 0;
792 converged_ = false;
793 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
794 pcl::Indices nn_indices(1);
795 std::vector<float> nn_dists(1);
796
797 pcl::transformPointCloud(output, output, guess);
798
799 while (!converged_) {
800 std::size_t cnt = 0;
801 pcl::Indices source_indices(indices_->size());
802 pcl::Indices target_indices(indices_->size());
803
804 // guess corresponds to base_t and transformation_ to t
805 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
806 for (std::size_t i = 0; i < 4; i++)
807 for (std::size_t j = 0; j < 4; j++)
808 for (std::size_t k = 0; k < 4; k++)
809 transform_R(i, j) += static_cast<double>(transformation_(i, k)) *
810 static_cast<double>(guess(k, j));
811
812 Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
813
814 for (std::size_t i = 0; i < N; i++) {
815 PointSource query = output[i];
816 query.getVector4fMap() =
817 transformation_.template cast<float>() * query.getVector4fMap();
818
819 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
820 PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
821 "in the target dataset for point %d in the source!\n",
822 getClassName().c_str(),
823 (*indices_)[i]);
824 return;
825 }
826
827 // Check if the distance to the nearest neighbor is smaller than the user imposed
828 // threshold
829 if (nn_dists[0] < dist_threshold) {
830 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
831 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
832 Eigen::Matrix3d& M = mahalanobis_[i];
833 // M = R*C1
834 M = R * C1;
835 // temp = M*R' + C2 = R*C1*R' + C2
836 Eigen::Matrix3d temp = M * R.transpose();
837 temp += C2;
838 // M = temp^-1
839 M = temp.inverse();
840 source_indices[cnt] = static_cast<int>(i);
841 target_indices[cnt] = nn_indices[0];
842 cnt++;
843 }
844 }
845 // Resize to the actual number of valid correspondences
846 source_indices.resize(cnt);
847 target_indices.resize(cnt);
848 /* optimize transformation using the current assignment and Mahalanobis metrics*/
849 previous_transformation_ = transformation_;
850 // optimization right here
851 try {
852 rigid_transformation_estimation_(
853 output, source_indices, *target_, target_indices, transformation_);
854 /* compute the delta from this iteration */
855 delta = 0.;
856 for (int k = 0; k < 4; k++) {
857 for (int l = 0; l < 4; l++) {
858 double ratio = 1;
859 if (k < 3 && l < 3) // rotation part of the transform
860 ratio = 1. / rotation_epsilon_;
861 else
862 ratio = 1. / transformation_epsilon_;
863 double c_delta =
864 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
865 if (c_delta > delta)
866 delta = c_delta;
867 }
868 }
869 } catch (PCLException& e) {
870 PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
871 getClassName().c_str(),
872 e.what());
873 break;
874 }
875 nr_iterations_++;
876
877 if (update_visualizer_ != nullptr) {
878 PointCloudSourcePtr input_transformed(new PointCloudSource);
879 pcl::transformPointCloud(output, *input_transformed, transformation_);
880 update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
881 }
882
883 // Check for convergence
884 if (nr_iterations_ >= max_iterations_ || delta < 1) {
885 converged_ = true;
886 PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of "
887 "iterations: %d out of %d. Transformation difference: %f\n",
888 getClassName().c_str(),
889 nr_iterations_,
890 max_iterations_,
891 (transformation_ - previous_transformation_).array().abs().sum());
892 previous_transformation_ = transformation_;
893 }
894 else
895 PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
896 getClassName().c_str());
897 }
898 final_transformation_ = previous_transformation_ * guess;
899
900 PCL_DEBUG("Transformation "
901 "is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%"
902 "5f\t%5f\t%5f\t%5f\n",
903 final_transformation_(0, 0),
904 final_transformation_(0, 1),
905 final_transformation_(0, 2),
906 final_transformation_(0, 3),
907 final_transformation_(1, 0),
908 final_transformation_(1, 1),
909 final_transformation_(1, 2),
910 final_transformation_(1, 3),
911 final_transformation_(2, 0),
912 final_transformation_(2, 1),
913 final_transformation_(2, 2),
914 final_transformation_(2, 3),
915 final_transformation_(3, 0),
916 final_transformation_(3, 1),
917 final_transformation_(3, 2),
918 final_transformation_(3, 3));
919
920 // Transform the point cloud
921 pcl::transformPointCloud(*input_, output, final_transformation_);
922}
923
924template <typename PointSource, typename PointTarget, typename Scalar>
925void
927 Matrix4& t, const Vector6d& x) const
928{
929 // Z Y X euler angles convention
930 Matrix3 R = (AngleAxis(static_cast<Scalar>(x[5]), Vector3::UnitZ()) *
931 AngleAxis(static_cast<Scalar>(x[4]), Vector3::UnitY()) *
932 AngleAxis(static_cast<Scalar>(x[3]), Vector3::UnitX()))
933 .toRotationMatrix();
934 Matrix4 T = Matrix4::Identity();
935 T.template block<3, 3>(0, 0) = R;
936 T.template block<3, 1>(0, 3) = Vector3(
937 static_cast<Scalar>(x[0]), static_cast<Scalar>(x[1]), static_cast<Scalar>(x[2]));
938 t = T * t;
939}
940
941} // namespace pcl
942
943#endif // PCL_REGISTRATION_IMPL_GICP_HPP_
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
Definition bfgs.h:121
BFGSSpace::Status testGradient()
Definition bfgs.h:476
BFGSSpace::Status minimizeInit(FVectorType &x)
Definition bfgs.h:361
BFGSSpace::Status minimizeOneStep(FVectorType &x)
Definition bfgs.h:393
Parameters parameters
Definition bfgs.h:169
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
Definition gicp.h:76
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
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
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition gicp.h:131
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition gicp.h:125
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition gicp.h:112
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
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition gicp.h:101
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition gicp.hpp:50
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
typename Eigen::Matrix< Scalar, 3, 3 > Matrix3
Definition gicp.h:128
Eigen::Matrix< double, 6, 1 > Vector6d
Definition gicp.h:127
An exception that is thrown when the number of correspondents is not equal to the minimum required.
Definition exceptions.h:63
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:67
std::size_t size() const
shared_ptr< const PointCloud< PointT > > ConstPtr
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
An exception that is thrown when the non linear solver didn't converge.
Definition exceptions.h:49
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition kdtree.hpp:88
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
@ NoProgress
Definition bfgs.h:75
@ Running
Definition bfgs.h:73
@ Success
Definition bfgs.h:74
@ NegativeGradientEpsilon
Definition bfgs.h:71
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
Scalar sigma
Definition bfgs.h:147
Scalar tau2
Definition bfgs.h:149
Scalar rho
Definition bfgs.h:146
Scalar tau1
Definition bfgs.h:148
Scalar tau3
Definition bfgs.h:150
Index order
Definition bfgs.h:152
void dfddf(const Vector6d &x, Vector6d &df, Matrix6d &ddf)
Definition gicp.hpp:601
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
A point structure representing Euclidean xyz coordinates, and the RGB color.