Point Cloud Library (PCL)  1.13.0-dev
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 
46 namespace pcl {
47 
48 template <typename PointSource, typename PointTarget, typename Scalar>
49 template <typename PointT>
50 void
52  typename pcl::PointCloud<PointT>::ConstPtr cloud,
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;
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 
129 template <typename PointSource, typename PointTarget, typename Scalar>
130 void
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 
185 template <typename PointSource, typename PointTarget, typename Scalar>
186 void
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 
261 template <typename PointSource, typename PointTarget, typename Scalar>
262 inline 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 += static_cast<double>(d.transpose() * Md);
288  }
289  return f / m;
290 }
291 
292 template <typename PointSource, typename PointTarget, typename Scalar>
293 inline 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 
335 template <typename PointSource, typename PointTarget, typename Scalar>
336 inline 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 += static_cast<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 /= static_cast<double>(m);
374  g.head<3>() *= (2.0 / m);
375  dCost_dR_T *= 2.0 / m;
376  gicp_->computeRDerivative(x, dCost_dR_T, g);
377 }
378 
379 template <typename PointSource, typename PointTarget, typename Scalar>
380 inline BFGSSpace::Status
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 
402 template <typename PointSource, typename PointTarget, typename Scalar>
403 inline void
405  computeTransformation(PointCloudSource& output, const Matrix4& guess)
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) += static_cast<double>(transformation_(i, k)) *
445  static_cast<double>(guess(k, j));
446 
447  Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
448 
449  for (std::size_t i = 0; i < N; i++) {
450  PointSource query = output[i];
451  query.getVector4fMap() =
452  transformation_.template cast<float>() * query.getVector4fMap();
453 
454  if (!searchForNeighbors(query, nn_indices, nn_dists)) {
455  PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
456  "in the target dataset for point %d in the source!\n",
457  getClassName().c_str(),
458  (*indices_)[i]);
459  return;
460  }
461 
462  // Check if the distance to the nearest neighbor is smaller than the user imposed
463  // threshold
464  if (nn_dists[0] < dist_threshold) {
465  Eigen::Matrix3d& C1 = (*input_covariances_)[i];
466  Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
467  Eigen::Matrix3d& M = mahalanobis_[i];
468  // M = R*C1
469  M = R * C1;
470  // temp = M*R' + C2 = R*C1*R' + C2
471  Eigen::Matrix3d temp = M * R.transpose();
472  temp += C2;
473  // M = temp^-1
474  M = temp.inverse();
475  source_indices[cnt] = static_cast<int>(i);
476  target_indices[cnt] = nn_indices[0];
477  cnt++;
478  }
479  }
480  // Resize to the actual number of valid correspondences
481  source_indices.resize(cnt);
482  target_indices.resize(cnt);
483  /* optimize transformation using the current assignment and Mahalanobis metrics*/
484  previous_transformation_ = transformation_;
485  // optimization right here
486  try {
487  rigid_transformation_estimation_(
488  output, source_indices, *target_, target_indices, transformation_);
489  /* compute the delta from this iteration */
490  delta = 0.;
491  for (int k = 0; k < 4; k++) {
492  for (int l = 0; l < 4; l++) {
493  double ratio = 1;
494  if (k < 3 && l < 3) // rotation part of the transform
495  ratio = 1. / rotation_epsilon_;
496  else
497  ratio = 1. / transformation_epsilon_;
498  double c_delta =
499  ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
500  if (c_delta > delta)
501  delta = c_delta;
502  }
503  }
504  } catch (PCLException& e) {
505  PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
506  getClassName().c_str(),
507  e.what());
508  break;
509  }
510  nr_iterations_++;
511 
512  if (update_visualizer_ != nullptr) {
513  PointCloudSourcePtr input_transformed(new PointCloudSource);
514  pcl::transformPointCloud(output, *input_transformed, transformation_);
515  update_visualizer_(*input_transformed, source_indices, *target_, target_indices);
516  }
517 
518  // Check for convergence
519  if (nr_iterations_ >= max_iterations_ || delta < 1) {
520  converged_ = true;
521  PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of "
522  "iterations: %d out of %d. Transformation difference: %f\n",
523  getClassName().c_str(),
524  nr_iterations_,
525  max_iterations_,
526  (transformation_ - previous_transformation_).array().abs().sum());
527  previous_transformation_ = transformation_;
528  }
529  else
530  PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
531  getClassName().c_str());
532  }
533  final_transformation_ = previous_transformation_ * guess;
534 
535  PCL_DEBUG("Transformation "
536  "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%"
537  "5f\t%5f\t%5f\t%5f\n",
538  final_transformation_(0, 0),
539  final_transformation_(0, 1),
540  final_transformation_(0, 2),
541  final_transformation_(0, 3),
542  final_transformation_(1, 0),
543  final_transformation_(1, 1),
544  final_transformation_(1, 2),
545  final_transformation_(1, 3),
546  final_transformation_(2, 0),
547  final_transformation_(2, 1),
548  final_transformation_(2, 2),
549  final_transformation_(2, 3),
550  final_transformation_(3, 0),
551  final_transformation_(3, 1),
552  final_transformation_(3, 2),
553  final_transformation_(3, 3));
554 
555  // Transform the point cloud
556  pcl::transformPointCloud(*input_, output, final_transformation_);
557 }
558 
559 template <typename PointSource, typename PointTarget, typename Scalar>
560 void
562  Matrix4& t, const Vector6d& x) const
563 {
564  // Z Y X euler angles convention
565  Matrix3 R = (AngleAxis(static_cast<Scalar>(x[5]), Vector3::UnitZ()) *
566  AngleAxis(static_cast<Scalar>(x[4]), Vector3::UnitY()) *
567  AngleAxis(static_cast<Scalar>(x[3]), Vector3::UnitX()))
568  .toRotationMatrix();
569  Matrix4 T = Matrix4::Identity();
570  T.template block<3, 3>(0, 0) = R;
571  T.template block<3, 1>(0, 3) = Vector3(
572  static_cast<Scalar>(x[0]), static_cast<Scalar>(x[1]), static_cast<Scalar>(x[2]));
573  t = T * t;
574 }
575 
576 } // namespace pcl
577 
578 #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:561
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
Definition: point_cloud.h:430
std::size_t size() const
Definition: point_cloud.h:443
iterator begin() noexcept
Definition: point_cloud.h:429
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
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.
Definition: transforms.hpp:221
Status
Definition: bfgs.h:70
@ 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.