Point Cloud Library (PCL)  1.11.1-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/boost.h>
45 #include <pcl/registration/exceptions.h>
46 
47 namespace pcl {
48 
49 template <typename PointSource, typename PointTarget>
50 template <typename PointT>
51 void
52 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(
53  typename pcl::PointCloud<PointT>::ConstPtr cloud,
54  const typename pcl::search::KdTree<PointT>::Ptr kdtree,
55  MatricesVector& cloud_covariances)
56 {
57  if (k_correspondences_ > int(cloud->size())) {
58  PCL_ERROR("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or "
59  "points in cloud (%lu) is less than k_correspondences_ (%lu)!\n",
60  cloud->size(),
61  k_correspondences_);
62  return;
63  }
64 
65  Eigen::Vector3d mean;
66  std::vector<int> nn_indecies;
67  nn_indecies.reserve(k_correspondences_);
68  std::vector<float> nn_dist_sq;
69  nn_dist_sq.reserve(k_correspondences_);
70 
71  // We should never get there but who knows
72  if (cloud_covariances.size() < cloud->size())
73  cloud_covariances.resize(cloud->size());
74 
75  MatricesVector::iterator matrices_iterator = cloud_covariances.begin();
76  for (auto points_iterator = cloud->begin(); points_iterator != cloud->end();
77  ++points_iterator, ++matrices_iterator) {
78  const PointT& query_point = *points_iterator;
79  Eigen::Matrix3d& cov = *matrices_iterator;
80  // Zero out the cov and mean
81  cov.setZero();
82  mean.setZero();
83 
84  // Search for the K nearest neighbours
85  kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
86 
87  // Find the covariance matrix
88  for (int j = 0; j < k_correspondences_; j++) {
89  const PointT& pt = (*cloud)[nn_indecies[j]];
90 
91  mean[0] += pt.x;
92  mean[1] += pt.y;
93  mean[2] += pt.z;
94 
95  cov(0, 0) += pt.x * pt.x;
96 
97  cov(1, 0) += pt.y * pt.x;
98  cov(1, 1) += pt.y * pt.y;
99 
100  cov(2, 0) += pt.z * pt.x;
101  cov(2, 1) += pt.z * pt.y;
102  cov(2, 2) += pt.z * pt.z;
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 
130 template <typename PointSource, typename PointTarget>
131 void
132 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(
133  const Vector6d& x, const Eigen::Matrix3d& R, 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, R);
182  g[4] = matricesInnerProd(dR_dTheta, R);
183  g[5] = matricesInnerProd(dR_dPsi, R);
184 }
185 
186 template <typename PointSource, typename PointTarget>
187 void
188 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
189  estimateRigidTransformationBFGS(const PointCloudSource& cloud_src,
190  const std::vector<int>& indices_src,
191  const PointCloudTarget& cloud_tgt,
192  const std::vector<int>& indices_tgt,
193  Eigen::Matrix4f& transformation_matrix)
194 {
195  if (indices_src.size() < 4) // need at least 4 samples
196  {
197  PCL_THROW_EXCEPTION(
198  NotEnoughPointsException,
199  "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need "
200  "at least 4 points to estimate a transform! Source and target have "
201  << indices_src.size() << " points!");
202  return;
203  }
204  // Set the initial solution
205  Vector6d x = Vector6d::Zero();
206  // translation part
207  x[0] = transformation_matrix(0, 3);
208  x[1] = transformation_matrix(1, 3);
209  x[2] = transformation_matrix(2, 3);
210  // rotation part (Z Y X euler angles convention)
211  // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
212  x[3] = std::atan2(transformation_matrix(2, 1), transformation_matrix(2, 2));
213  x[4] = asin(-transformation_matrix(2, 0));
214  x[5] = std::atan2(transformation_matrix(1, 0), transformation_matrix(0, 0));
215 
216  // Set temporary pointers
217  tmp_src_ = &cloud_src;
218  tmp_tgt_ = &cloud_tgt;
219  tmp_idx_src_ = &indices_src;
220  tmp_idx_tgt_ = &indices_tgt;
221 
222  // Optimize using forward-difference approximation LM
223  OptimizationFunctorWithIndices functor(this);
224  BFGS<OptimizationFunctorWithIndices> bfgs(functor);
225  bfgs.parameters.sigma = 0.01;
226  bfgs.parameters.rho = 0.01;
227  bfgs.parameters.tau1 = 9;
228  bfgs.parameters.tau2 = 0.05;
229  bfgs.parameters.tau3 = 0.5;
230  bfgs.parameters.order = 3;
231 
232  int inner_iterations_ = 0;
233  int result = bfgs.minimizeInit(x);
234  result = BFGSSpace::Running;
235  do {
236  inner_iterations_++;
237  result = bfgs.minimizeOneStep(x);
238  if (result) {
239  break;
240  }
241  result = bfgs.testGradient();
242  } while (result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
243  if (result == BFGSSpace::NoProgress || result == BFGSSpace::Success ||
244  inner_iterations_ == max_inner_iterations_) {
245  PCL_DEBUG("[pcl::registration::TransformationEstimationBFGS::"
246  "estimateRigidTransformation]");
247  PCL_DEBUG("BFGS solver finished with exit code %i \n", result);
248  transformation_matrix.setIdentity();
249  applyState(transformation_matrix, x);
250  }
251  else
252  PCL_THROW_EXCEPTION(
253  SolverDidntConvergeException,
254  "[pcl::" << getClassName()
255  << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS "
256  "solver didn't converge!");
257 }
258 
259 template <typename PointSource, typename PointTarget>
260 inline double
261 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
262  OptimizationFunctorWithIndices::operator()(const Vector6d& x)
263 {
264  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
265  gicp_->applyState(transformation_matrix, x);
266  double f = 0;
267  int m = static_cast<int>(gicp_->tmp_idx_src_->size());
268  for (int i = 0; i < m; ++i) {
269  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
270  Vector4fMapConst p_src =
271  (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
272  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
273  Vector4fMapConst p_tgt =
274  (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
275  Eigen::Vector4f pp(transformation_matrix * p_src);
276  // Estimate the distance (cost function)
277  // The last coordinate is still guaranteed to be set to 1.0
278  Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
279  Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
280  // increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone
281  // 1/num_matches after the loop closes)
282  f += double(res.transpose() * temp);
283  }
284  return f / m;
285 }
286 
287 template <typename PointSource, typename PointTarget>
288 inline void
289 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
290  OptimizationFunctorWithIndices::df(const Vector6d& x, Vector6d& g)
291 {
292  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
293  gicp_->applyState(transformation_matrix, x);
294  // Zero out g
295  g.setZero();
296  // Eigen::Vector3d g_t = g.head<3> ();
297  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
298  int m = static_cast<int>(gicp_->tmp_idx_src_->size());
299  for (int i = 0; i < m; ++i) {
300  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
301  Vector4fMapConst p_src =
302  (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
303  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
304  Vector4fMapConst p_tgt =
305  (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
306 
307  Eigen::Vector4f pp(transformation_matrix * p_src);
308  // The last coordinate is still guaranteed to be set to 1.0
309  Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
310  // temp = M*res
311  Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
312  // Increment translation gradient
313  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop
314  // closes)
315  g.head<3>() += temp;
316  // Increment rotation gradient
317  pp = gicp_->base_transformation_ * p_src;
318  Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
319  R += p_src3 * temp.transpose();
320  }
321  g.head<3>() *= 2.0 / m;
322  R *= 2.0 / m;
323  gicp_->computeRDerivative(x, R, g);
324 }
325 
326 template <typename PointSource, typename PointTarget>
327 inline void
328 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
329  OptimizationFunctorWithIndices::fdf(const Vector6d& x, double& f, Vector6d& g)
330 {
331  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
332  gicp_->applyState(transformation_matrix, x);
333  f = 0;
334  g.setZero();
335  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
336  const int m = static_cast<int>(gicp_->tmp_idx_src_->size());
337  for (int i = 0; i < m; ++i) {
338  // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
339  Vector4fMapConst p_src =
340  (*gicp_->tmp_src_)[(*gicp_->tmp_idx_src_)[i]].getVector4fMap();
341  // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
342  Vector4fMapConst p_tgt =
343  (*gicp_->tmp_tgt_)[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap();
344  Eigen::Vector4f pp(transformation_matrix * p_src);
345  // The last coordinate is still guaranteed to be set to 1.0
346  Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
347  // temp = M*res
348  Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
349  // Increment total error
350  f += double(res.transpose() * temp);
351  // Increment translation gradient
352  // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop
353  // closes)
354  g.head<3>() += temp;
355  pp = gicp_->base_transformation_ * p_src;
356  Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
357  // Increment rotation gradient
358  R += p_src3 * temp.transpose();
359  }
360  f /= double(m);
361  g.head<3>() *= double(2.0 / m);
362  R *= 2.0 / m;
363  gicp_->computeRDerivative(x, R, g);
364 }
365 
366 template <typename PointSource, typename PointTarget>
367 inline BFGSSpace::Status
368 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
369  OptimizationFunctorWithIndices::checkGradient(const Vector6d& g)
370 {
371  auto translation_epsilon = gicp_->translation_gradient_tolerance_;
372  auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
373 
374  if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
375  return BFGSSpace::NegativeGradientEpsilon;
376 
377  // express translation gradient as norm of translation parameters
378  auto translation_grad = g.head<3>().norm();
379 
380  // express rotation gradient as a norm of rotation parameters
381  auto rotation_grad = g.tail<3>().norm();
382 
383  if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
384  return BFGSSpace::Success;
385 
386  return BFGSSpace::Running;
387 }
388 
389 template <typename PointSource, typename PointTarget>
390 inline void
391 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation(
392  PointCloudSource& output, const Eigen::Matrix4f& guess)
393 {
394  pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal();
395  // Difference between consecutive transforms
396  double delta = 0;
397  // Get the size of the target
398  const std::size_t N = indices_->size();
399  // Set the mahalanobis matrices to identity
400  mahalanobis_.resize(N, Eigen::Matrix3d::Identity());
401  // Compute target cloud covariance matrices
402  if ((!target_covariances_) || (target_covariances_->empty())) {
403  target_covariances_.reset(new MatricesVector);
404  computeCovariances<PointTarget>(target_, tree_, *target_covariances_);
405  }
406  // Compute input cloud covariance matrices
407  if ((!input_covariances_) || (input_covariances_->empty())) {
408  input_covariances_.reset(new MatricesVector);
409  computeCovariances<PointSource>(input_, tree_reciprocal_, *input_covariances_);
410  }
411 
412  base_transformation_ = Eigen::Matrix4f::Identity();
413  nr_iterations_ = 0;
414  converged_ = false;
415  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
416  std::vector<int> nn_indices(1);
417  std::vector<float> nn_dists(1);
418 
419  pcl::transformPointCloud(output, output, guess);
420 
421  while (!converged_) {
422  std::size_t cnt = 0;
423  std::vector<int> source_indices(indices_->size());
424  std::vector<int> target_indices(indices_->size());
425 
426  // guess corresponds to base_t and transformation_ to t
427  Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero();
428  for (std::size_t i = 0; i < 4; i++)
429  for (std::size_t j = 0; j < 4; j++)
430  for (std::size_t k = 0; k < 4; k++)
431  transform_R(i, j) += double(transformation_(i, k)) * double(guess(k, j));
432 
433  Eigen::Matrix3d R = transform_R.topLeftCorner<3, 3>();
434 
435  for (std::size_t i = 0; i < N; i++) {
436  PointSource query = output[i];
437  query.getVector4fMap() = transformation_ * query.getVector4fMap();
438 
439  if (!searchForNeighbors(query, nn_indices, nn_dists)) {
440  PCL_ERROR("[pcl::%s::computeTransformation] Unable to find a nearest neighbor "
441  "in the target dataset for point %d in the source!\n",
442  getClassName().c_str(),
443  (*indices_)[i]);
444  return;
445  }
446 
447  // Check if the distance to the nearest neighbor is smaller than the user imposed
448  // threshold
449  if (nn_dists[0] < dist_threshold) {
450  Eigen::Matrix3d& C1 = (*input_covariances_)[i];
451  Eigen::Matrix3d& C2 = (*target_covariances_)[nn_indices[0]];
452  Eigen::Matrix3d& M = mahalanobis_[i];
453  // M = R*C1
454  M = R * C1;
455  // temp = M*R' + C2 = R*C1*R' + C2
456  Eigen::Matrix3d temp = M * R.transpose();
457  temp += C2;
458  // M = temp^-1
459  M = temp.inverse();
460  source_indices[cnt] = static_cast<int>(i);
461  target_indices[cnt] = nn_indices[0];
462  cnt++;
463  }
464  }
465  // Resize to the actual number of valid correspondences
466  source_indices.resize(cnt);
467  target_indices.resize(cnt);
468  /* optimize transformation using the current assignment and Mahalanobis metrics*/
469  previous_transformation_ = transformation_;
470  // optimization right here
471  try {
472  rigid_transformation_estimation_(
473  output, source_indices, *target_, target_indices, transformation_);
474  /* compute the delta from this iteration */
475  delta = 0.;
476  for (int k = 0; k < 4; k++) {
477  for (int l = 0; l < 4; l++) {
478  double ratio = 1;
479  if (k < 3 && l < 3) // rotation part of the transform
480  ratio = 1. / rotation_epsilon_;
481  else
482  ratio = 1. / transformation_epsilon_;
483  double c_delta =
484  ratio * std::abs(previous_transformation_(k, l) - transformation_(k, l));
485  if (c_delta > delta)
486  delta = c_delta;
487  }
488  }
489  } catch (PCLException& e) {
490  PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
491  getClassName().c_str(),
492  e.what());
493  break;
494  }
495  nr_iterations_++;
496  // Check for convergence
497  if (nr_iterations_ >= max_iterations_ || delta < 1) {
498  converged_ = true;
499  PCL_DEBUG("[pcl::%s::computeTransformation] Convergence reached. Number of "
500  "iterations: %d out of %d. Transformation difference: %f\n",
501  getClassName().c_str(),
502  nr_iterations_,
503  max_iterations_,
504  (transformation_ - previous_transformation_).array().abs().sum());
505  previous_transformation_ = transformation_;
506  }
507  else
508  PCL_DEBUG("[pcl::%s::computeTransformation] Convergence failed\n",
509  getClassName().c_str());
510  }
511  final_transformation_ = previous_transformation_ * guess;
512 
513  // Transform the point cloud
514  pcl::transformPointCloud(*input_, output, final_transformation_);
515 }
516 
517 template <typename PointSource, typename PointTarget>
518 void
519 GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(
520  Eigen::Matrix4f& t, const Vector6d& x) const
521 {
522  // Z Y X euler angles convention
523  Eigen::Matrix3f R;
524  R = Eigen::AngleAxisf(static_cast<float>(x[5]), Eigen::Vector3f::UnitZ()) *
525  Eigen::AngleAxisf(static_cast<float>(x[4]), Eigen::Vector3f::UnitY()) *
526  Eigen::AngleAxisf(static_cast<float>(x[3]), Eigen::Vector3f::UnitX());
527  t.topLeftCorner<3, 3>().matrix() = R * t.topLeftCorner<3, 3>().matrix();
528  Eigen::Vector4f T(static_cast<float>(x[0]),
529  static_cast<float>(x[1]),
530  static_cast<float>(x[2]),
531  0.0f);
532  t.col(3) += T;
533 }
534 
535 } // namespace pcl
536 
537 #endif // PCL_REGISTRATION_IMPL_GICP_HPP_
pcl::GeneralizedIterativeClosestPoint::computeRDerivative
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const
Computes rotation matrix derivative.
Definition: gicp.hpp:132
pcl
Definition: convolution.h:46
BFGSSpace::NegativeGradientEpsilon
@ NegativeGradientEpsilon
Definition: bfgs.h:71
pcl::search::KdTree::nearestKSearch
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
pcl::GeneralizedIterativeClosestPoint::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: gicp.h:103
pcl::NotEnoughPointsException
An exception that is thrown when the number of correspondents is not equal to the minimum required.
Definition: exceptions.h:63
pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const std::vector< int > &indices_src, const PointCloudTarget &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::Matrix4f &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
Definition: gicp.hpp:189
pcl::GeneralizedIterativeClosestPoint::applyState
void applyState(Eigen::Matrix4f &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition: gicp.hpp:519
pcl::PointCloud::begin
iterator begin() noexcept
Definition: point_cloud.h:423
BFGSSpace::NoProgress
@ NoProgress
Definition: bfgs.h:75
BFGS::minimizeInit
BFGSSpace::Status minimizeInit(FVectorType &x)
Definition: bfgs.h:363
pcl::GeneralizedIterativeClosestPoint::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition: gicp.hpp:391
pcl::Registration< PointSource, PointTarget, float >::initComputeReciprocal
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
Definition: registration.hpp:105
pcl::PointCloud< PointSource >
BFGS::minimizeOneStep
BFGSSpace::Status minimizeOneStep(FVectorType &x)
Definition: bfgs.h:395
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
BFGS::testGradient
BFGSSpace::Status testGradient()
Definition: bfgs.h:478
pcl::transformPointCloud
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
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices
optimization functor structure
Definition: gicp.h:421
BFGS
BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving unconstrained nonlinear op...
Definition: bfgs.h:121
BFGSSpace::Running
@ Running
Definition: bfgs.h:73
pcl::SolverDidntConvergeException
An exception that is thrown when the non linear solver didn't converge.
Definition: exceptions.h:49
pcl::PointCloud::end
iterator end() noexcept
Definition: point_cloud.h:424
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator()
double operator()(const Vector6d &x) override
Definition: gicp.hpp:262
pcl::GeneralizedIterativeClosestPoint::computeCovariances
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:52
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:63
BFGSSpace::Success
@ Success
Definition: bfgs.h:74
BFGSSpace::Status
Status
Definition: bfgs.h:70
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::GeneralizedIterativeClosestPoint::MatricesVector
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition: gicp.h:92
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df
void df(const Vector6d &x, Vector6d &df) override
Definition: gicp.hpp:290
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::checkGradient
BFGSSpace::Status checkGradient(const Vector6d &g) override
Definition: gicp.hpp:369
pcl::Vector4fMapConst
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
Definition: point_types.hpp:183
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf
void fdf(const Vector6d &x, double &f, Vector6d &df) override
Definition: gicp.hpp:329
BFGS::parameters
Parameters parameters
Definition: bfgs.h:171