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