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