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