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