Template Class NormalDistributionsTransform

Inheritance Relationships

Base Type

  • public pcl::Registration< PointSource, PointTarget >

Class Documentation

template<typename PointSource, typename PointTarget>
class NormalDistributionsTransform : public pcl::Registration<PointSource, PointTarget>

A 3D Normal Distribution Transform registration implementation for point cloud data.

Author

Brian Okorn (Space and Naval Warfare Systems Center Pacific)

Note

For more information please see Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection. PhD thesis, Orebro University. Orebro Studies in Technology 36., More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease In ACM Transactions on Mathematical Software. and Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100

Note

Math refactored by Todor Stoyanov.

Public Types

typedef pcl::shared_ptr<NormalDistributionsTransform<PointSource, PointTarget>> Ptr
typedef pcl::shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget>> ConstPtr

Public Functions

NormalDistributionsTransform()

Constructor. Sets outlier_ratio_ to 0.35, step_size_ to 0.05 and resolution_ to 1.0.

inline virtual ~NormalDistributionsTransform()

Empty destructor.

inline void setNumThreads(int n)
inline 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 to).

Parameters:

cloud – [in] the input point cloud target

inline void setResolution(float resolution)

Set/change the voxel grid resolution.

Parameters:

resolution – [in] side length of voxels

inline float getResolution() const

Get voxel grid resolution.

Returns:

side length of voxels

inline double getStepSize() const

Get the newton line search maximum step length.

Returns:

maximum step length

inline void setStepSize(double step_size)

Set/change the newton line search maximum step length.

Parameters:

step_size – [in] maximum step length

inline double getOutlierRatio() const

Get the point cloud outlier ratio.

Returns:

outlier ratio

inline void setOutlierRatio(double outlier_ratio)

Set/change the point cloud outlier ratio.

Parameters:

outlier_ratio – [in] outlier ratio

inline void setNeighborhoodSearchMethod(NeighborSearchMethod method)
inline double getTransformationProbability() const

Get the registration alignment probability.

Returns:

transformation probability

inline int getFinalNumIteration() const

Get the number of iterations required to calculate alignment.

Returns:

final number of iterations

double calculateScore(const PointCloudSource &cloud) const

Public Members

NeighborSearchMethod search_method

Public Static Functions

static inline void convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans)

Convert 6 element transformation vector to affine transformation.

Parameters:

  • x – [in] transformation vector of the form [x, y, z, roll, pitch, yaw]

  • trans – [out] affine transform corresponding to given transformation vector

static inline void convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans)

Convert 6 element transformation vector to transformation matrix.

Parameters:

  • x – [in] transformation vector of the form [x, y, z, roll, pitch, yaw]

  • trans – [out] 4x4 transformation matrix corresponding to given transformation vector

Protected Types

typedef pcl::Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource
typedef PointCloudSource::Ptr PointCloudSourcePtr
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
typedef pcl::Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget
typedef PointCloudTarget::Ptr PointCloudTargetPtr
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
typedef pcl::PointIndices::Ptr PointIndicesPtr
typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr
typedef pclomp::VoxelGridCovariance<PointTarget> TargetGrid

Typename of searchable voxel grid containing mean and covariance.

typedef TargetGrid *TargetGridPtr

Typename of pointer to searchable voxel grid.

typedef const TargetGrid *TargetGridConstPtr

Typename of const pointer to searchable voxel grid.

typedef TargetGrid::LeafConstPtr TargetGridLeafConstPtr

Typename of const pointer to searchable voxel grid leaf.

Protected Functions

inline virtual void computeTransformation(PointCloudSource &output)

Estimate the transformation and returns the transformed source (input) as output.

Parameters:

output – [out] the resultant input transformed point cloud dataset

virtual void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess)

Estimate the transformation and returns the transformed source (input) as output.

Parameters:

  • output – [out] the resultant input transformed point cloud dataset

  • guess – [in] the initial gross estimation of the transformation

inline void init()

Initiate covariance voxel structure.

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. the transformation vector.

Note

Equation 6.10, 6.12 and 6.13 [Magnusson 2009].

Parameters:

  • score_gradient – [out] the gradient vector of the probability function w.r.t. the transformation vector

  • hessian – [out] the hessian matrix of the probability function w.r.t. the transformation vector

  • trans_cloud – [in] transformed point cloud

  • p – [in] the current transform vector

  • compute_hessian – [in] flag to calculate hessian, unnecessary for step calculation.

double updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian, const Eigen::Matrix<float, 4, 6> &point_gradient_, const Eigen::Matrix<float, 24, 6> &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian = true) const

Compute individual point contributions to derivatives of probability function w.r.t. the transformation vector.

Note

Equation 6.10, 6.12 and 6.13 [Magnusson 2009].

