Point Cloud Library (PCL) 1.15.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 (target_) { // init() needs target_
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 Get the point cloud outlier ratio.
184 * \return outlier ratio
185 */
187 18,
188 "The method `getOulierRatio` has been renamed to "
189 "`getOutlierRatio`.")
190 inline double
192 {
193 return outlier_ratio_;
194 }
195
196 /** \brief Set/change the point cloud outlier ratio.
197 * \param[in] outlier_ratio outlier ratio
198 */
199 inline void
200 setOutlierRatio(double outlier_ratio)
201 {
202 outlier_ratio_ = outlier_ratio;
203 }
204
206 18,
207 "The method `setOulierRatio` has been renamed to "
208 "`setOutlierRatio`.")
209 /** \brief Set/change the point cloud outlier ratio.
210 * \param[in] outlier_ratio outlier ratio
211 */
212 inline void
213 setOulierRatio(double outlier_ratio)
214 {
215 outlier_ratio_ = outlier_ratio;
216 }
217
218 /** \brief Get the registration alignment likelihood.
219 * \return transformation likelihood
220 */
221 inline double
223 {
224 return trans_likelihood_;
225 }
226
227 /** \brief Get the registration alignment probability.
228 * \return transformation probability
229 */
231 16,
232 "The method `getTransformationProbability` has been renamed to "
233 "`getTransformationLikelihood`.")
234 inline double
236 {
237 return trans_likelihood_;
238 }
239
240 /** \brief Get the number of iterations required to calculate alignment.
241 * \return final number of iterations
242 */
243 inline int
245 {
246 return nr_iterations_;
247 }
248
249 /** \brief Get access to the `VoxelGridCovariance` generated from target cloud
250 * containing point means and covariances. Set the input target cloud before calling
251 * this. Useful for debugging, e.g.
252 * \code
253 * pcl::PointCloud<PointXYZ> visualize_cloud;
254 * ndt.getTargetCells().getDisplayCloud(visualize_cloud);
255 * \endcode
256 */
257 inline const TargetGrid&
259 {
260 return target_cells_;
261 }
262
263 /** \brief Convert 6 element transformation vector to affine transformation.
264 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
265 * \param[out] trans affine transform corresponding to given transformation
266 * vector
267 */
268 static void
269 convertTransform(const Eigen::Matrix<double, 6, 1>& x, Affine3& trans)
270 {
271 trans = Eigen::Translation<Scalar, 3>(x.head<3>().cast<Scalar>()) *
272 Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(3)), Vector3::UnitX()) *
273 Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(4)), Vector3::UnitY()) *
274 Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(5)), Vector3::UnitZ());
275 }
276
277 /** \brief Convert 6 element transformation vector to transformation matrix.
278 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
279 * \param[out] trans 4x4 transformation matrix corresponding to given
280 * transformation vector
281 */
282 static void
283 convertTransform(const Eigen::Matrix<double, 6, 1>& x, Matrix4& trans)
284 {
285 Affine3 _affine;
286 convertTransform(x, _affine);
287 trans = _affine.matrix();
288 }
289
290protected:
291 using Registration<PointSource, PointTarget, Scalar>::reg_name_;
292 using Registration<PointSource, PointTarget, Scalar>::getClassName;
293 using Registration<PointSource, PointTarget, Scalar>::input_;
294 using Registration<PointSource, PointTarget, Scalar>::indices_;
295 using Registration<PointSource, PointTarget, Scalar>::target_;
296 using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
297 using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
298 using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
299 using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
300 using Registration<PointSource, PointTarget, Scalar>::transformation_;
301 using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
302 using Registration<PointSource, PointTarget, Scalar>::
304 using Registration<PointSource, PointTarget, Scalar>::converged_;
305 using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
306 using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
307
308 using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
309
310 /** \brief Estimate the transformation and returns the transformed source
311 * (input) as output.
312 * \param[out] output the resultant input transformed point cloud dataset
313 */
314 virtual void
316 {
317 computeTransformation(output, Matrix4::Identity());
318 }
319
320 /** \brief Estimate the transformation and returns the transformed source
321 * (input) as output.
322 * \param[out] output the resultant input transformed point cloud dataset
323 * \param[in] guess the initial gross estimation of the transformation
324 */
325 void
326 computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
327
328 /** \brief Initiate covariance voxel structure. */
329 void inline init()
330 {
333 // Initiate voxel structure.
334 target_cells_.filter(true);
335 PCL_DEBUG("[pcl::%s::init] Computed voxel structure, got %zu voxels with valid "
336 "normal distributions.\n",
337 getClassName().c_str(),
338 target_cells_.getCentroids()->size());
339 }
340
341 /** \brief Compute derivatives of likelihood function w.r.t. the
342 * transformation vector.
343 * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
344 * \param[out] score_gradient the gradient vector of the likelihood function
345 * w.r.t. the transformation vector
346 * \param[out] hessian the hessian matrix of the likelihood function
347 * w.r.t. the transformation vector
348 * \param[in] trans_cloud transformed point cloud
349 * \param[in] transform the current transform vector
350 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
351 * calculation.
352 */
353 double
354 computeDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
355 Eigen::Matrix<double, 6, 6>& hessian,
356 const PointCloudSource& trans_cloud,
357 const Eigen::Matrix<double, 6, 1>& transform,
358 bool compute_hessian = true);
359
360 /** \brief Compute individual point contributions to derivatives of
361 * likelihood function w.r.t. the transformation vector.
362 * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
363 * \param[in,out] score_gradient the gradient vector of the likelihood
364 * function w.r.t. the transformation vector
365 * \param[in,out] hessian the hessian matrix of the likelihood function
366 * w.r.t. the transformation vector
367 * \param[in] x_trans transformed point minus mean of occupied covariance
368 * voxel
369 * \param[in] c_inv covariance of occupied covariance voxel
370 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
371 * calculation.
372 */
373 double
374 updateDerivatives(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 = true) const;
379
380 /** \brief Precompute angular components of derivatives.
381 * \note Equation 6.19 and 6.21 [Magnusson 2009].
382 * \param[in] transform the current transform vector
383 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
384 * calculation.
385 */
386 void
387 computeAngleDerivatives(const Eigen::Matrix<double, 6, 1>& transform,
388 bool compute_hessian = true);
389
390 /** \brief Compute point derivatives.
391 * \note Equation 6.18-21 [Magnusson 2009].
392 * \param[in] x point from the input cloud
393 * \param[in] compute_hessian flag to calculate hessian, unnecessary for step
394 * calculation.
395 */
396 void
397 computePointDerivatives(const Eigen::Vector3d& x, bool compute_hessian = true);
398
399 /** \brief Compute hessian of likelihood function w.r.t. the transformation
400 * vector.
401 * \note Equation 6.13 [Magnusson 2009].
402 * \param[out] hessian the hessian matrix of the likelihood function
403 * w.r.t. the transformation vector
404 * \param[in] trans_cloud transformed point cloud
405 */
406 void
407 computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
408 const PointCloudSource& trans_cloud);
409
410 /** \brief Compute individual point contributions to hessian of likelihood
411 * function w.r.t. the transformation vector.
412 * \note Equation 6.13 [Magnusson 2009].
413 * \param[in,out] hessian the hessian matrix of the likelihood function
414 * w.r.t. the transformation vector
415 * \param[in] x_trans transformed point minus mean of occupied covariance
416 * voxel
417 * \param[in] c_inv covariance of occupied covariance voxel
418 */
419 void
420 updateHessian(Eigen::Matrix<double, 6, 6>& hessian,
421 const Eigen::Vector3d& x_trans,
422 const Eigen::Matrix3d& c_inv) const;
423
424 /** \brief Compute line search step length and update transform and
425 * likelihood derivatives using More-Thuente method.
426 * \note Search Algorithm [More, Thuente 1994]
427 * \param[in] transform initial transformation vector, \f$ x \f$ in Equation
428 * 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson
429 * 2009]
430 * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore,
431 * Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2
432 * [Magnusson 2009]
433 * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in
434 * Moore-Thuente (1994) and the normal of \f$ \delta \vec{p} \f$ in Algorithm
435 * 2 [Magnusson 2009]
436 * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in
437 * Moore-Thuente (1994)
438 * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in
439 * Moore-Thuente (1994)
440 * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in
441 * Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2
442 * [Magnusson 2009]
443 * \param[in,out] score_gradient gradient of score function w.r.t.
444 * transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and
445 * \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
446 * \param[out] hessian hessian of score function w.r.t. transformation vector,
447 * \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in
448 * Algorithm 2 [Magnusson 2009]
449 * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed
450 * by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
451 * \return final step length
452 */
453 double
454 computeStepLengthMT(const Eigen::Matrix<double, 6, 1>& transform,
455 Eigen::Matrix<double, 6, 1>& step_dir,
456 double step_init,
457 double step_max,
458 double step_min,
459 double& score,
460 Eigen::Matrix<double, 6, 1>& score_gradient,
461 Eigen::Matrix<double, 6, 6>& hessian,
462 PointCloudSource& trans_cloud);
463
464 /** \brief Update interval of possible step lengths for More-Thuente method,
465 * \f$ I \f$ in More-Thuente (1994)
466 * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq
467 * 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ and Modified Updating Algorithm
468 * from then on [More, Thuente 1994].
469 * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$
470 * in Moore-Thuente (1994)
471 * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente
472 * (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l)
473 * \f$ for Modified Update Algorithm
474 * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in
475 * Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$
476 * \phi'(\alpha_l) \f$ for Modified Update Algorithm
477 * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$
478 * in Moore-Thuente (1994)
479 * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente
480 * (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u)
481 * \f$ for Modified Update Algorithm
482 * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in
483 * Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$
484 * \phi'(\alpha_u) \f$ for Modified Update Algorithm
485 * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
486 * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994),
487 * \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for
488 * Modified Update Algorithm
489 * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente
490 * (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$
491 * \phi'(\alpha_t) \f$ for Modified Update Algorithm
492 * \return if interval converges
493 */
494 bool
495 updateIntervalMT(double& a_l,
496 double& f_l,
497 double& g_l,
498 double& a_u,
499 double& f_u,
500 double& g_u,
501 double a_t,
502 double f_t,
503 double g_t) const;
504
505 /** \brief Select new trial value for More-Thuente method.
506 * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is
507 * used for \f$ f_k \f$ and \f$ g_k \f$ until some value satisfies the test
508 * \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ then \f$
509 * \phi(\alpha_k) \f$ is used from then on.
510 * \note Interpolation Minimizer equations from Optimization Theory and
511 * Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
512 * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in
513 * Moore-Thuente (1994)
514 * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
515 * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente
516 * (1994)
517 * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in
518 * Moore-Thuente (1994)
519 * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente
520 * (1994)
521 * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente
522 * (1994)
523 * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente
524 * (1994)
525 * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente
526 * (1994)
527 * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in
528 * Moore-Thuente (1994)
529 * \return new trial value
530 */
531 double
532 trialValueSelectionMT(double a_l,
533 double f_l,
534 double g_l,
535 double a_u,
536 double f_u,
537 double g_u,
538 double a_t,
539 double f_t,
540 double g_t) const;
541
542 /** \brief Auxiliary function used to determine endpoints of More-Thuente
543 * interval.
544 * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
545 * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
546 * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in
547 * More-Thuente (1994)
548 * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente
549 * (1994)
550 * \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente
551 * (1994)
552 * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More,
553 * Thuente 1994]
554 * \return sufficient decrease value
555 */
556 inline double
558 double a, double f_a, double f_0, double g_0, double mu = 1.e-4) const
559 {
560 return f_a - f_0 - mu * g_0 * a;
561 }
562
563 /** \brief Auxiliary function derivative used to determine endpoints of
564 * More-Thuente interval.
565 * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente
566 * 1994)
567 * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in
568 * More-Thuente (1994)
569 * \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente
570 * (1994)
571 * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More,
572 * Thuente 1994]
573 * \return sufficient decrease derivative
574 */
575 inline double
576 auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) const
577 {
578 return g_a - mu * g_0;
579 }
580
581 /** \brief The voxel grid generated from target cloud containing point means
582 * and covariances. */
584
585 /** \brief The side length of voxels. */
586 float resolution_{1.0f};
587
588 /** \brief The maximum step length. */
589 double step_size_{0.1};
590
591 /** \brief The ratio of outliers of points w.r.t. a normal distribution,
592 * Equation 6.7 [Magnusson 2009]. */
593 double outlier_ratio_{0.55};
594
595 /** \brief The normalization constants used fit the point distribution to a
596 * normal distribution, Equation 6.8 [Magnusson 2009]. */
597 double gauss_d1_{0.0}, gauss_d2_{0.0};
598
599 /** \brief The likelihood score of the transform applied to the input cloud,
600 * Equation 6.9 and 6.10 [Magnusson 2009]. */
601 union {
603 16,
604 "`trans_probability_` has been renamed to `trans_likelihood_`.")
606 double trans_likelihood_{0.0};
607 };
608
609 /** \brief Precomputed Angular Gradient
610 *
611 * The precomputed angular derivatives for the jacobian of a transformation
612 * vector, Equation 6.19 [Magnusson 2009].
613 */
614 Eigen::Matrix<double, 8, 4> angular_jacobian_;
615
616 /** \brief Precomputed Angular Hessian
617 *
618 * The precomputed angular derivatives for the hessian of a transformation
619 * vector, Equation 6.19 [Magnusson 2009].
620 */
621 Eigen::Matrix<double, 15, 4> angular_hessian_;
622
623 /** \brief The first order derivative of the transformation of a point
624 * w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson
625 * 2009]. */
626 Eigen::Matrix<double, 3, 6> point_jacobian_;
627
628 /** \brief The second order derivative of the transformation of a point
629 * w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson
630 * 2009]. */
631 Eigen::Matrix<double, 18, 6> point_hessian_;
632
633public:
635};
636} // namespace pcl
637
638#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
const TargetGrid & getTargetCells() const
Get access to the VoxelGridCovariance generated from target cloud containing point means and covarian...
Definition ndt.h:258
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:631
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
Definition ndt.h:315
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:244
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
void setOutlierRatio(double outlier_ratio)
Set/change the point cloud outlier ratio.
Definition ndt.h:200
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:329
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:576
float resolution_
The side length of voxels.
Definition ndt.h:586
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:593
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
VoxelGridCovariance< PointTarget > TargetGrid
Typename of searchable voxel grid containing mean and covariance.
Definition ndt.h:84
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:235
TargetGrid target_cells_
The voxel grid generated from target cloud containing point means and covariances.
Definition ndt.h:583
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ndt.h:71
Eigen::Matrix< double, 8, 4 > angular_jacobian_
Precomputed Angular Gradient.
Definition ndt.h:614
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:213
double getOulierRatio() const
Get the point cloud outlier ratio.
Definition ndt.h:191
Eigen::Matrix< double, 15, 4 > angular_hessian_
Precomputed Angular Hessian.
Definition ndt.h:621
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:283
void setResolution(float resolution)
Set/change the voxel grid resolution.
Definition ndt.h:124
double step_size_
The maximum step length.
Definition ndt.h:589
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution,...
Definition ndt.h:597
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:626
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:557
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:269
typename Eigen::Transform< Scalar, 3, Eigen::Affine > Affine3
Definition ndt.h:99
double getOutlierRatio() const
Get the point cloud outlier ratio.
Definition ndt.h:178
double getTransformationLikelihood() const
Get the registration alignment likelihood.
Definition ndt.h:222
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.
PointCloudPtr getCentroids()
Get a pointcloud containing the voxel centroids.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:247
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:86
Defines functions, macros and traits for allocating and using memory.
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:158
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr