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