Point Cloud Library (PCL) 1.13.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_ > 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;
66 nn_indices.reserve(k_correspondences_);
67 std::vector<float> nn_dist_sq;
68 nn_dist_sq.reserve(k_correspondences_);
69
70 // We should never get there but who knows
71 if (cloud_covariances.size() < cloud->size())
72 cloud_covariances.resize(cloud->size());
73
74 auto matrices_iterator = cloud_covariances.begin();
75 for (auto points_iterator = cloud->begin(); points_iterator != cloud->end();
76 ++points_iterator, ++matrices_iterator) {
77 const PointT& query_point = *points_iterator;
78 Eigen::Matrix3d& cov = *matrices_iterator;
79 // Zero out the cov and mean
80 cov.setZero();
81 mean.setZero();
82
83 // Search for the K nearest neighbours
84 kdtree->nearestKSearch(query_point, k_correspondences_, nn_indices, nn_dist_sq);
85
86 // Find the covariance matrix
87 for (int j = 0; j < k_correspondences_; j++) {
88 const PointT& pt = (*cloud)[nn_indices[j]];
89
90 mean[0] += pt.x;
91 mean[1] += pt.y;
92 mean[2] += pt.z;
93
94 cov(0, 0) += pt.x * pt.x;
95
96 cov(1, 0) += pt.y * pt.x;
97 cov(1, 1) += pt.y * pt.y;
98
99 cov(2, 0) += pt.z * pt.x;
100 cov(2, 1) += pt.z * pt.y;
101 cov(2, 2) += pt.z * pt.z;
102 }
103
104 mean /= static_cast<double>(k_correspondences_);
105 // Get the actual covariance
106 for (int k = 0; k < 3; k++)
107 for (int l = 0; l <= k; l++) {
108 cov(k, l) /= static_cast<double>(k_correspondences_);
109 cov(k, l) -= mean[k] * mean[l];
110 cov(l, k) = cov(k, l);
111 }
112
113 // Compute the SVD (covariance matrix is symmetric so U = V')
114 Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
115 cov.setZero();
116 Eigen::Matrix3d U = svd.matrixU();
117 // Reconstitute the covariance matrix with modified singular values using the column
118 // // vectors in V.
119 for (int k = 0; k < 3; k++) {
120 Eigen::Vector3d col = U.col(k);
121 double v = 1.; // biggest 2 singular values replaced by 1
122 if (k == 2) // smallest singular value replaced by gicp_epsilon
123 v = gicp_epsilon_;
124 cov += v * col * col.transpose();
125 }
126 }
127}
128
129template <typename PointSource, typename PointTarget, typename Scalar>
130void
132 const Vector6d& x, const Eigen::Matrix3d& dCost_dR_T, Vector6d& g) const
133{
134 Eigen::Matrix3d dR_dPhi;
135 Eigen::Matrix3d dR_dTheta;
136 Eigen::Matrix3d dR_dPsi;
137
138 double phi = x[3], theta = x[4], psi = x[5];
139
140 double cphi = std::cos(phi), sphi = sin(phi);
141 double ctheta = std::cos(theta), stheta = sin(theta);
142 double cpsi = std::cos(psi), spsi = sin(psi);
143
144 dR_dPhi(0, 0) = 0.;
145 dR_dPhi(1, 0) = 0.;
146 dR_dPhi(2, 0) = 0.;
147
148 dR_dPhi(0, 1) = sphi * spsi + cphi * cpsi * stheta;
149 dR_dPhi(1, 1) = -cpsi * sphi + cphi * spsi * stheta;
150 dR_dPhi(2, 1) = cphi * ctheta;
151
152 dR_dPhi(0, 2) = cphi * spsi - cpsi * sphi * stheta;
153 dR_dPhi(1, 2) = -cphi * cpsi - sphi * spsi * stheta;
154 dR_dPhi(2, 2) = -ctheta * sphi;
155
156 dR_dTheta(0, 0) = -cpsi * stheta;
157 dR_dTheta(1, 0) = -spsi * stheta;
158 dR_dTheta(2, 0) = -ctheta;
159
160 dR_dTheta(0, 1) = cpsi * ctheta * sphi;
161 dR_dTheta(1, 1) = ctheta * sphi * spsi;
162 dR_dTheta(2, 1) = -sphi * stheta;
163
164 dR_dTheta(0, 2) = cphi * cpsi * ctheta;
165 dR_dTheta(1, 2) = cphi * ctheta * spsi;
166 dR_dTheta(2, 2) = -cphi * stheta;
167
168 dR_dPsi(0, 0) = -ctheta * spsi;
169 dR_dPsi(1, 0) = cpsi * ctheta;
170 dR_dPsi(2, 0) = 0.;
171
172 dR_dPsi(0, 1) = -cphi * cpsi - sphi * spsi * stheta;
173 dR_dPsi(1, 1) = -cphi * spsi + cpsi * sphi * stheta;
174 dR_dPsi(2, 1) = 0.;
175
176 dR_dPsi(0, 2) = cpsi * sphi - cphi * spsi * stheta;
177 dR_dPsi(1, 2) = sphi * spsi + cphi * cpsi * stheta;
178 dR_dPsi(2, 2) = 0.;
179
180 g[3] = matricesInnerProd(dR_dPhi, dCost_dR_T);
181 g[4] = matricesInnerProd(dR_dTheta, dCost_dR_T);
182 g[5] = matricesInnerProd(dR_dPsi, dCost_dR_T);
183}
184
185template <typename PointSource, typename PointTarget, typename Scalar>
186void
189 const pcl::Indices& indices_src,
190 const PointCloudTarget& cloud_tgt,
191 const pcl::Indices& indices_tgt,
192 Matrix4& transformation_matrix)
193{
194 // need at least min_number_correspondences_ samples
195 if (indices_src.size() < min_number_correspondences_) {
196 PCL_THROW_EXCEPTION(
198 "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
199 "at least "
200 << min_number_correspondences_
201 << " points to estimate a transform! "
202 "Source and target have "
203 << indices_src.size() << " points!");
204 return;
205 }
206 // Set the initial solution
207 Vector6d x = Vector6d::Zero();
208 // translation part
209 x[0] = transformation_matrix(0, 3);
210 x[1] = transformation_matrix(1, 3);
211 x[2] = transformation_matrix(2, 3);
212 // rotation part (Z Y X euler angles convention)
213 // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
214 x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
215 x[4] = asin(-transformation_matrix(2, 0));
216 x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
217
218 // Set temporary pointers
219 tmp_src_ = &cloud_src;
220 tmp_tgt_ = &cloud_tgt;
221 tmp_idx_src_ = &indices_src;
222 tmp_idx_tgt_ = &indices_tgt;
223
224 // Optimize using BFGS
225 OptimizationFunctorWithIndices functor(this);
227 bfgs.parameters.sigma = 0.01;
228 bfgs.parameters.rho = 0.01;
229 bfgs.parameters.tau1 = 9;
230 bfgs.parameters.tau2 = 0.05;
231 bfgs.parameters.tau3 = 0.5;
232 bfgs.parameters.order = 3;
233
234 int inner_iterations_ = 0;
235 int result = bfgs.minimizeInit(x);
236 result = BFGSSpace::Running;
237 do {
238 inner_iterations_++;
239 result = bfgs.minimizeOneStep(x);
240 if (result) {
241 break;
242 }
243 result = bfgs.testGradient();
244 } while (result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
245 if (result == BFGSSpace::NoProgress || result == BFGSSpace::Success ||
246 inner_iterations_ == max_inner_iterations_) {
247 PCL_DEBUG("[pcl::registration::TransformationEstimationBFGS::"
248 "estimateRigidTransformation]");
249 PCL_DEBUG("BFGS solver finished with exit code %i \n", result);
250 transformation_matrix.setIdentity();
251 applyState(transformation_matrix, x);
252 }
253 else
254 PCL_THROW_EXCEPTION(
255 SolverDidntConvergeException,
256 "[pcl::" << getClassName()
257 << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
258 "solver didn't converge!");
259}
260
261template <typename PointSource, typename PointTarget, typename Scalar>
262inline double
265{
266 Matrix4 transformation_matrix = gicp_->base_transformation_;
267 gicp_->applyState(transformation_matrix, x);
268 double f = 0;
269 int m = static_cast<int>(gicp_->tmp_idx_src_->size());
270 for (int i = 0; i < m; ++i) {
271 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
272 Vector4fMapConst p_src =
273 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
274 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
275 Vector4fMapConst p_tgt =
276 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
277 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
278 // Estimate the distance (cost function)
279 // The last coordinate is still guaranteed to be set to 1.0
280 // The d here is the negative of the d in the paper
281 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
282 p_trans_src[1] - p_tgt[1],
283 p_trans_src[2] - p_tgt[2]);
284 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
285 // increment= d'*Md/num_matches = d'*M*d/num_matches (we postpone
286 // 1/num_matches after the loop closes)
287 f += double(d.transpose() * Md);
288 }
289 return f / m;
290}
291
292template <typename PointSource, typename PointTarget, typename Scalar>
293inline void
296{
297 Matrix4 transformation_matrix = gicp_->base_transformation_;
298 gicp_->applyState(transformation_matrix, x);
299 // Zero out g
300 g.setZero();
301 // Eigen::Vector3d g_t = g.head<3> ();
302 // the transpose of the derivative of the cost function w.r.t rotation matrix
303 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
304 int m = static_cast<int>(gicp_->tmp_idx_src_->size());
305 for (int i = 0; i < m; ++i) {
306 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
307 Vector4fMapConst p_src =
308 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
309 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
310 Vector4fMapConst p_tgt =
311 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
312
313 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
314 // The last coordinate is still guaranteed to be set to 1.0
315 // The d here is the negative of the d in the paper
316 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
317 p_trans_src[1] - p_tgt[1],
318 p_trans_src[2] - p_tgt[2]);
319 // Md = M*d
320 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
321 // Increment translation gradient
322 // g.head<3> ()+= 2*M*d/num_matches (we postpone 2/num_matches after the loop
323 // closes)
324 g.head<3>() += Md;
325 // Increment rotation gradient
326 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
327 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
328 dCost_dR_T += p_base_src * Md.transpose();
329 }
330 g.head<3>() *= 2.0 / m;
331 dCost_dR_T *= 2.0 / m;
332 gicp_->computeRDerivative(x, dCost_dR_T, g);
333}
334
335template <typename PointSource, typename PointTarget, typename Scalar>
336inline void
339{
340 Matrix4 transformation_matrix = gicp_->base_transformation_;
341 gicp_->applyState(transformation_matrix, x);
342 f = 0;
343 g.setZero();
344 // the transpose of the derivative of the cost function w.r.t rotation matrix
345 Eigen::Matrix3d dCost_dR_T = Eigen::Matrix3d::Zero();
346 const int m = static_cast<int>(gicp_->tmp_idx_src_->size());
347 for (int i = 0; i < m; ++i) {
348 // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
349 Vector4fMapConst p_src =
350 (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
351 // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
352 Vector4fMapConst p_tgt =
353 (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
354 Eigen::Vector4f p_trans_src(transformation_matrix.template cast<float>() * p_src);
355 // The last coordinate is still guaranteed to be set to 1.0
356 // The d here is the negative of the d in the paper
357 Eigen::Vector3d d(p_trans_src[0] - p_tgt[0],
358 p_trans_src[1] - p_tgt[1],
359 p_trans_src[2] - p_tgt[2]);
360 // Md = M*d
361 Eigen::Vector3d Md(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * d);
362 // Increment total error
363 f += double(d.transpose() * Md);
364 // Increment translation gradient
365 // g.head<3> ()+= 2*M*d/num_matches (we postpone 2/num_matches after the loop
366 // closes)
367 g.head<3>() += Md;
368 p_trans_src = gicp_->base_transformation_.template cast<float>() * p_src;
369 Eigen::Vector3d p_base_src(p_trans_src[0], p_trans_src[1], p_trans_src[2]);
370 // Increment rotation gradient
371 dCost_dR_T += p_base_src * Md.transpose();
372 }
373 f /= double(m);
374 g.head<3>() *= double(2.0 / m);
375 dCost_dR_T *= 2.0 / m;
376 gicp_->computeRDerivative(x, dCost_dR_T, g);
377}
378
379template <typename PointSource, typename PointTarget, typename Scalar>
383{
384 auto translation_epsilon = gicp_->translation_gradient_tolerance_;
385 auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
386
387 if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
389
390 // express translation gradient as norm of translation parameters
391 auto translation_grad = g.head<3>().norm();
392
393 // express rotation gradient as a norm of rotation parameters
394 auto rotation_grad = g.tail<3>().norm();
395
396 if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
397 return BFGSSpace::Success;
398
399 return BFGSSpace::Running;
400}
401
402template <typename PointSource, typename PointTarget, typename Scalar>
403inline void
406{
408 // Difference between consecutive transforms
409 double delta = 0;
410 // Get the size of the source point cloud
411 const std::size_t N = indices_->size();
412 // Set the mahalanobis matrices to identity
413 mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
414 // Compute target cloud covariance matrices
415 if ((!target_covariances_) || (target_covariances_->empty())) {
416 target_covariances_.reset(new MatricesVector);
417 computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
418 }
419 // Compute input cloud covariance matrices
420 if ((!input_covariances_) || (input_covariances_->empty())) {
421 input_covariances_.reset(new MatricesVector);
422 computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
423 }
424
425 base_transformation_ = Matrix4::Identity();
426 nr_iterations_ = 0;
427 converged_ = false;
428 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
429 pcl::Indices nn_indices(1);
430 std::vector<float> nn_dists(1);
431
432 pcl::transformPointCloud(output, output, guess);
433
434 while (!converged_) {
435 std::size_t cnt = 0;
436 pcl::Indices source_indices(indices_->size());
437 pcl::Indices target_indices(indices_->size());
438
439 // guess corresponds to base_t and transformation_ to t
440 Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
441 for (std::size_t i = 0; i < 4; i++)
442 for (std::size_t j = 0; j < 4; j++)
443 for (std::size_t k = 0; k < 4; k++)
444 transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j));
446 Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
448 for (std::size_t i = 0; i < N; i++) {
449 PointSource query = output[i];
450 query.getVector4fMap() =
451 transformation_.template cast<float>() * query.getVector4fMap();
452
453 if (!searchForNeighbors(query, nn_indices, nn_dists)) {
454 PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
455 "in the target dataset for point %d in the source!\n",
456 getClassName().c_str(),
457 (*indices_)[i]);
458 return;
459 }
460
461 // Check if the distance to the nearest neighbor is smaller than the user imposed
462 // threshold
463 if (nn_dists[0] < dist_threshold) {
464 Eigen::Matrix3d& C1 = (*input_covariances_)[i];
465 Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
466 Eigen::Matrix3d& M = mahalanobis_[i];
467 // M = R*C1
468 M = R * C1;
469 // temp = M*R' + C2 = R*C1*R' + C2
470 Eigen::Matrix3d temp = M * R.transpose();
471 temp += C2;
472 // M = temp^-1
473 M = temp.inverse();
474 source_indices[cnt] = static_cast<int>(i);
475 target_indices[cnt] = nn_indices[0];
476 cnt++;
477 }
478 }
479 // Resize to the actual number of valid correspondences
480 source_indices.resize(cnt);
481 target_indices.resize(cnt);
482 /* optimize transformation using the current assignment and Mahalanobis metrics*/
483 previous_transformation_ = transformation_;
484 // optimization right here
485 try {
486 rigid_transformation_estimation_(
487 output, source_indices, *target_, target_indices, transformation_);
488 /* compute the delta from this iteration */
489 delta = 0.;
490 for (int k = 0; k < 4; k++) {
491 for (int l = 0; l < 4; l++) {
492 double ratio = 1;
493 if (k < 3 && l < 3) // rotation part of the transform
494 ratio = 1. / rotation_epsilon_;
495 else
496 ratio = 1. / transformation_epsilon_;
497 double c_delta =
498 ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
499 if (c_delta > delta)
500 delta = c_delta;
501 }
502 }
503 } catch (PCLException& e) {
504 PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
505 getClassName().c_str(),
506 e.what());
507 break;
508 }
509 nr_iterations_++;
510
511 if (update_visualizer_ != nullptr) {
512 PointCloudSourcePtr input_transformed(new PointCloudSource);
513 pcl::transformPointCloud(output, *input_transformed, transformation_);
514 update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
515 }
516
517 // Check for convergence
518 if (nr_iterations_ >= max_iterations_ || delta < 1) {
519 converged_ = true;
520 PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of "
521 "iterations: %d out of %d. Transformation difference: %f\n",
522 getClassName().c_str(),
523 nr_iterations_,
524 max_iterations_,
525 (transformation_ - previous_transformation_).array().abs().sum());
526 previous_transformation_ = transformation_;
527 }
528 else
529 PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
530 getClassName().c_str());
531 }
532 final_transformation_ = previous_transformation_ * guess;
533
534 PCL_DEBUG("Transformation "
535 "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%"
536 "5f\t%5f\t%5f\t%5f\n",
537 final_transformation_(0, 0),
538 final_transformation_(0, 1),
539 final_transformation_(0, 2),
540 final_transformation_(0, 3),
541 final_transformation_(1, 0),
542 final_transformation_(1, 1),
543 final_transformation_(1, 2),
544 final_transformation_(1, 3),
545 final_transformation_(2, 0),
546 final_transformation_(2, 1),
547 final_transformation_(2, 2),
548 final_transformation_(2, 3),
549 final_transformation_(3, 0),
550 final_transformation_(3, 1),
551 final_transformation_(3, 2),
552 final_transformation_(3, 3));
553
554 // Transform the point cloud
555 pcl::transformPointCloud(*input_, output, final_transformation_);
556}
557
558template <typename PointSource, typename PointTarget, typename Scalar>
559void
561 Matrix4& t, const Vector6d& x) const
562{
563 // Z Y X euler angles convention
564 Matrix3 R = (AngleAxis(static_cast<Scalar>(x[5]), Vector3::UnitZ()) *
565 AngleAxis(static_cast<Scalar>(x[4]), Vector3::UnitY()) *
566 AngleAxis(static_cast<Scalar>(x[3]), Vector3::UnitX()))
567 .toRotationMatrix();
568 Matrix4 T = Matrix4::Identity();
569 T.template block<3, 3>(0, 0) = R;
570 T.template block<3, 1>(0, 3) = Vector3(
571 static_cast<Scalar>(x[0]), static_cast<Scalar>(x[1]), static_cast<Scalar>(x[2]));
572 t = T * t;
573}
574
575} // namespace pcl
576
577#endif // PCL_REGISTRATION_IMPL_GICP_HPP_
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
Definition bfgs.h:121
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:188
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition gicp.h:113
void applyState(Matrix4 &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition gicp.hpp:560
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition gicp.h:108
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition gicp.h:95
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 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:131
typename Eigen::AngleAxis< Scalar > AngleAxis
Definition gicp.h:114
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition gicp.hpp:405
typename Eigen::Matrix< Scalar, 3, 3 > Matrix3
Definition gicp.h:111
Eigen::Matrix< double, 6, 1 > Vector6d
Definition gicp.h:110
An exception that is thrown when the number of correspondents is not equal to the minimum required.
Definition exceptions.h:63
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.
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:87
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
void df(const Vector6d &x, Vector6d &df) override
Definition gicp.hpp:295
BFGSSpace::Status checkGradient(const Vector6d &g) override
Definition gicp.hpp:382
void fdf(const Vector6d &x, double &f, Vector6d &df) override
Definition gicp.hpp:338
A point structure representing Euclidean xyz coordinates, and the RGB color.