Point Cloud Library (PCL)  1.15.0-dev
ndt.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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_NDT_IMPL_H_
42 #define PCL_REGISTRATION_NDT_IMPL_H_
43 
44 namespace pcl {
45 
46 template <typename PointSource, typename PointTarget, typename Scalar>
49 : target_cells_()
50 {
51  reg_name_ = "NormalDistributionsTransform";
52 
53  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
54  const double gauss_c1 = 10.0 * (1 - outlier_ratio_);
55  const double gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
56  const double gauss_d3 = -std::log(gauss_c2);
57  gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
58  gauss_d2_ =
59  -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
60  gauss_d1_);
61 
63  max_iterations_ = 35;
64 }
65 
66 template <typename PointSource, typename PointTarget, typename Scalar>
67 void
69  PointCloudSource& output, const Matrix4& guess)
70 {
71  nr_iterations_ = 0;
72  converged_ = false;
73  if (target_cells_.getCentroids()->empty()) {
74  PCL_ERROR("[%s::computeTransformation] Voxel grid is not searchable!\n",
75  getClassName().c_str());
76  return;
77  }
78 
79  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
80  const double gauss_c1 = 10 * (1 - outlier_ratio_);
81  const double gauss_c2 = outlier_ratio_ / pow(resolution_, 3);
82  const double gauss_d3 = -std::log(gauss_c2);
83  gauss_d1_ = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
84  gauss_d2_ =
85  -2 * std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) /
86  gauss_d1_);
87 
88  if (guess != Matrix4::Identity()) {
89  // Initialise final transformation to the guessed one
90  final_transformation_ = guess;
91  // Apply guessed transformation prior to search for neighbours
92  transformPointCloud(output, output, guess);
93  }
94 
95  // Initialize Point Gradient and Hessian
96  point_jacobian_.setZero();
97  point_jacobian_.block<3, 3>(0, 0).setIdentity();
98  point_hessian_.setZero();
99 
100  Eigen::Transform<Scalar, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
101  eig_transformation.matrix() = final_transformation_;
102 
103  // Convert initial guess matrix to 6 element transformation vector
104  Eigen::Matrix<double, 6, 1> transform, score_gradient;
105  Vector3 init_translation = eig_transformation.translation();
106  Vector3 init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2);
107  transform << init_translation.template cast<double>(),
108  init_rotation.template cast<double>();
109 
110  Eigen::Matrix<double, 6, 6> hessian;
111 
112  // Calculate derivates of initial transform vector, subsequent derivative calculations
113  // are done in the step length determination.
114  double score = computeDerivatives(score_gradient, hessian, output, transform);
115 
116  while (!converged_) {
117  // Store previous transformation
118  previous_transformation_ = transformation_;
119 
120  // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson
121  // 2009]
122  Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6>> sv(
123  hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
124 #if EIGEN_VERSION_AT_LEAST(3, 4, 0)
125  if (sv.info() != Eigen::ComputationInfo::Success) {
126  trans_likelihood_ = score / static_cast<double>(input_->size());
127  converged_ = 0;
128  PCL_ERROR("[%s::computeTransformation] JacobiSVD on hessian failed!\n",
129  getClassName().c_str());
130  return;
131  }
132 #endif
133  // Negative for maximization as opposed to minimization
134  Eigen::Matrix<double, 6, 1> delta = sv.solve(-score_gradient);
135 
136  // Calculate step length with guaranteed sufficient decrease [More, Thuente 1994]
137  double delta_norm = delta.norm();
138 
139  if (delta_norm == 0 || std::isnan(delta_norm)) {
140  trans_likelihood_ = score / static_cast<double>(input_->size());
141  converged_ = delta_norm == 0;
142  return;
143  }
144 
145  delta /= delta_norm;
146  delta_norm = computeStepLengthMT(transform,
147  delta,
148  delta_norm,
149  step_size_,
150  transformation_epsilon_ / 2,
151  score,
152  score_gradient,
153  hessian,
154  output);
155  delta *= delta_norm;
156 
157  // Convert delta into matrix form
158  convertTransform(delta, transformation_);
159 
160  transform += delta;
161 
162  // Update Visualizer (untested)
163  if (update_visualizer_)
164  update_visualizer_(output, pcl::Indices(), *target_, pcl::Indices());
165 
166  const double cos_angle =
167  0.5 * (transformation_.template block<3, 3>(0, 0).trace() - 1);
168  const double translation_sqr =
169  transformation_.template block<3, 1>(0, 3).squaredNorm();
170 
171  nr_iterations_++;
172 
173  if (nr_iterations_ >= max_iterations_ ||
174  ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
175  (transformation_rotation_epsilon_ > 0 &&
176  cos_angle >= transformation_rotation_epsilon_)) ||
177  ((transformation_epsilon_ <= 0) &&
178  (transformation_rotation_epsilon_ > 0 &&
179  cos_angle >= transformation_rotation_epsilon_)) ||
180  ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
181  (transformation_rotation_epsilon_ <= 0))) {
182  converged_ = true;
183  }
184  }
185 
186  // Store transformation likelihood. The relative differences within each scan
187  // registration are accurate but the normalization constants need to be modified for
188  // it to be globally accurate
189  trans_likelihood_ = score / static_cast<double>(input_->size());
190 }
191 
192 template <typename PointSource, typename PointTarget, typename Scalar>
193 double
195  Eigen::Matrix<double, 6, 1>& score_gradient,
196  Eigen::Matrix<double, 6, 6>& hessian,
197  const PointCloudSource& trans_cloud,
198  const Eigen::Matrix<double, 6, 1>& transform,
199  bool compute_hessian)
200 {
201  score_gradient.setZero();
202  hessian.setZero();
203  double score = 0;
204 
205  // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
206  computeAngleDerivatives(transform);
207 
208  // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
209  for (std::size_t idx = 0; idx < input_->size(); idx++) {
210  // Transformed Point
211  const auto& x_trans_pt = trans_cloud[idx];
212 
213  // Find neighbors (Radius search has been experimentally faster than direct neighbor
214  // checking.
215  std::vector<TargetGridLeafConstPtr> neighborhood;
216  std::vector<float> distances;
217  target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
218 
219  for (const auto& cell : neighborhood) {
220  // Original Point
221  const auto& x_pt = (*input_)[idx];
222  const Eigen::Vector3d x = x_pt.getVector3fMap().template cast<double>();
223 
224  // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
225  const Eigen::Vector3d x_trans =
226  x_trans_pt.getVector3fMap().template cast<double>() - cell->getMean();
227  // Inverse Covariance of Occupied Voxel
228  // Uses precomputed covariance for speed.
229  const Eigen::Matrix3d c_inv = cell->getInverseCov();
230 
231  // Compute derivative of transform function w.r.t. transform vector, J_E and H_E
232  // in Equations 6.18 and 6.20 [Magnusson 2009]
233  computePointDerivatives(x);
234  // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to
235  // Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
236  score +=
237  updateDerivatives(score_gradient, hessian, x_trans, c_inv, compute_hessian);
238  }
239  }
240  return score;
241 }
242 
243 template <typename PointSource, typename PointTarget, typename Scalar>
244 void
246  const Eigen::Matrix<double, 6, 1>& transform, bool compute_hessian)
247 {
248  // Simplified math for near 0 angles
249  const auto calculate_cos_sin = [](double angle, double& c, double& s) {
250  if (std::abs(angle) < 10e-5) {
251  c = 1.0;
252  s = 0.0;
253  }
254  else {
255  c = std::cos(angle);
256  s = std::sin(angle);
257  }
258  };
259 
260  double cx, cy, cz, sx, sy, sz;
261  calculate_cos_sin(transform(3), cx, sx);
262  calculate_cos_sin(transform(4), cy, sy);
263  calculate_cos_sin(transform(5), cz, sz);
264 
265  // Precomputed angular gradient components. Letters correspond to Equation 6.19
266  // [Magnusson 2009]
267  angular_jacobian_.setZero();
268  angular_jacobian_.row(0).noalias() = Eigen::Vector4d(
269  (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 1.0); // a
270  angular_jacobian_.row(1).noalias() = Eigen::Vector4d(
271  (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 1.0); // b
272  angular_jacobian_.row(2).noalias() =
273  Eigen::Vector4d((-sy * cz), sy * sz, cy, 1.0); // c
274  angular_jacobian_.row(3).noalias() =
275  Eigen::Vector4d(sx * cy * cz, (-sx * cy * sz), sx * sy, 1.0); // d
276  angular_jacobian_.row(4).noalias() =
277  Eigen::Vector4d((-cx * cy * cz), cx * cy * sz, (-cx * sy), 1.0); // e
278  angular_jacobian_.row(5).noalias() =
279  Eigen::Vector4d((-cy * sz), (-cy * cz), 0, 1.0); // f
280  angular_jacobian_.row(6).noalias() =
281  Eigen::Vector4d((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 1.0); // g
282  angular_jacobian_.row(7).noalias() =
283  Eigen::Vector4d((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 1.0); // h
284 
285  if (compute_hessian) {
286  // Precomputed angular hessian components. Letters correspond to Equation 6.21 and
287  // numbers correspond to row index [Magnusson 2009]
288  angular_hessian_.setZero();
289  angular_hessian_.row(0).noalias() = Eigen::Vector4d(
290  (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f); // a2
291  angular_hessian_.row(1).noalias() = Eigen::Vector4d(
292  (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f); // a3
293 
294  angular_hessian_.row(2).noalias() =
295  Eigen::Vector4d((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f); // b2
296  angular_hessian_.row(3).noalias() =
297  Eigen::Vector4d((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f); // b3
298 
299  // The sign of 'sx * sz' in c2 is incorrect in the thesis, and is fixed here.
300  angular_hessian_.row(4).noalias() = Eigen::Vector4d(
301  (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f); // c2
302  angular_hessian_.row(5).noalias() = Eigen::Vector4d(
303  (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f); // c3
304 
305  angular_hessian_.row(6).noalias() =
306  Eigen::Vector4d((-cy * cz), (cy * sz), (-sy), 0.0f); // d1
307  angular_hessian_.row(7).noalias() =
308  Eigen::Vector4d((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f); // d2
309  angular_hessian_.row(8).noalias() =
310  Eigen::Vector4d((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f); // d3
311 
312  angular_hessian_.row(9).noalias() =
313  Eigen::Vector4d((sy * sz), (sy * cz), 0, 0.0f); // e1
314  angular_hessian_.row(10).noalias() =
315  Eigen::Vector4d((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f); // e2
316  angular_hessian_.row(11).noalias() =
317  Eigen::Vector4d((cx * cy * sz), (cx * cy * cz), 0, 0.0f); // e3
318 
319  angular_hessian_.row(12).noalias() =
320  Eigen::Vector4d((-cy * cz), (cy * sz), 0, 0.0f); // f1
321  angular_hessian_.row(13).noalias() = Eigen::Vector4d(
322  (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f); // f2
323  angular_hessian_.row(14).noalias() = Eigen::Vector4d(
324  (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f); // f3
325  }
326 }
327 
328 template <typename PointSource, typename PointTarget, typename Scalar>
329 void
331  const Eigen::Vector3d& x, bool compute_hessian)
332 {
333  // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector.
334  // Derivative w.r.t. ith element of transform vector corresponds to column i,
335  // Equation 6.18 and 6.19 [Magnusson 2009]
336  Eigen::Matrix<double, 8, 1> point_angular_jacobian =
337  angular_jacobian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
338  point_jacobian_(1, 3) = point_angular_jacobian[0];
339  point_jacobian_(2, 3) = point_angular_jacobian[1];
340  point_jacobian_(0, 4) = point_angular_jacobian[2];
341  point_jacobian_(1, 4) = point_angular_jacobian[3];
342  point_jacobian_(2, 4) = point_angular_jacobian[4];
343  point_jacobian_(0, 5) = point_angular_jacobian[5];
344  point_jacobian_(1, 5) = point_angular_jacobian[6];
345  point_jacobian_(2, 5) = point_angular_jacobian[7];
346 
347  if (compute_hessian) {
348  Eigen::Matrix<double, 15, 1> point_angular_hessian =
349  angular_hessian_ * Eigen::Vector4d(x[0], x[1], x[2], 0.0);
350 
351  // Vectors from Equation 6.21 [Magnusson 2009]
352  const Eigen::Vector3d a(0, point_angular_hessian[0], point_angular_hessian[1]);
353  const Eigen::Vector3d b(0, point_angular_hessian[2], point_angular_hessian[3]);
354  const Eigen::Vector3d c(0, point_angular_hessian[4], point_angular_hessian[5]);
355  const Eigen::Vector3d d = point_angular_hessian.block<3, 1>(6, 0);
356  const Eigen::Vector3d e = point_angular_hessian.block<3, 1>(9, 0);
357  const Eigen::Vector3d f = point_angular_hessian.block<3, 1>(12, 0);
358 
359  // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform
360  // vector. Derivative w.r.t. ith and jth elements of transform vector corresponds to
361  // the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
362  point_hessian_.block<3, 1>(9, 3) = a;
363  point_hessian_.block<3, 1>(12, 3) = b;
364  point_hessian_.block<3, 1>(15, 3) = c;
365  point_hessian_.block<3, 1>(9, 4) = b;
366  point_hessian_.block<3, 1>(12, 4) = d;
367  point_hessian_.block<3, 1>(15, 4) = e;
368  point_hessian_.block<3, 1>(9, 5) = c;
369  point_hessian_.block<3, 1>(12, 5) = e;
370  point_hessian_.block<3, 1>(15, 5) = f;
371  }
372 }
373 
374 template <typename PointSource, typename PointTarget, typename Scalar>
375 double
377  Eigen::Matrix<double, 6, 1>& score_gradient,
378  Eigen::Matrix<double, 6, 6>& hessian,
379  const Eigen::Vector3d& x_trans,
380  const Eigen::Matrix3d& c_inv,
381  bool compute_hessian) const
382 {
383  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
384  double e_x_cov_x = std::exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
385  // Calculate likelihood of transformed points existence, Equation 6.9 [Magnusson
386  // 2009]
387  const double score_inc = -gauss_d1_ * e_x_cov_x;
388 
389  e_x_cov_x = gauss_d2_ * e_x_cov_x;
390 
391  // Error checking for invalid values.
392  if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) {
393  return 0;
394  }
395 
396  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
397  e_x_cov_x *= gauss_d1_;
398 
399  for (int i = 0; i < 6; i++) {
400  // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson
401  // 2009]
402  const Eigen::Vector3d cov_dxd_pi = c_inv * point_jacobian_.col(i);
403 
404  // Update gradient, Equation 6.12 [Magnusson 2009]
405  score_gradient(i) += x_trans.dot(cov_dxd_pi) * e_x_cov_x;
406 
407  if (compute_hessian) {
408  for (Eigen::Index j = 0; j < hessian.cols(); j++) {
409  // Update hessian, Equation 6.13 [Magnusson 2009]
410  hessian(i, j) +=
411  e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
412  x_trans.dot(c_inv * point_jacobian_.col(j)) +
413  x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
414  point_jacobian_.col(j).dot(cov_dxd_pi));
415  }
416  }
417  }
418 
419  return score_inc;
420 }
421 
422 template <typename PointSource, typename PointTarget, typename Scalar>
423 void
425  Eigen::Matrix<double, 6, 6>& hessian, const PointCloudSource& trans_cloud)
426 {
427  hessian.setZero();
428 
429  // Precompute Angular Derivatives unnecessary because only used after regular
430  // derivative calculation Update hessian for each point, line 17 in Algorithm 2
431  // [Magnusson 2009]
432  for (std::size_t idx = 0; idx < input_->size(); idx++) {
433  // Transformed Point
434  const auto& x_trans_pt = trans_cloud[idx];
435 
436  // Find neighbors (Radius search has been experimentally faster than direct neighbor
437  // checking.
438  std::vector<TargetGridLeafConstPtr> neighborhood;
439  std::vector<float> distances;
440  target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
441 
442  for (const auto& cell : neighborhood) {
443  // Original Point
444  const auto& x_pt = (*input_)[idx];
445  const Eigen::Vector3d x = x_pt.getVector3fMap().template cast<double>();
446 
447  // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
448  const Eigen::Vector3d x_trans =
449  x_trans_pt.getVector3fMap().template cast<double>() - cell->getMean();
450  // Inverse Covariance of Occupied Voxel
451  // Uses precomputed covariance for speed.
452  const Eigen::Matrix3d c_inv = cell->getInverseCov();
453 
454  // Compute derivative of transform function w.r.t. transform vector, J_E and H_E
455  // in Equations 6.18 and 6.20 [Magnusson 2009]
456  computePointDerivatives(x);
457  // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12
458  // and 6.13, respectively [Magnusson 2009]
459  updateHessian(hessian, x_trans, c_inv);
460  }
461  }
462 }
463 
464 template <typename PointSource, typename PointTarget, typename Scalar>
465 void
467  Eigen::Matrix<double, 6, 6>& hessian,
468  const Eigen::Vector3d& x_trans,
469  const Eigen::Matrix3d& c_inv) const
470 {
471  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
472  double e_x_cov_x =
473  gauss_d2_ * std::exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
474 
475  // Error checking for invalid values.
476  if (e_x_cov_x > 1 || e_x_cov_x < 0 || std::isnan(e_x_cov_x)) {
477  return;
478  }
479 
480  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
481  e_x_cov_x *= gauss_d1_;
482 
483  for (int i = 0; i < 6; i++) {
484  // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson
485  // 2009]
486  const Eigen::Vector3d cov_dxd_pi = c_inv * point_jacobian_.col(i);
487 
488  for (Eigen::Index j = 0; j < hessian.cols(); j++) {
489  // Update hessian, Equation 6.13 [Magnusson 2009]
490  hessian(i, j) +=
491  e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) *
492  x_trans.dot(c_inv * point_jacobian_.col(j)) +
493  x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
494  point_jacobian_.col(j).dot(cov_dxd_pi));
495  }
496  }
497 }
498 
499 template <typename PointSource, typename PointTarget, typename Scalar>
500 bool
502  double& a_l,
503  double& f_l,
504  double& g_l,
505  double& a_u,
506  double& f_u,
507  double& g_u,
508  double a_t,
509  double f_t,
510  double g_t) const
511 {
512  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente
513  // 1994]
514  if (f_t > f_l) {
515  a_u = a_t;
516  f_u = f_t;
517  g_u = g_t;
518  return false;
519  }
520  // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente
521  // 1994]
522  if (g_t * (a_l - a_t) > 0) {
523  a_l = a_t;
524  f_l = f_t;
525  g_l = g_t;
526  return false;
527  }
528  // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente
529  // 1994]
530  if (g_t * (a_l - a_t) < 0) {
531  a_u = a_l;
532  f_u = f_l;
533  g_u = g_l;
534 
535  a_l = a_t;
536  f_l = f_t;
537  g_l = g_t;
538  return false;
539  }
540  // Interval Converged
541  return true;
542 }
543 
544 template <typename PointSource, typename PointTarget, typename Scalar>
545 double
547  double a_l,
548  double f_l,
549  double g_l,
550  double a_u,
551  double f_u,
552  double g_u,
553  double a_t,
554  double f_t,
555  double g_t) const
556 {
557  if (a_t == a_l && a_t == a_u) {
558  return a_t;
559  }
560 
561  // Endpoints condition check [More, Thuente 1994], p.299 - 300
562  enum class EndpointsCondition { Case1, Case2, Case3, Case4 };
563  EndpointsCondition condition;
564 
565  if (a_t == a_l) {
566  condition = EndpointsCondition::Case4;
567  }
568  else if (f_t > f_l) {
569  condition = EndpointsCondition::Case1;
570  }
571  else if (g_t * g_l < 0) {
572  condition = EndpointsCondition::Case2;
573  }
574  else if (std::fabs(g_t) <= std::fabs(g_l)) {
575  condition = EndpointsCondition::Case3;
576  }
577  else {
578  condition = EndpointsCondition::Case4;
579  }
580 
581  switch (condition) {
582  case EndpointsCondition::Case1: {
583  // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
584  // Equation 2.4.52 [Sun, Yuan 2006]
585  const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
586  const double w = std::sqrt(z * z - g_t * g_l);
587  // Equation 2.4.56 [Sun, Yuan 2006]
588  const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
589 
590  // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l
591  // Equation 2.4.2 [Sun, Yuan 2006]
592  const double a_q =
593  a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
594 
595  if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) {
596  return a_c;
597  }
598  return 0.5 * (a_q + a_c);
599  }
600 
601  case EndpointsCondition::Case2: {
602  // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
603  // Equation 2.4.52 [Sun, Yuan 2006]
604  const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
605  const double w = std::sqrt(z * z - g_t * g_l);
606  // Equation 2.4.56 [Sun, Yuan 2006]
607  const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
608 
609  // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t
610  // Equation 2.4.5 [Sun, Yuan 2006]
611  const double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
612 
613  if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) {
614  return a_c;
615  }
616  return a_s;
617  }
618 
619  case EndpointsCondition::Case3: {
620  // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
621  // Equation 2.4.52 [Sun, Yuan 2006]
622  const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
623  const double w = std::sqrt(z * z - g_t * g_l);
624  const double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
625 
626  // Calculate the minimizer of the quadratic that interpolates g_l and g_t
627  // Equation 2.4.5 [Sun, Yuan 2006]
628  const double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
629 
630  double a_t_next;
631 
632  if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) {
633  a_t_next = a_c;
634  }
635  else {
636  a_t_next = a_s;
637  }
638 
639  if (a_t > a_l) {
640  return std::min(a_t + 0.66 * (a_u - a_t), a_t_next);
641  }
642  return std::max(a_t + 0.66 * (a_u - a_t), a_t_next);
643  }
644 
645  default:
646  case EndpointsCondition::Case4: {
647  // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
648  // Equation 2.4.52 [Sun, Yuan 2006]
649  const double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
650  const double w = std::sqrt(z * z - g_t * g_u);
651  // Equation 2.4.56 [Sun, Yuan 2006]
652  return a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w);
653  }
654  }
655 }
656 
657 template <typename PointSource, typename PointTarget, typename Scalar>
658 double
660  const Eigen::Matrix<double, 6, 1>& x,
661  Eigen::Matrix<double, 6, 1>& step_dir,
662  double step_init,
663  double step_max,
664  double step_min,
665  double& score,
666  Eigen::Matrix<double, 6, 1>& score_gradient,
667  Eigen::Matrix<double, 6, 6>& hessian,
668  PointCloudSource& trans_cloud)
669 {
670  // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
671  const double phi_0 = -score;
672  // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
673  double d_phi_0 = -(score_gradient.dot(step_dir));
674 
675  if (d_phi_0 >= 0) {
676  // Not a decent direction
677  if (d_phi_0 == 0) {
678  return 0;
679  }
680  // Reverse step direction and calculate optimal step.
681  d_phi_0 *= -1;
682  step_dir *= -1;
683  }
684 
685  // The Search Algorithm for T(mu) [More, Thuente 1994]
686 
687  constexpr int max_step_iterations = 10;
688  int step_iterations = 0;
689 
690  // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994]
691  constexpr double mu = 1.e-4;
692  // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
693  constexpr double nu = 0.9;
694 
695  // Initial endpoints of Interval I,
696  double a_l = 0, a_u = 0;
697 
698  // Auxiliary function psi is used until I is determined ot be a closed interval,
699  // Equation 2.1 [More, Thuente 1994]
700  double f_l = auxilaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu);
701  double g_l = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
702 
703  double f_u = auxilaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu);
704  double g_u = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu);
705 
706  // Check used to allow More-Thuente step length calculation to be skipped by making
707  // step_min == step_max
708  bool interval_converged = (step_max - step_min) < 0, open_interval = true;
709 
710  double a_t = step_init;
711  a_t = std::min(a_t, step_max);
712  a_t = std::max(a_t, step_min);
713 
714  Eigen::Matrix<double, 6, 1> x_t = x + step_dir * a_t;
715 
716  // Convert x_t into matrix form
717  convertTransform(x_t, final_transformation_);
718 
719  // New transformed point cloud
720  transformPointCloud(*input_, trans_cloud, final_transformation_);
721 
722  // Updates score, gradient and hessian. Hessian calculation is unnecessary but
723  // testing showed that most step calculations use the initial step suggestion and
724  // recalculation the reusable portions of the hessian would entail more computation
725  // time.
726  score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, true);
727 
728  // Calculate phi(alpha_t)
729  double phi_t = -score;
730  // Calculate phi'(alpha_t)
731  double d_phi_t = -(score_gradient.dot(step_dir));
732 
733  // Calculate psi(alpha_t)
734  double psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
735  // Calculate psi'(alpha_t)
736  double d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
737 
738  // Iterate until max number of iterations, interval convergence or a value satisfies
739  // the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More,
740  // Thuente 1994]
741  while (!interval_converged && step_iterations < max_step_iterations &&
742  (psi_t > 0 /*Sufficient Decrease*/ ||
743  d_phi_t > -nu * d_phi_0 /*Curvature Condition*/)) {
744  // Use auxiliary function if interval I is not closed
745  if (open_interval) {
746  a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
747  }
748  else {
749  a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
750  }
751 
752  a_t = std::min(a_t, step_max);
753  a_t = std::max(a_t, step_min);
754 
755  x_t = x + step_dir * a_t;
756 
757  // Convert x_t into matrix form
758  convertTransform(x_t, final_transformation_);
759 
760  // New transformed point cloud
761  // Done on final cloud to prevent wasted computation
762  transformPointCloud(*input_, trans_cloud, final_transformation_);
763 
764  // Updates score, gradient. Values stored to prevent wasted computation.
765  score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false);
766 
767  // Calculate phi(alpha_t+)
768  phi_t = -score;
769  // Calculate phi'(alpha_t+)
770  d_phi_t = -(score_gradient.dot(step_dir));
771 
772  // Calculate psi(alpha_t+)
773  psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
774  // Calculate psi'(alpha_t+)
775  d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);
776 
777  // Check if I is now a closed interval
778  if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
779  open_interval = false;
780 
781  // Converts f_l and g_l from psi to phi
782  f_l += phi_0 - mu * d_phi_0 * a_l;
783  g_l += mu * d_phi_0;
784 
785  // Converts f_u and g_u from psi to phi
786  f_u += phi_0 - mu * d_phi_0 * a_u;
787  g_u += mu * d_phi_0;
788  }
789 
790  if (open_interval) {
791  // Update interval end points using Updating Algorithm [More, Thuente 1994]
792  interval_converged =
793  updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
794  }
795  else {
796  // Update interval end points using Modified Updating Algorithm [More, Thuente
797  // 1994]
798  interval_converged =
799  updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
800  }
801 
802  step_iterations++;
803  }
804 
805  // If inner loop was run then hessian needs to be calculated.
806  // Hessian is unnecessary for step length determination but gradients are required
807  // so derivative and transform data is stored for the next iteration.
808  if (step_iterations) {
809  computeHessian(hessian, trans_cloud);
810  }
811 
812  return a_t;
813 }
814 
815 } // namespace pcl
816 
817 #endif // PCL_REGISTRATION_NDT_IMPL_H_
void computePointDerivatives(const Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
Definition: ndt.hpp:330
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
Definition: ndt.h:315
double updateDerivatives(Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian=true) const
Compute individual point contributions to derivatives of likelihood function w.r.t.
Definition: ndt.hpp:376
double computeDerivatives(Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud, const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Compute derivatives of likelihood function w.r.t.
Definition: ndt.hpp:194
void computeAngleDerivatives(const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Precompute angular components of derivatives.
Definition: ndt.hpp:245
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: ndt.h:70
NormalDistributionsTransform()
Constructor.
Definition: ndt.hpp:48
bool updateIntervalMT(double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t) const
Update interval of possible step lengths for More-Thuente method, in More-Thuente (1994)
Definition: ndt.hpp:501
void computeHessian(Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud)
Compute hessian of likelihood function w.r.t.
Definition: ndt.hpp:424
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: ndt.h:97
float resolution_
The side length of voxels.
Definition: ndt.h:586
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: ndt.h:98
double outlier_ratio_
The ratio of outliers of points w.r.t.
Definition: ndt.h:593
void updateHessian(Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const
Compute individual point contributions to hessian of likelihood function w.r.t.
Definition: ndt.hpp:466
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution,...
Definition: ndt.h:597
double trialValueSelectionMT(double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, double g_t) const
Select new trial value for More-Thuente method.
Definition: ndt.hpp:546
double computeStepLengthMT(const Eigen::Matrix< double, 6, 1 > &transform, Eigen::Matrix< double, 6, 1 > &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, PointCloudSource &trans_cloud)
Compute line search step length and update transform and likelihood derivatives using More-Thuente me...
Definition: ndt.hpp:659
std::string reg_name_
The registration method name.
Definition: registration.h:548
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:563
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:585
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133