Point Cloud Library (PCL) 1.12.0
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
44namespace pcl {
45
46template <typename PointSource, typename PointTarget>
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
68 max_iterations_ = 35;
69}
70
71template <typename PointSource, typename PointTarget>
72void
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, pcl::Indices(), *target_, pcl::Indices());
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
182template <typename PointSource, typename PointTarget>
183double
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
233template <typename PointSource, typename PointTarget>
234void
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
317template <typename PointSource, typename PointTarget>
318void
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
363template <typename PointSource, typename PointTarget>
364double
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
411template <typename PointSource, typename PointTarget>
412void
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
453template <typename PointSource, typename PointTarget>
454void
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
488template <typename PointSource, typename PointTarget>
489bool
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
533template <typename PointSource, typename PointTarget>
534double
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
646template <typename PointSource, typename PointTarget>
647double
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_
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
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
void computePointDerivatives(const Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
Definition: ndt.hpp:319
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
Definition: ndt.h:238
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: ndt.h:66
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
double outlier_ratio_
The ratio of outliers of points w.r.t.
Definition: ndt.h:480
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution,...
Definition: ndt.h:484
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
float resolution_
The side length of voxels.
Definition: ndt.h:473
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
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
void computeAngleDerivatives(const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Precompute anglular components of derivatives.
Definition: ndt.hpp:235
void computeHessian(Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud)
Compute hessian of probability function w.r.t.
Definition: ndt.hpp:413
NormalDistributionsTransform()
Constructor.
Definition: ndt.hpp:47
std::string reg_name_
The registration method name.
Definition: registration.h:558
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition: registration.h:573
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:595
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