Point Cloud Library (PCL) 1.12.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 point
53 * cloud data. \note For more information please see <b>Magnusson, M. (2009). The
54 * Three-Dimensional Normal-Distributions Transform — an Efficient Representation for
55 * Registration, Surface Analysis, and Loop Detection. PhD thesis, Orebro University.
56 * Orebro Studies in Technology 36.</b>, <b>More, J., and Thuente, D. (1994). Line
57 * Search Algorithm with Guaranteed Sufficient Decrease In ACM Transactions on
58 * Mathematical Software.</b> and Sun, W. and Yuan, Y, (2006) Optimization Theory and
59 * Methods: Nonlinear Programming. 89-100 \note Math refactored by Todor Stoyanov.
60 * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
61 */
62template <typename PointSource, typename PointTarget>
63class NormalDistributionsTransform : public Registration<PointSource, PointTarget> {
64protected:
67 using PointCloudSourcePtr = typename PointCloudSource::Ptr;
68 using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
69
72 using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
73 using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
74
77
78 /** \brief Typename of searchable voxel grid containing mean and covariance. */
80 /** \brief Typename of pointer to searchable voxel grid. */
82 /** \brief Typename of const pointer to searchable voxel grid. */
84 /** \brief Typename of const pointer to searchable voxel grid leaf. */
86
87public:
88 using Ptr = shared_ptr<NormalDistributionsTransform<PointSource, PointTarget>>;
89 using ConstPtr =
90 shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget>>;
91
92 /** \brief Constructor.
93 * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_
94 * to 1.0
95 */
97
98 /** \brief Empty destructor */
100
101 /** \brief Provide a pointer to the input target (e.g., the point cloud that we want
102 * to align the input source to). \param[in] cloud the input point cloud target
103 */
104 inline void
110
111 /** \brief Set/change the voxel grid resolution.
112 * \param[in] resolution side length of voxels
113 */
114 inline void
115 setResolution(float resolution)
116 {
117 // Prevents unnessary voxel initiations
118 if (resolution_ != resolution) {
119 resolution_ = resolution;
120 if (input_) {
121 init();
122 }
123 }
124 }
125
126 /** \brief Get voxel grid resolution.
127 * \return side length of voxels
128 */
129 inline float
131 {
132 return resolution_;
133 }
134
135 /** \brief Get the newton line search maximum step length.
136 * \return maximum step length
137 */
138 inline double
140 {
141 return step_size_;
142 }
143
144 /** \brief Set/change the newton line search maximum step length.
145 * \param[in] step_size maximum step length
146 */
147 inline void
148 setStepSize(double step_size)
149 {
150 step_size_ = step_size;
151 }
152
153 /** \brief Get the point cloud outlier ratio.
154 * \return outlier ratio
155 */
156 inline double
158 {
159 return outlier_ratio_;
160 }
161
162 /** \brief Set/change the point cloud outlier ratio.
163 * \param[in] outlier_ratio outlier ratio
164 */
165 inline void
166 setOulierRatio(double outlier_ratio)
167 {
168 outlier_ratio_ = outlier_ratio;
169 }
170
171 /** \brief Get the registration alignment probability.
172 * \return transformation probability
173 */
174 inline double
176 {
177 return trans_probability_;
178 }
179
180 /** \brief Get the number of iterations required to calculate alignment.
181 * \return final number of iterations
182 */
183 inline int
185 {
186 return nr_iterations_;
187 }
188
189 /** \brief Convert 6 element transformation vector to affine transformation.
190 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
191 * \param[out] trans affine transform corresponding to given transfomation vector
192 */
193 static void
194 convertTransform(const Eigen::Matrix<double, 6, 1>& x, Eigen::Affine3f& trans)
195 {
196 trans = Eigen::Translation<float, 3>(x.head<3>().cast<float>()) *
197 Eigen::AngleAxis<float>(float(x(3)), Eigen::Vector3f::UnitX()) *
198 Eigen::AngleAxis<float>(float(x(4)), Eigen::Vector3f::UnitY()) *
199 Eigen::AngleAxis<float>(float(x(5)), Eigen::Vector3f::UnitZ());
200 }
201
202 /** \brief Convert 6 element transformation vector to transformation matrix.
203 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
204 * \param[out] trans 4x4 transformation matrix corresponding to given transfomation
205 * vector
206 */
207 static void
208 convertTransform(const Eigen::Matrix<double, 6, 1>& x, Eigen::Matrix4f& trans)
209 {
210 Eigen::Affine3f _affine;
211 convertTransform(x, _affine);
212 trans = _affine.matrix();
213 }
214
215protected:
216 using Registration<PointSource, PointTarget>::reg_name_;
217 using Registration<PointSource, PointTarget>::getClassName;
218 using Registration<PointSource, PointTarget>::input_;
219 using Registration<PointSource, PointTarget>::indices_;
220 using Registration<PointSource, PointTarget>::target_;
221 using Registration<PointSource, PointTarget>::nr_iterations_;
222 using Registration<PointSource, PointTarget>::max_iterations_;
223 using Registration<PointSource, PointTarget>::previous_transformation_;
224 using Registration<PointSource, PointTarget>::final_transformation_;
225 using Registration<PointSource, PointTarget>::transformation_;
226 using Registration<PointSource, PointTarget>::transformation_epsilon_;
227 using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
228 using Registration<PointSource, PointTarget>::converged_;
229 using Registration<PointSource, PointTarget>::corr_dist_threshold_;
230 using Registration<PointSource, PointTarget>::inlier_threshold_;
231
232 using Registration<PointSource, PointTarget>::update_visualizer_;
233
234 /** \brief Estimate the transformation and returns the transformed source (input) as
235 * output. \param[out] output the resultant input transformed point cloud dataset
236 */
237 virtual void
239 {
240 computeTransformation(output, Eigen::Matrix4f::Identity());
241 }
242
243 /** \brief Estimate the transformation and returns the transformed source (input) as
244 * output. \param[out] output the resultant input transformed point cloud dataset
245 * \param[in] guess the initial gross estimation of the transformation
246 */
247 void
249 const Eigen::Matrix4f& guess) override;
250
251 /** \brief Initiate covariance voxel structure. */
252 void inline init()
253 {
256 // Initiate voxel structure.
257 target_cells_.filter(true);
258 }
259
260 /** \brief Compute derivatives of probability function w.r.t. the transformation
261 * vector. \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. \param[out]
262 * score_gradient the gradient vector of the probability function w.r.t. the
263 * transformation vector \param[out] hessian the hessian matrix of the probability
264 * function w.r.t. the transformation vector \param[in] trans_cloud transformed point
265 * cloud \param[in] transform the current transform vector \param[in] compute_hessian
266 * flag to calculate hessian, unnessissary for step calculation.
267 */
268 double
269 computeDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
270 Eigen::Matrix<double, 6, 6>& hessian,
271 const PointCloudSource& trans_cloud,
272 const Eigen::Matrix<double, 6, 1>& transform,
273 bool compute_hessian = true);
274
275 /** \brief Compute individual point contirbutions to derivatives of probability
276 * function w.r.t. the transformation vector. \note Equation 6.10, 6.12 and 6.13
277 * [Magnusson 2009]. \param[in,out] score_gradient the gradient vector of the
278 * probability function w.r.t. the transformation vector \param[in,out] hessian the
279 * hessian matrix of the probability function w.r.t. the transformation vector
280 * \param[in] x_trans transformed point minus mean of occupied covariance voxel
281 * \param[in] c_inv covariance of occupied covariance voxel
282 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step
283 * calculation.
284 */
285 double
286 updateDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
287 Eigen::Matrix<double, 6, 6>& hessian,
288 const Eigen::Vector3d& x_trans,
289 const Eigen::Matrix3d& c_inv,
290 bool compute_hessian = true) const;
291
292 /** \brief Precompute anglular components of derivatives.
293 * \note Equation 6.19 and 6.21 [Magnusson 2009].
294 * \param[in] transform the current transform vector
295 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step
296 * calculation.
297 */
298 void
299 computeAngleDerivatives(const Eigen::Matrix<double, 6, 1>& transform,
300 bool compute_hessian = true);
301
302 /** \brief Compute point derivatives.
303 * \note Equation 6.18-21 [Magnusson 2009].
304 * \param[in] x point from the input cloud
305 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step
306 * calculation.
307 */
308 void
309 computePointDerivatives(const Eigen::Vector3d& x, bool compute_hessian = true);
310
311 /** \brief Compute hessian of probability function w.r.t. the transformation vector.
312 * \note Equation 6.13 [Magnusson 2009].
313 * \param[out] hessian the hessian matrix of the probability function w.r.t. the
314 * transformation vector \param[in] trans_cloud transformed point cloud
315 */
316 void
317 computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
318 const PointCloudSource& trans_cloud);
319
320 /** \brief Compute hessian of probability function w.r.t. the transformation vector.
321 * \note Equation 6.13 [Magnusson 2009].
322 * \param[out] hessian the hessian matrix of the probability function w.r.t. the
323 * transformation vector \param[in] trans_cloud transformed point cloud \param[in]
324 * transform the current transform vector
325 */
326 PCL_DEPRECATED(1, 15, "Parameter `transform` is not required")
327 void
328 computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
329 const PointCloudSource& trans_cloud,
330 const Eigen::Matrix<double, 6, 1>& transform)
331 {
332 pcl::utils::ignore(transform);
333 computeHessian(hessian, trans_cloud);
334 }
335
336 /** \brief Compute individual point contirbutions to hessian of probability function
337 * w.r.t. the transformation vector. \note Equation 6.13 [Magnusson 2009].
338 * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the
339 * transformation vector \param[in] x_trans transformed point minus mean of occupied
340 * covariance voxel \param[in] c_inv covariance of occupied covariance voxel
341 */
342 void
343 updateHessian(Eigen::Matrix<double, 6, 6>& hessian,
344 const Eigen::Vector3d& x_trans,
345 const Eigen::Matrix3d& c_inv) const;
346
347 /** \brief Compute line search step length and update transform and probability
348 * derivatives using More-Thuente method. \note Search Algorithm [More, Thuente 1994]
349 * \param[in] transform initial transformation vector, \f$ x \f$ in Equation 1.3
350 * (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
351 * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente
352 * 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
353 * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in
354 * Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2
355 * [Magnusson 2009] \param[in] step_max maximum step length, \f$ \alpha_max \f$ in
356 * Moore-Thuente (1994) \param[in] step_min minimum step length, \f$ \alpha_min \f$ in
357 * Moore-Thuente (1994) \param[out] score final score function value, \f$ f(x + \alpha
358 * p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2
359 * [Magnusson 2009] \param[in,out] score_gradient gradient of score function w.r.t.
360 * transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$
361 * \vec{g} \f$ in Algorithm 2 [Magnusson 2009] \param[out] hessian hessian of score
362 * function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente
363 * (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009] \param[in,out] trans_cloud
364 * transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in
365 * Algorithm 2 [Magnusson 2009] \return final step length
366 */
367 double
368 computeStepLengthMT(const Eigen::Matrix<double, 6, 1>& transform,
369 Eigen::Matrix<double, 6, 1>& step_dir,
370 double step_init,
371 double step_max,
372 double step_min,
373 double& score,
374 Eigen::Matrix<double, 6, 1>& score_gradient,
375 Eigen::Matrix<double, 6, 6>& hessian,
376 PointCloudSource& trans_cloud);
377
378 /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$
379 * in More-Thuente (1994) \note Updating Algorithm until some value satisfies \f$
380 * \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ and Modified Updating
381 * Algorithm from then on [More, Thuente 1994]. \param[in,out] a_l first endpoint of
382 * interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) \param[in,out] f_l
383 * value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l)
384 * \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
385 * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente
386 * (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$
387 * for Modified Update Algorithm \param[in,out] a_u second endpoint of interval \f$ I
388 * \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) \param[in,out] f_u value at second
389 * endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update
390 * Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm \param[in,out]
391 * g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$
392 * \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified
393 * Update Algorithm \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente
394 * (1994) \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994),
395 * \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified
396 * Update Algorithm \param[in] g_t derivative at trial value, \f$ g_t \f$ in
397 * Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$
398 * \phi'(\alpha_t) \f$ for Modified Update Algorithm \return if interval converges
399 */
400 bool
401 updateIntervalMT(double& a_l,
402 double& f_l,
403 double& g_l,
404 double& a_u,
405 double& f_u,
406 double& g_u,
407 double a_t,
408 double f_t,
409 double g_t) const;
410
411 /** \brief Select new trial value for More-Thuente method.
412 * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used
413 * for \f$ f_k \f$ and \f$ g_k \f$ until some value satisfies the test \f$
414 * \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ then \f$
415 * \phi(\alpha_k) \f$ is used from then on. \note Interpolation Minimizer equations
416 * from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang
417 * Yuan (89-100). \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l
418 * \f$ in Moore-Thuente (1994) \param[in] f_l value at first endpoint, \f$ f_l \f$ in
419 * Moore-Thuente (1994) \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in
420 * Moore-Thuente (1994) \param[in] a_u second endpoint of interval \f$ I \f$, \f$
421 * \alpha_u \f$ in Moore-Thuente (1994) \param[in] f_u value at second endpoint, \f$
422 * f_u \f$ in Moore-Thuente (1994) \param[in] g_u derivative at second endpoint, \f$
423 * g_u \f$ in Moore-Thuente (1994) \param[in] a_t previous trial value, \f$ \alpha_t
424 * \f$ in Moore-Thuente (1994) \param[in] f_t value at previous trial value, \f$ f_t
425 * \f$ in Moore-Thuente (1994) \param[in] g_t derivative at previous trial value, \f$
426 * g_t \f$ in Moore-Thuente (1994) \return new trial value
427 */
428 double
429 trialValueSelectionMT(double a_l,
430 double f_l,
431 double g_l,
432 double a_u,
433 double f_u,
434 double g_u,
435 double a_t,
436 double f_t,
437 double g_t) const;
438
439 /** \brief Auxiliary function used to determine endpoints of More-Thuente interval.
440 * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
441 * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
442 * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in
443 * More-Thuente (1994) \param[in] f_0 initial function value, \f$ \phi(0) \f$ in
444 * Moore-Thuente (1994) \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in
445 * More-Thuente (1994) \param[in] mu the step length, constant \f$ \mu \f$ in
446 * Equation 1.1 [More, Thuente 1994] \return sufficient decrease value
447 */
448 inline double
450 double a, double f_a, double f_0, double g_0, double mu = 1.e-4) const
451 {
452 return f_a - f_0 - mu * g_0 * a;
453 }
454
455 /** \brief Auxiliary function derivative used to determine endpoints of More-Thuente
456 * interval. \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente
457 * 1994) \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in
458 * More-Thuente (1994) \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in
459 * More-Thuente (1994) \param[in] mu the step length, constant \f$ \mu \f$ in
460 * Equation 1.1 [More, Thuente 1994] \return sufficient decrease derivative
461 */
462 inline double
463 auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) const
464 {
465 return g_a - mu * g_0;
466 }
467
468 /** \brief The voxel grid generated from target cloud containing point means and
469 * covariances. */
471
472 /** \brief The side length of voxels. */
474
475 /** \brief The maximum step length. */
477
478 /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7
479 * [Magnusson 2009]. */
481
482 /** \brief The normalization constants used fit the point distribution to a normal
483 * distribution, Equation 6.8 [Magnusson 2009]. */
485
486 /** \brief The probability score of the transform applied to the input cloud,
487 * Equation 6.9 and 6.10 [Magnusson 2009]. */
489
490 /** \brief Precomputed Angular Gradient
491 *
492 * The precomputed angular derivatives for the jacobian of a transformation vector,
493 * Equation 6.19 [Magnusson 2009].
494 */
495 Eigen::Matrix<double, 8, 4> angular_jacobian_;
496
497 /** \brief Precomputed Angular Hessian
498 *
499 * The precomputed angular derivatives for the hessian of a transformation vector,
500 * Equation 6.19 [Magnusson 2009].
501 */
502 Eigen::Matrix<double, 15, 4> angular_hessian_;
503
504 /** \brief The first order derivative of the transformation of a point w.r.t. the
505 * transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
506 Eigen::Matrix<double, 3, 6> point_jacobian_;
507
508 /** \brief The second order derivative of the transformation of a point w.r.t. the
509 * transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
510 Eigen::Matrix<double, 18, 6> point_hessian_;
511
512public:
514};
515} // namespace pcl
516
517#include <pcl/registration/impl/ndt.hpp>
A 3D Normal Distribution Transform registration implementation for point cloud data.
Definition ndt.h:63
Eigen::Matrix< double, 15, 4 > angular_hessian_
Precomputed Angular Hessian.
Definition ndt.h:502
bool updateIntervalMT(double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t) const
Update interval of possible step lengths for More-Thuente method, in More-Thuente (1994)
Definition ndt.hpp:490
double trans_probability_
The probability score of the transform applied to the input cloud, Equation 6.9 and 6....
Definition ndt.h:488
double step_size_
The maximum step length.
Definition ndt.h:476
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:105
shared_ptr< const NormalDistributionsTransform< PointSource, PointTarget > > ConstPtr
Definition ndt.h:89
float getResolution() const
Get voxel grid resolution.
Definition ndt.h:130
void updateHessian(Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const
Compute individual point contirbutions to hessian of probability function w.r.t.
Definition ndt.hpp:455
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:463
void computePointDerivatives(const Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
Definition ndt.hpp:319
void setResolution(float resolution)
Set/change the voxel grid resolution.
Definition ndt.h:115
double getStepSize() const
Get the newton line search maximum step length.
Definition ndt.h:139
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition ndt.h:65
double getTransformationProbability() const
Get the registration alignment probability.
Definition ndt.h:175
Eigen::Matrix< double, 3, 6 > point_jacobian_
The first order derivative of the transformation of a point w.r.t.
Definition ndt.h:506
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
Definition ndt.h:238
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Eigen::Affine3f &trans)
Convert 6 element transformation vector to affine transformation.
Definition ndt.h:194
int getFinalNumIteration() const
Get the number of iterations required to calculate alignment.
Definition ndt.h:184
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition ndt.h:68
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Eigen::Matrix4f &trans)
Convert 6 element transformation vector to transformation matrix.
Definition ndt.h:208
double computeDerivatives(Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud, const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Compute derivatives of probability function w.r.t.
Definition ndt.hpp:184
double outlier_ratio_
The ratio of outliers of points w.r.t.
Definition ndt.h:480
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution,...
Definition ndt.h:484
PointIndices::ConstPtr PointIndicesConstPtr
Definition ndt.h:76
double updateDerivatives(Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian=true) const
Compute individual point contirbutions to derivatives of probability function w.r....
Definition ndt.hpp:365
float resolution_
The side length of voxels.
Definition ndt.h:473
shared_ptr< NormalDistributionsTransform< PointSource, PointTarget > > Ptr
Definition ndt.h:88
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ndt.h:67
double trialValueSelectionMT(double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, double g_t) const
Select new trial value for More-Thuente method.
Definition ndt.hpp:535
double computeStepLengthMT(const Eigen::Matrix< double, 6, 1 > &transform, Eigen::Matrix< double, 6, 1 > &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, PointCloudSource &trans_cloud)
Compute line search step length and update transform and probability derivatives using More-Thuente m...
Definition ndt.hpp:648
typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr
Typename of const pointer to searchable voxel grid leaf.
Definition ndt.h:85
~NormalDistributionsTransform()
Empty destructor.
Definition ndt.h:99
void computeAngleDerivatives(const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Precompute anglular components of derivatives.
Definition ndt.hpp:235
Eigen::Matrix< double, 8, 4 > angular_jacobian_
Precomputed Angular Gradient.
Definition ndt.h:495
void init()
Initiate covariance voxel structure.
Definition ndt.h:252
PointIndices::Ptr PointIndicesPtr
Definition ndt.h:75
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition ndt.h:73
void setOulierRatio(double outlier_ratio)
Set/change the point cloud outlier ratio.
Definition ndt.h:166
TargetGrid target_cells_
The voxel grid generated from target cloud containing point means and covariances.
Definition ndt.h:470
Eigen::Matrix< double, 18, 6 > point_hessian_
The second order derivative of the transformation of a point w.r.t.
Definition ndt.h:510
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:449
void computeHessian(Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud)
Compute hessian of probability function w.r.t.
Definition ndt.hpp:413
double getOulierRatio() const
Get the point cloud outlier ratio.
Definition ndt.h:157
void setStepSize(double step_size)
Set/change the newton line search maximum step length.
Definition ndt.h:148
NormalDistributionsTransform()
Constructor.
Definition ndt.hpp:47
typename Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
Definition ndt.h:70
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition ndt.h:72
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.
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 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