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