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