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