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