Parameters:

  • score_gradient – [inout] the gradient vector of the probability function w.r.t. the transformation vector

  • hessian – [inout] the hessian matrix of the probability function w.r.t. the transformation vector

  • x_trans – [in] transformed point minus mean of occupied covariance voxel

  • c_inv – [in] covariance of occupied covariance voxel

  • compute_hessian – [in] flag to calculate hessian, unnecessary for step calculation.

void computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true)

Precompute angular components of derivatives.

Note

Equation 6.19 and 6.21 [Magnusson 2009].

Parameters:

  • p – [in] the current transform vector

  • compute_hessian – [in] flag to calculate hessian, unnecessary for step calculation.

void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6> &point_gradient_, Eigen::Matrix<double, 18, 6> &point_hessian_, bool compute_hessian = true) const

Compute point derivatives.

Note

Equation 6.18-21 [Magnusson 2009].

Parameters:

  • x – [in] point from the input cloud

  • compute_hessian – [in] flag to calculate hessian, unnecessary for step calculation.

void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6> &point_gradient_, Eigen::Matrix<float, 24, 6> &point_hessian_, bool compute_hessian = true) const
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. the transformation vector.

Note

Equation 6.13 [Magnusson 2009].

Parameters:

  • hessian – [out] the hessian matrix of the probability function w.r.t. the transformation vector

  • trans_cloud – [in] transformed point cloud

  • p – [in] the current transform vector

void updateHessian(Eigen::Matrix<double, 6, 6> &hessian, const Eigen::Matrix<double, 3, 6> &point_gradient_, const Eigen::Matrix<double, 18, 6> &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const

Compute individual point contributions to hessian of probability function w.r.t. the transformation vector.

Note

Equation 6.13 [Magnusson 2009].

Parameters:

  • hessian – [inout] the hessian matrix of the probability function w.r.t. the transformation vector

  • x_trans – [in] transformed point minus mean of occupied covariance voxel

  • c_inv – [in] covariance of occupied covariance voxel

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 method.

Note

Search Algorithm [More, Thuente 1994]

Parameters:

  • x – [in] initial transformation vector, \( x \) in Equation 1.3 (Moore, Thuente 1994) and \( \vec{p} \) in Algorithm 2 [Magnusson 2009]

  • step_dir – [in] descent direction, \( p \) in Equation 1.3 (Moore, Thuente 1994) and \( \delta \vec{p} \) normalized in Algorithm 2 [Magnusson 2009]

  • step_init – [in] initial step length estimate, \( \alpha_0 \) in Moore-Thuente (1994) and the normal of \( \delta \vec{p} \) in Algorithm 2 [Magnusson 2009]

  • step_max – [in] maximum step length, \( \alpha_max \) in Moore-Thuente (1994)

  • step_min – [in] minimum step length, \( \alpha_min \) in Moore-Thuente (1994)

  • score – [out] final score function value, \( f(x + \alpha p) \) in Equation 1.3 (Moore, Thuente 1994) and \( score \) in Algorithm 2 [Magnusson 2009]

  • score_gradient – [inout] gradient of score function w.r.t. transformation vector, \( f'(x + \alpha p) \) in Moore-Thuente (1994) and \( \vec{g} \) in Algorithm 2 [Magnusson 2009]

  • hessian – [out] hessian of score function w.r.t. transformation vector, \( f''(x + \alpha p) \) in Moore-Thuente (1994) and \( H \) in Algorithm 2 [Magnusson 2009]

  • trans_cloud – [inout] transformed point cloud, \( X \) transformed by \( T(\vec{p},\vec{x}) \) in Algorithm 2 [Magnusson 2009]

Returns:

final step length

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, \( I \) in More-Thuente (1994)

Note

Updating Algorithm until some value satisfies \( \psi(\alpha_k) \leq 0 \) and \( \phi'(\alpha_k) \geq 0 \) and Modified Updating Algorithm from then on [More, Thuente 1994].

Parameters:

  • a_l – [inout] first endpoint of interval \( I \), \( \alpha_l \) in Moore-Thuente (1994)

  • f_l – [inout] value at first endpoint, \( f_l \) in Moore-Thuente (1994), \( \psi(\alpha_l) \) for Update Algorithm and \( \phi(\alpha_l) \) for Modified Update Algorithm

  • g_l – [inout] derivative at first endpoint, \( g_l \) in Moore-Thuente (1994), \( \psi'(\alpha_l) \) for Update Algorithm and \( \phi'(\alpha_l) \) for Modified Update Algorithm

  • a_u – [inout] second endpoint of interval \( I \), \( \alpha_u \) in Moore-Thuente (1994)

  • f_u – [inout] value at second endpoint, \( f_u \) in Moore-Thuente (1994), \( \psi(\alpha_u) \) for Update Algorithm and \( \phi(\alpha_u) \) for Modified Update Algorithm

  • g_u – [inout] derivative at second endpoint, \( g_u \) in Moore-Thuente (1994), \( \psi'(\alpha_u) \) for Update Algorithm and \( \phi'(\alpha_u) \) for Modified Update Algorithm

  • a_t – [in] trial value, \( \alpha_t \) in Moore-Thuente (1994)

  • f_t – [in] value at trial value, \( f_t \) in Moore-Thuente (1994), \( \psi(\alpha_t) \) for Update Algorithm and \( \phi(\alpha_t) \) for Modified Update Algorithm

  • g_t – [in] derivative at trial value, \( g_t \) in Moore-Thuente (1994), \( \psi'(\alpha_t) \) for Update Algorithm and \( \phi'(\alpha_t) \) for Modified Update Algorithm

Returns:

if interval converges

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.

Note

Trial Value Selection [More, Thuente 1994], \( \psi(\alpha_k) \) is used for \( f_k \) and \( g_k \) until some value satisfies the test \( \psi(\alpha_k) \leq 0 \) and \( \phi'(\alpha_k) \geq 0 \) then \( \phi(\alpha_k) \) is used from then on.

Note

Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).

Parameters:

  • a_l – [in] first endpoint of interval \( I \), \( \alpha_l \) in Moore-Thuente (1994)

  • f_l – [in] value at first endpoint, \( f_l \) in Moore-Thuente (1994)

  • g_l – [in] derivative at first endpoint, \( g_l \) in Moore-Thuente (1994)

  • a_u – [in] second endpoint of interval \( I \), \( \alpha_u \) in Moore-Thuente (1994)

  • f_u – [in] value at second endpoint, \( f_u \) in Moore-Thuente (1994)

  • g_u – [in] derivative at second endpoint, \( g_u \) in Moore-Thuente (1994)

  • a_t – [in] previous trial value, \( \alpha_t \) in Moore-Thuente (1994)

  • f_t – [in] value at previous trial value, \( f_t \) in Moore-Thuente (1994)

  • g_t – [in] derivative at previous trial value, \( g_t \) in Moore-Thuente (1994)

Returns:

new trial value

inline double auxiliaryFunction_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.

Note

\( \psi(\alpha) \) in Equation 1.6 (Moore, Thuente 1994)

Parameters:

  • a – [in] the step length, \( \alpha \) in More-Thuente (1994)

  • f_a – [in] function value at step length a, \( \phi(\alpha) \) in More-Thuente (1994)

  • f_0 – [in] initial function value, \( \phi(0) \) in Moore-Thuente (1994)

  • g_0 – [in] initial function gradiant, \( \phi'(0) \) in More-Thuente (1994)

  • mu – [in] the step length, constant \( \mu \) in Equation 1.1 [More, Thuente 1994]

Returns:

sufficient decrease value

inline double auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4)

Auxiliary function derivative used to determine endpoints of More-Thuente interval.

Note

\( \psi'(\alpha) \), derivative of Equation 1.6 (Moore, Thuente 1994)

Parameters:

  • g_a – [in] function gradient at step length a, \( \phi'(\alpha) \) in More-Thuente (1994)

  • g_0 – [in] initial function gradiant, \( \phi'(0) \) in More-Thuente (1994)

  • mu – [in] the step length, constant \( \mu \) in Equation 1.1 [More, Thuente 1994]

Returns:

sufficient decrease derivative

Protected Attributes

TargetGrid target_cells_

The voxel grid generated from target cloud containing point means and covariances.

float resolution_

The side length of voxels.

double step_size_

The maximum step length.

double outlier_ratio_

The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009].

double gauss_d1_

The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009].

double gauss_d2_
double gauss_d3_
double trans_probability_

The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009].

Eigen::Vector3d j_ang_a_

Precomputed Angular Gradient.

The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].

Eigen::Vector3d j_ang_b_
Eigen::Vector3d j_ang_c_
Eigen::Vector3d j_ang_d_
Eigen::Vector3d j_ang_e_
Eigen::Vector3d j_ang_f_
Eigen::Vector3d j_ang_g_
Eigen::Vector3d j_ang_h_
Eigen::Matrix<float, 8, 4> j_ang
Eigen::Vector3d h_ang_a2_

Precomputed Angular Hessian.

The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].

Eigen::Vector3d h_ang_a3_
Eigen::Vector3d h_ang_b2_
Eigen::Vector3d h_ang_b3_
Eigen::Vector3d h_ang_c2_
Eigen::Vector3d h_ang_c3_
Eigen::Vector3d h_ang_d1_
Eigen::Vector3d h_ang_d2_
Eigen::Vector3d h_ang_d3_
Eigen::Vector3d h_ang_e1_
Eigen::Vector3d h_ang_e2_
Eigen::Vector3d h_ang_e3_
Eigen::Vector3d h_ang_f1_
Eigen::Vector3d h_ang_f2_
Eigen::Vector3d h_ang_f3_
Eigen::Matrix<float, 16, 4> h_ang
int num_threads_

The first order derivative of the transformation of a point w.r.t. the transform vector, \( J_E \) in Equation 6.18 [Magnusson 2009].

The second order derivative of the transformation of a point w.r.t. the transform vector, \( H_E \) in Equation 6.20 [Magnusson 2009].