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.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
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 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
159  getStepSize() const
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  */
203  PCL_DEPRECATED(1,
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 
249 protected:
266 
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. */
559  float resolution_;
560 
561  /** \brief The maximum step length. */
562  double step_size_;
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 {
575  PCL_DEPRECATED(1,
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 
606 public:
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
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 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
Definition: PointIndices.h:13
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14