Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
ndt.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, 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#pragma once
42
43#include <pcl/common/utils.h>
44#include <pcl/filters/voxel_grid_covariance.h>
45#include <pcl/registration/registration.h>
46#include <pcl/memory.h>
47#include <pcl/pcl_macros.h>
48
49#include <unsupported/Eigen/NonLinearOptimization>
50
51namespace pcl {
52/** \brief A 3D Normal Distribution Transform registration implementation for
53 * point cloud data.
54 * \note For more information please see <b>Magnusson, M. (2009). The
55 * Three-Dimensional Normal-Distributions Transform — an Efficient Representation
56 * for Registration, Surface Analysis, and Loop Detection. PhD thesis, Orebro
57 * University. Orebro Studies in Technology 36.</b>, <b>More, J., and Thuente,
58 * D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease In ACM
59 * Transactions on Mathematical Software.</b> and Sun, W. and Yuan, Y, (2006)
60 * Optimization Theory and Methods: Nonlinear Programming. 89-100
61 * \note Math refactored by Todor Stoyanov.
62 * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
63 * \ingroup registration
64 */
65template <typename PointSource, typename PointTarget, typename Scalar = float>
67: public Registration<PointSource, PointTarget, Scalar> {
68protected:
71 using PointCloudSourcePtr = typename PointCloudSource::Ptr;
72 using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
73
76 using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
77 using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
78
81
82 /** \brief Typename of searchable voxel grid containing mean and
83 * covariance. */
85 /** \brief Typename of pointer to searchable voxel grid. */
87 /** \brief Typename of const pointer to searchable voxel grid. */
89 /** \brief Typename of const pointer to searchable voxel grid leaf. */
91
92public:
93 using Ptr =
94 shared_ptr<NormalDistributionsTransform<PointSource, PointTarget, Scalar>>;
95 using ConstPtr =
96 shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget, Scalar>>;
97 using Vector3 = typename Eigen::Matrix<Scalar, 3, 1>;
99 using Affine3 = typename Eigen::Transform<Scalar, 3, Eigen::Affine>;
100
101 /** \brief Constructor. Sets \ref outlier_ratio_ to 0.55, \ref step_size_ to
102 * 0.1 and \ref resolution_ to 1.0
103 */
105
106 /** \brief Empty destructor */
107 ~NormalDistributionsTransform() override = default;
108
109 /** \brief Provide a pointer to the input target (e.g., the point cloud that
110 * we want to align the input source to).
111 * \param[in] cloud the input point cloud target
112 */
113 inline void
119
120 /** \brief Set/change the voxel grid resolution.
121 * \param[in] resolution side length of voxels
122 */
123 inline void
124 setResolution(float resolution)
125 {
126 // Prevents unnecessary voxel initiations
127 if (resolution_ != resolution) {
128 resolution_ = resolution;
129 if (input_) {
130 init();
131 }
132 }
133 }
134
135 /** \brief Set the minimum number of points required for a cell to be used (must be 3
136 * or greater for covariance calculation). Calls the function of the underlying
137 * VoxelGridCovariance. This function must be called before `setInputTarget` and
138 * `setResolution`. \param[in] min_points_per_voxel the minimum number of points
139 * required for a voxel to be used
140 */
141 inline void
142 setMinPointPerVoxel(unsigned int min_points_per_voxel)
143 {
144 target_cells_.setMinPointPerVoxel(min_points_per_voxel);
145 }
146
147 /** \brief Get voxel grid resolution.
148 * \return side length of voxels
149 */
150 inline float
152 {
153 return resolution_;
154 }
155
156 /** \brief Get the newton line search maximum step length.
157 * \return maximum step length
158 */
159 inline double
161 {
162 return step_size_;
163 }
164
165 /** \brief Set/change the newton line search maximum step length.
166 * \param[in] step_size maximum step length
167 */
168 inline void
169 setStepSize(double step_size)
170 {
171 step_size_ = step_size;
172 }
173
174 /** \brief Get the point cloud outlier ratio.
175 * \return outlier ratio
176 */
177 inline double
179 {
180 return outlier_ratio_;
181 }
182
183 /** \brief Set/change the point cloud outlier ratio.
184 * \param[in] outlier_ratio outlier ratio
185 */
186 inline void
187 setOulierRatio(double outlier_ratio)
188 {
189 outlier_ratio_ = outlier_ratio;
190 }
191
192 /** \brief Get the registration alignment likelihood.
193 * \return transformation likelihood
194 */
195 inline double
197 {
198 return trans_likelihood_;
199 }
200
201 /** \brief Get the registration alignment probability.
202 * \return transformation probability
203 */
205 16,
206 "The method `getTransformationProbability` has been renamed to "
207 "`getTransformationLikelihood`.")
208 inline double
210 {
211 return trans_likelihood_;
212 }
213
214 /** \brief Get the number of iterations required to calculate alignment.
215 * \return final number of iterations
216 */
217 inline int
219 {
220 return nr_iterations_;
221 }
222
223 /** \brief Convert 6 element transformation vector to affine transformation.
224 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
225 * \param[out] trans affine transform corresponding to given transformation
226 * vector
227 */
228 static void
229 convertTransform(const Eigen::Matrix<double, 6, 1>& x, Affine3& trans)
230 {
231 trans = Eigen::Translation<Scalar, 3>(x.head<3>().cast<Scalar>()) *
232 Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(3)), Vector3::UnitX()) *
233 Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(4)), Vector3::UnitY()) *
234 Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(5)), Vector3::UnitZ());
235 }
236
237 /** \brief Convert 6 element transformation vector to transformation matrix.
238 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
239 * \param[out] trans 4x4 transformation matrix corresponding to given
240 * transformation vector
241 */
242 static void
243 convertTransform(const Eigen::Matrix<double, 6, 1>& x, Matrix4& trans)
244 {
245 Affine3 _affine;
246 convertTransform(x, _affine);
247 trans = _affine.matrix();
248 }
249
250protected:
251 using Registration<PointSource, PointTarget, Scalar>::reg_name_;
252 using Registration<PointSource, PointTarget, Scalar>::getClassName;
253 using Registration<PointSource, PointTarget, Scalar>::input_;
254 using Registration<PointSource, PointTarget, Scalar>::indices_;
255 using Registration<PointSource, PointTarget, Scalar>::target_;
256 using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
257 using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
258 using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
259 using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
260 using Registration<PointSource, PointTarget, Scalar>::transformation_;
261 using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
262 using Registration<PointSource, PointTarget, Scalar>::
264 using Registration<PointSource, PointTarget, Scalar>::converged_;
265 using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
266 using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
267
268 using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
269
270 /** \brief Estimate the transformation and returns the transformed source
271 * (input) as output.
272 * \param[out] output the resultant input transformed point cloud dataset
273 */
274 virtual void
276 {
277 computeTransformation(output, Matrix4::Identity());
278 }
279
280 /** \brief Estimate the transformation and returns the transformed source
281 * (input) as output.
282 * \param[out] output the resultant input transformed point cloud dataset
283 * \param[in] guess the initial gross estimation of the transformation
284 */
285 void
286 computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
287
288 /** \brief Initiate covariance voxel structure. */
289 void inline init()
290 {
293 // Initiate voxel structure.
294 target_cells_.filter(true);
295 }
296
297 /** \brief Compute derivatives of likelihood function w.r.t. the
298 * transformation vector.
299 * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
300 * \param[out] score_gradient the gradient vector of the likelihood function
301 * w.r.t. the transformation vector
302 * \param[out] hessian the hessian matrix of the likelihood function
303 * w.r.t. the transformation vector
304 * \param[in] trans_cloud transformed point cloud
305 * \param[in] transform the current transform vector
306 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
307 * calculation.
308 */
309 double
310 computeDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
311 Eigen::Matrix<double, 6, 6>& hessian,
312 const PointCloudSource& trans_cloud,
313 const Eigen::Matrix<double, 6, 1>& transform,
314 bool compute_hessian = true);
315
316 /** \brief Compute individual point contributions to derivatives of
317 * likelihood function w.r.t. the transformation vector.
318 * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
319 * \param[in,out] score_gradient the gradient vector of the likelihood
320 * function w.r.t. the transformation vector
321 * \param[in,out] hessian the hessian matrix of the likelihood function
322 * w.r.t. the transformation vector
323 * \param[in] x_trans transformed point minus mean of occupied covariance
324 * voxel
325 * \param[in] c_inv covariance of occupied covariance voxel
326 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
327 * calculation.
328 */
329 double
330 updateDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
331 Eigen::Matrix<double, 6, 6>& hessian,
332 const Eigen::Vector3d& x_trans,
333 const Eigen::Matrix3d& c_inv,
334 bool compute_hessian = true) const;
335
336 /** \brief Precompute angular components of derivatives.
337 * \note Equation 6.19 and 6.21 [Magnusson 2009].
338 * \param[in] transform the current transform vector
339 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
340 * calculation.
341 */
342 void
343 computeAngleDerivatives(const Eigen::Matrix<double, 6, 1>& transform,
344 bool compute_hessian = true);
345
346 /** \brief Compute point derivatives.
347 * \note Equation 6.18-21 [Magnusson 2009].
348 * \param[in] x point from the input cloud
349 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
350 * calculation.
351 */
352 void
353 computePointDerivatives(const Eigen::Vector3d& x, bool compute_hessian = true);
354
355 /** \brief Compute hessian of likelihood function w.r.t. the transformation
356 * vector.
357 * \note Equation 6.13 [Magnusson 2009].
358 * \param[out] hessian the hessian matrix of the likelihood function
359 * w.r.t. the transformation vector
360 * \param[in] trans_cloud transformed point cloud
361 */
362 void
363 computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
364 const PointCloudSource& trans_cloud);
365
366 /** \brief Compute hessian of likelihood function w.r.t. the transformation
367 * vector.
368 * \note Equation 6.13 [Magnusson 2009].
369 * \param[out] hessian the hessian matrix of the likelihood function
370 * w.r.t. the transformation vector
371 * \param[in] trans_cloud transformed point cloud
372 * \param[in] transform the current transform vector
373 */
374 PCL_DEPRECATED(1, 15, "Parameter `transform` is not required")
375 void
376 computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
377 const PointCloudSource& trans_cloud,
378 const Eigen::Matrix<double, 6, 1>& transform)
379 {
380 pcl::utils::ignore(transform);
381 computeHessian(hessian, trans_cloud);
382 }
383
384 /** \brief Compute individual point contributions to hessian of likelihood
385 * function w.r.t. the transformation vector.
386 * \note Equation 6.13 [Magnusson 2009].
387 * \param[in,out] hessian the hessian matrix of the likelihood function
388 * w.r.t. the transformation vector
389 * \param[in] x_trans transformed point minus mean of occupied covariance
390 * voxel
391 * \param[in] c_inv covariance of occupied covariance voxel
392 */
393 void
394 updateHessian(Eigen::Matrix<double, 6, 6>& hessian,
395 const Eigen::Vector3d& x_trans,
396 const Eigen::Matrix3d& c_inv) const;
397
398 /** \brief Compute line search step length and update transform and
399 * likelihood derivatives using More-Thuente method.
400 * \note Search Algorithm [More, Thuente 1994]
401 * \param[in] transform initial transformation vector, \f$ x \f$ in Equation
402 * 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson
403 * 2009]
404 * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore,
405 * Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2
406 * [Magnusson 2009]
407 * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in
408 * Moore-Thuente (1994) and the normal of \f$ \delta \vec{p} \f$ in Algorithm
409 * 2 [Magnusson 2009]
410 * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in
411 * Moore-Thuente (1994)
412 * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in
413 * Moore-Thuente (1994)
414 * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in
415 * Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2
416 * [Magnusson 2009]
417 * \param[in,out] score_gradient gradient of score function w.r.t.
418 * transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and
419 * \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
420 * \param[out] hessian hessian of score function w.r.t. transformation vector,
421 * \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in
422 * Algorithm 2 [Magnusson 2009]
423 * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed
424 * by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
425 * \return final step length
426 */
427 double
428 computeStepLengthMT(const Eigen::Matrix<double, 6, 1>& transform,
429 Eigen::Matrix<double, 6, 1>& step_dir,
430 double step_init,
431 double step_max,
432 double step_min,
433 double& score,
434 Eigen::Matrix<double, 6, 1>& score_gradient,
435 Eigen::Matrix<double, 6, 6>& hessian,
436 PointCloudSource& trans_cloud);
437
438 /** \brief Update interval of possible step lengths for More-Thuente method,
439 * \f$ I \f$ in More-Thuente (1994)
440 * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq
441 * 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ and Modified Updating Algorithm
442 * from then on [More, Thuente 1994].
443 * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$
444 * in Moore-Thuente (1994)
445 * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente
446 * (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l)
447 * \f$ for Modified Update Algorithm
448 * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in
449 * Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$
450 * \phi'(\alpha_l) \f$ for Modified Update Algorithm
451 * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$
452 * in Moore-Thuente (1994)
453 * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente
454 * (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u)
455 * \f$ for Modified Update Algorithm
456 * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in
457 * Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$
458 * \phi'(\alpha_u) \f$ for Modified Update Algorithm
459 * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
460 * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994),
461 * \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for
462 * Modified Update Algorithm
463 * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente
464 * (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$
465 * \phi'(\alpha_t) \f$ for Modified Update Algorithm
466 * \return if interval converges
467 */
468 bool
469 updateIntervalMT(double& a_l,
470 double& f_l,
471 double& g_l,
472 double& a_u,
473 double& f_u,
474 double& g_u,
475 double a_t,
476 double f_t,
477 double g_t) const;
478
479 /** \brief Select new trial value for More-Thuente method.
480 * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is
481 * used for \f$ f_k \f$ and \f$ g_k \f$ until some value satisfies the test
482 * \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ then \f$
483 * \phi(\alpha_k) \f$ is used from then on.
484 * \note Interpolation Minimizer equations from Optimization Theory and
485 * Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
486 * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in
487 * Moore-Thuente (1994)
488 * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
489 * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente
490 * (1994)
491 * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in
492 * Moore-Thuente (1994)
493 * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente
494 * (1994)
495 * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente
496 * (1994)
497 * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente
498 * (1994)
499 * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente
500 * (1994)
501 * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in
502 * Moore-Thuente (1994)
503 * \return new trial value
504 */
505 double
506 trialValueSelectionMT(double a_l,
507 double f_l,
508 double g_l,
509 double a_u,
510 double f_u,
511 double g_u,
512 double a_t,
513 double f_t,
514 double g_t) const;
515
516 /** \brief Auxiliary function used to determine endpoints of More-Thuente
517 * interval.
518 * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
519 * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
520 * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in
521 * More-Thuente (1994)
522 * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente
523 * (1994)
524 * \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente
525 * (1994)
526 * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More,
527 * Thuente 1994]
528 * \return sufficient decrease value
529 */
530 inline double
532 double a, double f_a, double f_0, double g_0, double mu = 1.e-4) const
533 {
534 return f_a - f_0 - mu * g_0 * a;
535 }
536
537 /** \brief Auxiliary function derivative used to determine endpoints of
538 * More-Thuente interval.
539 * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente
540 * 1994)
541 * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in
542 * More-Thuente (1994)
543 * \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente
544 * (1994)
545 * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More,
546 * Thuente 1994]
547 * \return sufficient decrease derivative
548 */
549 inline double
550 auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) const
551 {
552 return g_a - mu * g_0;
553 }
554
555 /** \brief The voxel grid generated from target cloud containing point means
556 * and covariances. */
558
559 /** \brief The side length of voxels. */
560 float resolution_{1.0f};
561
562 /** \brief The maximum step length. */
563 double step_size_{0.1};
564
565 /** \brief The ratio of outliers of points w.r.t. a normal distribution,
566 * Equation 6.7 [Magnusson 2009]. */
567 double outlier_ratio_{0.55};
568
569 /** \brief The normalization constants used fit the point distribution to a
570 * normal distribution, Equation 6.8 [Magnusson 2009]. */
571 double gauss_d1_{0.0}, gauss_d2_{0.0};
572
573 /** \brief The likelihood score of the transform applied to the input cloud,
574 * Equation 6.9 and 6.10 [Magnusson 2009]. */
575 union {
577 16,
578 "`trans_probability_` has been renamed to `trans_likelihood_`.")
580 double trans_likelihood_{0.0};
581 };
582
583 /** \brief Precomputed Angular Gradient
584 *
585 * The precomputed angular derivatives for the jacobian of a transformation
586 * vector, Equation 6.19 [Magnusson 2009].
587 */
588 Eigen::Matrix<double, 8, 4> angular_jacobian_;
589
590 /** \brief Precomputed Angular Hessian
591 *
592 * The precomputed angular derivatives for the hessian of a transformation
593 * vector, Equation 6.19 [Magnusson 2009].
594 */
595 Eigen::Matrix<double, 15, 4> angular_hessian_;
596
597 /** \brief The first order derivative of the transformation of a point
598 * w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson
599 * 2009]. */
600 Eigen::Matrix<double, 3, 6> point_jacobian_;
601
602 /** \brief The second order derivative of the transformation of a point
603 * w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson
604 * 2009]. */
605 Eigen::Matrix<double, 18, 6> point_hessian_;
606
607public:
609};
610} // namespace pcl
611
612#include <pcl/registration/impl/ndt.hpp>
A 3D Normal Distribution Transform registration implementation for point cloud data.
Definition ndt.h:67
void computePointDerivatives(const Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
Definition ndt.hpp:321
float getResolution() const
Get voxel grid resolution.
Definition ndt.h:151
shared_ptr< const NormalDistributionsTransform< PointSource, PointTarget, Scalar > > ConstPtr
Definition ndt.h:96
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition ndt.h:76
PointIndices::ConstPtr PointIndicesConstPtr
Definition ndt.h:80
double getStepSize() const
Get the newton line search maximum step length.
Definition ndt.h:160
Eigen::Matrix< double, 18, 6 > point_hessian_
The second order derivative of the transformation of a point w.r.t.
Definition ndt.h:605
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
Definition ndt.h:275
typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr
Typename of const pointer to searchable voxel grid leaf.
Definition ndt.h:90
int getFinalNumIteration() const
Get the number of iterations required to calculate alignment.
Definition ndt.h:218
typename Registration< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition ndt.h:75
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:367
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:185
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition ndt.h:77
void computeAngleDerivatives(const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Precompute angular components of derivatives.
Definition ndt.hpp:236
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition ndt.h:72
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition ndt.h:70
NormalDistributionsTransform()
Constructor.
Definition ndt.hpp:48
bool updateIntervalMT(double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t) const
Update interval of possible step lengths for More-Thuente method, in More-Thuente (1994)
Definition ndt.hpp:492
void computeHessian(Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud)
Compute hessian of likelihood function w.r.t.
Definition ndt.hpp:415
void init()
Initiate covariance voxel structure.
Definition ndt.h:289
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition ndt.h:97
double auxilaryFunction_dPsiMT(double g_a, double g_0, double mu=1.e-4) const
Auxiliary function derivative used to determine endpoints of More-Thuente interval.
Definition ndt.h:550
float resolution_
The side length of voxels.
Definition ndt.h:560
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition ndt.h:98
~NormalDistributionsTransform() override=default
Empty destructor.
double outlier_ratio_
The ratio of outliers of points w.r.t.
Definition ndt.h:567
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:457
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition ndt.h:114
double getTransformationProbability() const
Get the registration alignment probability.
Definition ndt.h:209
TargetGrid target_cells_
The voxel grid generated from target cloud containing point means and covariances.
Definition ndt.h:557
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ndt.h:71
Eigen::Matrix< double, 8, 4 > angular_jacobian_
Precomputed Angular Gradient.
Definition ndt.h:588
void setMinPointPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance ...
Definition ndt.h:142
void setOulierRatio(double outlier_ratio)
Set/change the point cloud outlier ratio.
Definition ndt.h:187
double getOulierRatio() const
Get the point cloud outlier ratio.
Definition ndt.h:178
Eigen::Matrix< double, 15, 4 > angular_hessian_
Precomputed Angular Hessian.
Definition ndt.h:595
shared_ptr< NormalDistributionsTransform< PointSource, PointTarget, Scalar > > Ptr
Definition ndt.h:94
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Matrix4 &trans)
Convert 6 element transformation vector to transformation matrix.
Definition ndt.h:243
void setResolution(float resolution)
Set/change the voxel grid resolution.
Definition ndt.h:124
double step_size_
The maximum step length.
Definition ndt.h:563
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution,...
Definition ndt.h:571
void setStepSize(double step_size)
Set/change the newton line search maximum step length.
Definition ndt.h:169
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:537
PointIndices::Ptr PointIndicesPtr
Definition ndt.h:79
Eigen::Matrix< double, 3, 6 > point_jacobian_
The first order derivative of the transformation of a point w.r.t.
Definition ndt.h:600
double auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu=1.e-4) const
Auxiliary function used to determine endpoints of More-Thuente interval.
Definition ndt.h:531
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:650
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Affine3 &trans)
Convert 6 element transformation vector to affine transformation.
Definition ndt.h:229
typename Eigen::Transform< Scalar, 3, Eigen::Affine > Affine3
Definition ndt.h:99
double getTransformationLikelihood() const
Get the registration alignment likelihood.
Definition ndt.h:196
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
Registration represents the base registration class for general purpose, ICP-like methods.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
std::string reg_name_
The registration method name.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
bool converged_
Holds internal convergence state, given user parameters.
Eigen::Matrix< Scalar, 4, 4 > Matrix4
int max_iterations_
The maximum number of iterations the internal optimization should run for.
double transformation_rotation_epsilon_
The maximum rotation difference between two consecutive transformations in order to consider converge...
double inlier_threshold_
The inlier distance threshold for the internal RANSAC outlier rejection loop.
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
PointCloudTargetConstPtr target_
The input point cloud dataset target.
const std::string & getClassName() const
Abstract class get name method.
void setMinPointPerVoxel(int min_points_per_voxel)
Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance ...
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:214
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition bfgs.h:10
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
Definition utils.h:62
Defines all the PCL and non-PCL macros used.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition pcl_macros.h:156
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr