Point Cloud Library (PCL)  1.9.1-dev
List of all members | Public Types | Public Member Functions | Static Public Member Functions | Protected Types | Protected Member Functions | Protected Attributes
pcl::NormalDistributionsTransform< PointSource, PointTarget > Class Template Reference

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

#include <pcl/registration/ndt.h>

+ Inheritance diagram for pcl::NormalDistributionsTransform< PointSource, PointTarget >:

Public Types

typedef boost::shared_ptr< NormalDistributionsTransform< PointSource, PointTarget > > Ptr
 
typedef boost::shared_ptr< const NormalDistributionsTransform< PointSource, PointTarget > > ConstPtr
 
- Public Types inherited from pcl::Registration< PointSource, PointTarget >
typedef Eigen::Matrix< float, 4, 4 > Matrix4
 
typedef boost::shared_ptr< Registration< PointSource, PointTarget, float > > Ptr
 
typedef boost::shared_ptr< const Registration< PointSource, PointTarget, float > > ConstPtr
 
typedef pcl::registration::CorrespondenceRejector::Ptr CorrespondenceRejectorPtr
 
typedef pcl::search::KdTree< PointTarget > KdTree
 
typedef pcl::search::KdTree< PointTarget >::Ptr KdTreePtr
 
typedef pcl::search::KdTree< PointSource > KdTreeReciprocal
 
typedef KdTreeReciprocal::Ptr KdTreeReciprocalPtr
 
typedef pcl::PointCloud< PointSource > PointCloudSource
 
typedef PointCloudSource::Ptr PointCloudSourcePtr
 
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
 
typedef pcl::PointCloud< PointTarget > PointCloudTarget
 
typedef PointCloudTarget::Ptr PointCloudTargetPtr
 
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
 
typedef KdTree::PointRepresentationConstPtr PointRepresentationConstPtr
 
typedef pcl::registration::TransformationEstimation< PointSource, PointTarget, float > TransformationEstimation
 
typedef TransformationEstimation::Ptr TransformationEstimationPtr
 
typedef TransformationEstimation::ConstPtr TransformationEstimationConstPtr
 
typedef pcl::registration::CorrespondenceEstimationBase< PointSource, PointTarget, float > CorrespondenceEstimation
 
typedef CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
 
typedef CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr
 
- Public Types inherited from pcl::PCLBase< PointSource >
typedef pcl::PointCloud< PointSource > PointCloud
 
typedef PointCloud::Ptr PointCloudPtr
 
typedef PointCloud::ConstPtr PointCloudConstPtr
 
typedef boost::shared_ptr< PointIndicesPointIndicesPtr
 
typedef boost::shared_ptr< PointIndices const > PointIndicesConstPtr
 

Public Member Functions

 NormalDistributionsTransform ()
 Constructor. More...
 
virtual ~NormalDistributionsTransform ()
 Empty destructor. More...
 
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). More...
 
void setResolution (float resolution)
 Set/change the voxel grid resolution. More...
 
float getResolution () const
 Get voxel grid resolution. More...
 
double getStepSize () const
 Get the newton line search maximum step length. More...
 
void setStepSize (double step_size)
 Set/change the newton line search maximum step length. More...
 
double getOulierRatio () const
 Get the point cloud outlier ratio. More...
 
void setOulierRatio (double outlier_ratio)
 Set/change the point cloud outlier ratio. More...
 
double getTransformationProbability () const
 Get the registration alignment probability. More...
 
int getFinalNumIteration () const
 Get the number of iterations required to calculate alignment. More...
 
- Public Member Functions inherited from pcl::Registration< PointSource, PointTarget >
 Registration ()
 Empty constructor. More...
 
virtual ~Registration ()
 destructor. More...
 
void setTransformationEstimation (const TransformationEstimationPtr &te)
 Provide a pointer to the transformation estimation object. More...
 
void setCorrespondenceEstimation (const CorrespondenceEstimationPtr &ce)
 Provide a pointer to the correspondence estimation object. More...
 
virtual void setInputSource (const PointCloudSourceConstPtr &cloud)
 Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) More...
 
PointCloudSourceConstPtr const getInputSource ()
 Get a pointer to the input point cloud dataset target. More...
 
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 to) More...
 
PointCloudTargetConstPtr const getInputTarget ()
 Get a pointer to the input point cloud dataset target. More...
 
void setSearchMethodTarget (const KdTreePtr &tree, bool force_no_recompute=false)
 Provide a pointer to the search object used to find correspondences in the target cloud. More...
 
KdTreePtr getSearchMethodTarget () const
 Get a pointer to the search method used to find correspondences in the target cloud. More...
 
void setSearchMethodSource (const KdTreeReciprocalPtr &tree, bool force_no_recompute=false)
 Provide a pointer to the search object used to find correspondences in the source cloud (usually used by reciprocal correspondence finding). More...
 
KdTreeReciprocalPtr getSearchMethodSource () const
 Get a pointer to the search method used to find correspondences in the source cloud. More...
 
Matrix4 getFinalTransformation ()
 Get the final transformation matrix estimated by the registration method. More...
 
Matrix4 getLastIncrementalTransformation ()
 Get the last incremental transformation matrix estimated by the registration method. More...
 
void setMaximumIterations (int nr_iterations)
 Set the maximum number of iterations the internal optimization should run for. More...
 
int getMaximumIterations ()
 Get the maximum number of iterations the internal optimization should run for, as set by the user. More...
 
void setRANSACIterations (int ransac_iterations)
 Set the number of iterations RANSAC should run for. More...
 
double getRANSACIterations ()
 Get the number of iterations RANSAC should run for, as set by the user. More...
 
void setRANSACOutlierRejectionThreshold (double inlier_threshold)
 Set the inlier distance threshold for the internal RANSAC outlier rejection loop. More...
 
double getRANSACOutlierRejectionThreshold ()
 Get the inlier distance threshold for the internal outlier rejection loop as set by the user. More...
 
void setMaxCorrespondenceDistance (double distance_threshold)
 Set the maximum distance threshold between two correspondent points in source <-> target. More...
 
double getMaxCorrespondenceDistance ()
 Get the maximum distance threshold between two correspondent points in source <-> target. More...
 
void setTransformationEpsilon (double epsilon)
 Set the transformation epsilon (maximum allowable translation squared difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. More...
 
double getTransformationEpsilon ()
 Get the transformation epsilon (maximum allowable translation squared difference between two consecutive transformations) as set by the user. More...
 
void setTransformationRotationEpsilon (double epsilon)
 Set the transformation rotation epsilon (maximum allowable rotation difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. More...
 
double getTransformationRotationEpsilon ()
 Get the transformation rotation epsilon (maximum allowable difference between two consecutive transformations) as set by the user (epsilon is the cos(angle) in a axis-angle representation). More...
 
void setEuclideanFitnessEpsilon (double epsilon)
 Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged. More...
 
double getEuclideanFitnessEpsilon ()
 Get the maximum allowed distance error before the algorithm will be considered to have converged, as set by the user. More...
 
void setPointRepresentation (const PointRepresentationConstPtr &point_representation)
 Provide a boost shared pointer to the PointRepresentation to be used when comparing points. More...
 
bool registerVisualizationCallback (boost::function< FunctionSignature > &visualizerCallback)
 Register the user callback function which will be called from registration thread in order to update point cloud obtained after each iteration. More...
 
double getFitnessScore (double max_range=std::numeric_limits< double >::max())
 Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) More...
 
double getFitnessScore (const std::vector< float > &distances_a, const std::vector< float > &distances_b)
 Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) from two sets of correspondence distances (distances between source and target points) More...
 
bool hasConverged ()
 Return the state of convergence after the last align run. More...
 
void align (PointCloudSource &output)
 Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. More...
 
void align (PointCloudSource &output, const Matrix4 &guess)
 Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output. More...
 
const std::string & getClassName () const
 Abstract class get name method. More...
 
bool initCompute ()
 Internal computation initialization. More...
 
bool initComputeReciprocal ()
 Internal computation when reciprocal lookup is needed. More...
 
void addCorrespondenceRejector (const CorrespondenceRejectorPtr &rejector)
 Add a new correspondence rejector to the list. More...
 
std::vector< CorrespondenceRejectorPtrgetCorrespondenceRejectors ()
 Get the list of correspondence rejectors. More...
 
bool removeCorrespondenceRejector (unsigned int i)
 Remove the i-th correspondence rejector in the list. More...
 
void clearCorrespondenceRejectors ()
 Clear the list of correspondence rejectors. More...
 
- Public Member Functions inherited from pcl::PCLBase< PointSource >
 PCLBase ()
 Empty constructor. More...
 
 PCLBase (const PCLBase &base)
 Copy constructor. More...
 
virtual ~PCLBase ()
 Destructor. More...
 
virtual void setInputCloud (const PointCloudConstPtr &cloud)
 Provide a pointer to the input dataset. More...
 
PointCloudConstPtr const getInputCloud () const
 Get a pointer to the input point cloud dataset. More...
 
virtual void setIndices (const IndicesPtr &indices)
 Provide a pointer to the vector of indices that represents the input data. More...
 
virtual void setIndices (const IndicesConstPtr &indices)
 Provide a pointer to the vector of indices that represents the input data. More...
 
virtual void setIndices (const PointIndicesConstPtr &indices)
 Provide a pointer to the vector of indices that represents the input data. More...
 
virtual void setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols)
 Set the indices for the points laying within an interest region of the point cloud. More...
 
IndicesPtr const getIndices ()
 Get a pointer to the vector of indices used. More...
 
IndicesConstPtr const getIndices () const
 Get a pointer to the vector of indices used. More...
 
const PointSource & operator[] (size_t pos) const
 Override PointCloud operator[] to shorten code. More...
 

Static Public Member Functions

static void convertTransform (const Eigen::Matrix< double, 6, 1 > &x, Eigen::Affine3f &trans)
 Convert 6 element transformation vector to affine transformation. More...
 
static void convertTransform (const Eigen::Matrix< double, 6, 1 > &x, Eigen::Matrix4f &trans)
 Convert 6 element transformation vector to transformation matrix. More...
 

Protected Types

typedef Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
 
typedef PointCloudSource::Ptr PointCloudSourcePtr
 
typedef PointCloudSource::ConstPtr PointCloudSourceConstPtr
 
typedef Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
 
typedef PointCloudTarget::Ptr PointCloudTargetPtr
 
typedef PointCloudTarget::ConstPtr PointCloudTargetConstPtr
 
typedef PointIndices::Ptr PointIndicesPtr
 
typedef PointIndices::ConstPtr PointIndicesConstPtr
 
typedef VoxelGridCovariance< PointTarget > TargetGrid
 Typename of searchable voxel grid containing mean and covariance. More...
 
typedef TargetGridTargetGridPtr
 Typename of pointer to searchable voxel grid. More...
 
typedef const TargetGridTargetGridConstPtr
 Typename of const pointer to searchable voxel grid. More...
 
typedef TargetGrid::LeafConstPtr TargetGridLeafConstPtr
 Typename of const pointer to searchable voxel grid leaf. More...
 

Protected Member Functions

virtual void computeTransformation (PointCloudSource &output)
 Estimate the transformation and returns the transformed source (input) as output. More...
 
virtual void computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
 Estimate the transformation and returns the transformed source (input) as output. More...
 
void init ()
 Initiate covariance voxel structure. More...
 
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. More...
 
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.t. More...
 
void computeAngleDerivatives (Eigen::Matrix< double, 6, 1 > &p, bool compute_hessian=true)
 Precompute anglular components of derivatives. More...
 
void computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian=true)
 Compute point derivatives. More...
 
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. More...
 
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. More...
 
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. More...
 
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) More...
 
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. More...
 
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. More...
 
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. More...
 
- Protected Member Functions inherited from pcl::Registration< PointSource, PointTarget >
bool searchForNeighbors (const PointCloudSource &cloud, int index, std::vector< int > &indices, std::vector< float > &distances)
 Search for the closest nearest neighbor of a given point. More...
 
virtual void computeTransformation (PointCloudSource &output, const Matrix4 &guess)=0
 Abstract transformation computation method with initial guess. More...
 
- Protected Member Functions inherited from pcl::PCLBase< PointSource >
bool initCompute ()
 This method should get called before starting the actual computation. More...
 
bool deinitCompute ()
 This method should get called after finishing the actual computation. More...
 

Protected Attributes

TargetGrid target_cells_
 The voxel grid generated from target cloud containing point means and covariances. More...
 
float resolution_
 The side length of voxels. More...
 
double step_size_
 The maximum step length. More...
 
double outlier_ratio_
 The ratio of outliers of points w.r.t. More...
 
double gauss_d1_
 The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. More...
 
double gauss_d2_
 
double trans_probability_
 The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. More...
 
Eigen::Vector3d j_ang_a_
 Precomputed Angular Gradient. More...
 
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::Vector3d h_ang_a2_
 Precomputed Angular Hessian. More...
 
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< double, 3, 6 > point_gradient_
 The first order derivative of the transformation of a point w.r.t. More...
 
Eigen::Matrix< double, 18, 6 > point_hessian_
 The second order derivative of the transformation of a point w.r.t. More...
 
- Protected Attributes inherited from pcl::Registration< PointSource, PointTarget >
std::string reg_name_
 The registration method name. More...
 
KdTreePtr tree_
 A pointer to the spatial search object. More...
 
KdTreeReciprocalPtr tree_reciprocal_
 A pointer to the spatial search object of the source. More...
 
int nr_iterations_
 The number of iterations the internal optimization ran for (used internally). More...
 
int max_iterations_
 The maximum number of iterations the internal optimization should run for. More...
 
int ransac_iterations_
 The number of iterations RANSAC should run for. More...
 
PointCloudTargetConstPtr target_
 The input point cloud dataset target. More...
 
Matrix4 final_transformation_
 The final transformation matrix estimated by the registration method after N iterations. More...
 
Matrix4 transformation_
 The transformation matrix estimated by the registration method. More...
 
Matrix4 previous_transformation_
 The previous transformation matrix estimated by the registration method (used internally). More...
 
double transformation_epsilon_
 The maximum difference between two consecutive transformations in order to consider convergence (user defined). More...
 
double transformation_rotation_epsilon_
 The maximum rotation difference between two consecutive transformations in order to consider convergence (user defined). More...
 
double euclidean_fitness_epsilon_
 The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the algorithm is considered to have converged. More...
 
double corr_dist_threshold_
 The maximum distance threshold between two correspondent points in source <-> target. More...
 
double inlier_threshold_
 The inlier distance threshold for the internal RANSAC outlier rejection loop. More...
 
bool converged_
 Holds internal convergence state, given user parameters. More...
 
int min_number_correspondences_
 The minimum number of correspondences that the algorithm needs before attempting to estimate the transformation. More...
 
CorrespondencesPtr correspondences_
 The set of correspondences determined at this ICP step. More...
 
TransformationEstimationPtr transformation_estimation_
 A TransformationEstimation object, used to calculate the 4x4 rigid transformation. More...
 
CorrespondenceEstimationPtr correspondence_estimation_
 A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. More...
 
std::vector< CorrespondenceRejectorPtrcorrespondence_rejectors_
 The list of correspondence rejectors to use. More...
 
bool target_cloud_updated_
 Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. More...
 
bool source_cloud_updated_
 Variable that stores whether we have a new source cloud, meaning we need to pre-process it again. More...
 
bool force_no_recompute_
 A flag which, if set, means the tree operating on the target cloud will never be recomputed. More...
 
bool force_no_recompute_reciprocal_
 A flag which, if set, means the tree operating on the source cloud will never be recomputed. More...
 
boost::function< void(const pcl::PointCloud< PointSource > &cloud_src, const std::vector< int > &indices_src, const pcl::PointCloud< PointTarget > &cloud_tgt, const std::vector< int > &indices_tgt)> update_visualizer_
 Callback function to update intermediate source point cloud position during it's registration to the target point cloud. More...
 
- Protected Attributes inherited from pcl::PCLBase< PointSource >
PointCloudConstPtr input_
 The input point cloud dataset. More...
 
IndicesPtr indices_
 A pointer to the vector of point indices to use. More...
 
bool use_indices_
 Set to true if point indices are used. More...
 
bool fake_indices_
 If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud. More...
 

Detailed Description

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

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

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
Math refactored by Todor Stoyanov.
Author
Brian Okorn (Space and Naval Warfare Systems Center Pacific)

Definition at line 62 of file ndt.h.

Member Typedef Documentation

template<typename PointSource , typename PointTarget >
typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > pcl::NormalDistributionsTransform< PointSource, PointTarget >::ConstPtr

Definition at line 90 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef Registration<PointSource, PointTarget>::PointCloudSource pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointCloudSource
protected

Definition at line 66 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudSource::ConstPtr pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointCloudSourceConstPtr
protected

Definition at line 68 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudSource::Ptr pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointCloudSourcePtr
protected

Definition at line 67 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef Registration<PointSource, PointTarget>::PointCloudTarget pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointCloudTarget
protected

Definition at line 70 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudTarget::ConstPtr pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointCloudTargetConstPtr
protected

Definition at line 72 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef PointCloudTarget::Ptr pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointCloudTargetPtr
protected

Definition at line 71 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef PointIndices::ConstPtr pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointIndicesConstPtr
protected

Definition at line 75 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef PointIndices::Ptr pcl::NormalDistributionsTransform< PointSource, PointTarget >::PointIndicesPtr
protected

Definition at line 74 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > pcl::NormalDistributionsTransform< PointSource, PointTarget >::Ptr

Definition at line 89 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef VoxelGridCovariance<PointTarget> pcl::NormalDistributionsTransform< PointSource, PointTarget >::TargetGrid
protected

Typename of searchable voxel grid containing mean and covariance.

Definition at line 78 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef const TargetGrid* pcl::NormalDistributionsTransform< PointSource, PointTarget >::TargetGridConstPtr
protected

Typename of const pointer to searchable voxel grid.

Definition at line 82 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef TargetGrid::LeafConstPtr pcl::NormalDistributionsTransform< PointSource, PointTarget >::TargetGridLeafConstPtr
protected

Typename of const pointer to searchable voxel grid leaf.

Definition at line 84 of file ndt.h.

template<typename PointSource , typename PointTarget >
typedef TargetGrid* pcl::NormalDistributionsTransform< PointSource, PointTarget >::TargetGridPtr
protected

Typename of pointer to searchable voxel grid.

Definition at line 80 of file ndt.h.

Constructor & Destructor Documentation

template<typename PointSource , typename PointTarget >
pcl::NormalDistributionsTransform< PointSource, PointTarget >::NormalDistributionsTransform ( )
template<typename PointSource , typename PointTarget >
virtual pcl::NormalDistributionsTransform< PointSource, PointTarget >::~NormalDistributionsTransform ( )
inlinevirtual

Empty destructor.

Definition at line 99 of file ndt.h.

Member Function Documentation

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::auxilaryFunction_dPsiMT ( double  g_a,
double  g_0,
double  mu = 1.e-4 
)
inlineprotected

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

Note
$ \psi'(\alpha) $, derivative of Equation 1.6 (Moore, Thuente 1994)
Parameters
[in]g_afunction gradient at step length a, $ \phi'(\alpha) $ in More-Thuente (1994)
[in]g_0initial function gradiant, $ \phi'(0) $ in More-Thuente (1994)
[in]muthe step length, constant $ \mu $ in Equation 1.1 [More, Thuente 1994]
Returns
sufficient decrease derivative

Definition at line 412 of file ndt.h.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeStepLengthMT().

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::auxilaryFunction_PsiMT ( double  a,
double  f_a,
double  f_0,
double  g_0,
double  mu = 1.e-4 
)
inlineprotected

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

Note
$ \psi(\alpha) $ in Equation 1.6 (Moore, Thuente 1994)
Parameters
[in]athe step length, $ \alpha $ in More-Thuente (1994)
[in]f_afunction value at step length a, $ \phi(\alpha) $ in More-Thuente (1994)
[in]f_0initial function value, $ \phi(0) $ in Moore-Thuente (1994)
[in]g_0initial function gradiant, $ \phi'(0) $ in More-Thuente (1994)
[in]muthe step length, constant $ \mu $ in Equation 1.1 [More, Thuente 1994]
Returns
sufficient decrease value

Definition at line 399 of file ndt.h.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeStepLengthMT().

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeAngleDerivatives ( Eigen::Matrix< double, 6, 1 > &  p,
bool  compute_hessian = true 
)
protected

Precompute anglular components of derivatives.

Note
Equation 6.19 and 6.21 [Magnusson 2009].
Parameters
[in]pthe current transform vector
[in]compute_hessianflag to calculate hessian, unnessissary for step calculation.

Definition at line 239 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_a2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_a3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_b2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_b3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_c2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_c3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_a_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_b_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_c_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_d_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_e_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_f_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_g_, and pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_h_.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeDerivatives(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::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 
)
protected

Compute derivatives of probability function w.r.t.

the transformation vector.

Note
Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
Parameters
[out]score_gradientthe gradient vector of the probability function w.r.t. the transformation vector
[out]hessianthe hessian matrix of the probability function w.r.t. the transformation vector
[in]trans_cloudtransformed point cloud
[in]pthe current transform vector
[in]compute_hessianflag to calculate hessian, unnessissary for step calculation.

Definition at line 182 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeAngleDerivatives(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::computePointDerivatives(), pcl::PCLBase< PointSource >::input_, pcl::VoxelGridCovariance< PointT >::radiusSearch(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::resolution_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::target_cells_, and pcl::NormalDistributionsTransform< PointSource, PointTarget >::updateDerivatives().

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeStepLengthMT(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeTransformation(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeHessian ( Eigen::Matrix< double, 6, 6 > &  hessian,
PointCloudSource trans_cloud,
Eigen::Matrix< double, 6, 1 > &  p 
)
protected
template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::computePointDerivatives ( Eigen::Vector3d &  x,
bool  compute_hessian = true 
)
protected

Compute point derivatives.

Note
Equation 6.18-21 [Magnusson 2009].
Parameters
[in]xpoint from the input cloud
[in]compute_hessianflag to calculate hessian, unnessissary for step calculation.

Definition at line 316 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_a2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_a3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_b2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_b3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_c2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_c3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f3_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_a_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_b_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_c_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_d_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_e_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_f_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_g_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_h_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_gradient_, and pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_hessian_.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeDerivatives(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeHessian(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::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 
)
protected

Compute line search step length and update transform and probability derivatives using More-Thuente method.

Note
Search Algorithm [More, Thuente 1994]
Parameters
[in]xinitial transformation vector, $ x $ in Equation 1.3 (Moore, Thuente 1994) and $ \vec{p} $ in Algorithm 2 [Magnusson 2009]
[in]step_dirdescent direction, $ p $ in Equation 1.3 (Moore, Thuente 1994) and $ \delta \vec{p} $ normalized in Algorithm 2 [Magnusson 2009]
[in]step_initinitial step length estimate, $ \alpha_0 $ in Moore-Thuente (1994) and the noramal of $ \delta \vec{p} $ in Algorithm 2 [Magnusson 2009]
[in]step_maxmaximum step length, $ \alpha_max $ in Moore-Thuente (1994)
[in]step_minminimum step length, $ \alpha_min $ in Moore-Thuente (1994)
[out]scorefinal score function value, $ f(x + \alpha p) $ in Equation 1.3 (Moore, Thuente 1994) and $ score $ in Algorithm 2 [Magnusson 2009]
[in,out]score_gradientgradient of score function w.r.t. transformation vector, $ f'(x + \alpha p) $ in Moore-Thuente (1994) and $ \vec{g} $ in Algorithm 2 [Magnusson 2009]
[out]hessianhessian of score function w.r.t. transformation vector, $ f''(x + \alpha p) $ in Moore-Thuente (1994) and $ H $ in Algorithm 2 [Magnusson 2009]
[in,out]trans_cloudtransformed point cloud, $ X $ transformed by $ T(\vec{p},\vec{x}) $ in Algorithm 2 [Magnusson 2009]
Returns
final step length

Definition at line 610 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::auxilaryFunction_dPsiMT(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::auxilaryFunction_PsiMT(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeDerivatives(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeHessian(), pcl::Registration< PointSource, PointTarget >::final_transformation_, pcl::PCLBase< PointSource >::input_, pcl::transformPointCloud(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::trialValueSelectionMT(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::updateIntervalMT().

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeTransformation(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
virtual void pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeTransformation ( PointCloudSource output)
inlineprotectedvirtual

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

Parameters
[out]outputthe resultant input transformed point cloud dataset

Definition at line 238 of file ndt.h.

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeTransformation ( PointCloudSource output,
const Eigen::Matrix4f &  guess 
)
protectedvirtual

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

Parameters
[out]outputthe resultant input transformed point cloud dataset
[in]guessthe initial gross estimation of the transformation

Definition at line 77 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeDerivatives(), pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeStepLengthMT(), pcl::Registration< PointSource, PointTarget >::converged_, pcl::Registration< PointSource, PointTarget >::final_transformation_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d2_, pcl::PCLBase< PointSource >::input_, pcl::Registration< PointSource, PointTarget >::max_iterations_, pcl::Registration< PointSource, PointTarget >::nr_iterations_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::outlier_ratio_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_gradient_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_hessian_, pcl::Registration< PointSource, PointTarget >::previous_transformation_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::resolution_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::step_size_, pcl::Registration< PointSource, PointTarget >::target_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::trans_probability_, pcl::Registration< PointSource, PointTarget >::transformation_, pcl::Registration< PointSource, PointTarget >::transformation_epsilon_, pcl::Registration< PointSource, PointTarget >::transformation_rotation_epsilon_, pcl::transformPointCloud(), and pcl::Registration< PointSource, PointTarget >::update_visualizer_.

template<typename PointSource , typename PointTarget >
static void pcl::NormalDistributionsTransform< PointSource, PointTarget >::convertTransform ( const Eigen::Matrix< double, 6, 1 > &  x,
Eigen::Affine3f &  trans 
)
inlinestatic

Convert 6 element transformation vector to affine transformation.

Parameters
[in]xtransformation vector of the form [x, y, z, roll, pitch, yaw]
[out]transaffine transform corresponding to given transfomation vector

Definition at line 194 of file ndt.h.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::convertTransform().

template<typename PointSource , typename PointTarget >
static void pcl::NormalDistributionsTransform< PointSource, PointTarget >::convertTransform ( const Eigen::Matrix< double, 6, 1 > &  x,
Eigen::Matrix4f &  trans 
)
inlinestatic

Convert 6 element transformation vector to transformation matrix.

Parameters
[in]xtransformation vector of the form [x, y, z, roll, pitch, yaw]
[out]trans4x4 transformation matrix corresponding to given transfomation vector

Definition at line 207 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::convertTransform().

template<typename PointSource , typename PointTarget >
int pcl::NormalDistributionsTransform< PointSource, PointTarget >::getFinalNumIteration ( ) const
inline

Get the number of iterations required to calculate alignment.

Returns
final number of iterations

Definition at line 184 of file ndt.h.

References pcl::Registration< PointSource, PointTarget >::nr_iterations_.

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::getOulierRatio ( ) const
inline

Get the point cloud outlier ratio.

Returns
outlier ratio

Definition at line 157 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::outlier_ratio_.

template<typename PointSource , typename PointTarget >
float pcl::NormalDistributionsTransform< PointSource, PointTarget >::getResolution ( ) const
inline

Get voxel grid resolution.

Returns
side length of voxels

Definition at line 130 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::resolution_.

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::getStepSize ( ) const
inline

Get the newton line search maximum step length.

Returns
maximum step length

Definition at line 139 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::step_size_.

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::getTransformationProbability ( ) const
inline

Get the registration alignment probability.

Returns
transformation probability

Definition at line 175 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::trans_probability_.

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::init ( )
inlineprotected
template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::setInputTarget ( const PointCloudTargetConstPtr cloud)
inline

Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).

Parameters
[in]cloudthe input point cloud target

Definition at line 105 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::init(), and pcl::Registration< PointSource, PointTarget, Scalar >::setInputTarget().

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::setOulierRatio ( double  outlier_ratio)
inline

Set/change the point cloud outlier ratio.

Parameters
[in]outlier_ratiooutlier ratio

Definition at line 166 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::outlier_ratio_.

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::setResolution ( float  resolution)
inline

Set/change the voxel grid resolution.

Parameters
[in]resolutionside length of voxels

Definition at line 115 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::init(), pcl::PCLBase< PointSource >::input_, and pcl::NormalDistributionsTransform< PointSource, PointTarget >::resolution_.

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::setStepSize ( double  step_size)
inline

Set/change the newton line search maximum step length.

Parameters
[in]step_sizemaximum step length

Definition at line 148 of file ndt.h.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::step_size_.

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::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 
)
protected

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.
Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
Parameters
[in]a_lfirst endpoint of interval $ I $, $ \alpha_l $ in Moore-Thuente (1994)
[in]f_lvalue at first endpoint, $ f_l $ in Moore-Thuente (1994)
[in]g_lderivative at first endpoint, $ g_l $ in Moore-Thuente (1994)
[in]a_usecond endpoint of interval $ I $, $ \alpha_u $ in Moore-Thuente (1994)
[in]f_uvalue at second endpoint, $ f_u $ in Moore-Thuente (1994)
[in]g_uderivative at second endpoint, $ g_u $ in Moore-Thuente (1994)
[in]a_tprevious trial value, $ \alpha_t $ in Moore-Thuente (1994)
[in]f_tvalue at previous trial value, $ f_t $ in Moore-Thuente (1994)
[in]g_tderivative at previous trial value, $ g_t $ in Moore-Thuente (1994)
Returns
new trial value

Definition at line 527 of file ndt.hpp.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeStepLengthMT(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::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 
)
protected

Compute individual point contirbutions to derivatives of probability function w.r.t.

the transformation vector.

Note
Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
Parameters
[in,out]score_gradientthe gradient vector of the probability function w.r.t. the transformation vector
[in,out]hessianthe hessian matrix of the probability function w.r.t. the transformation vector
[in]x_transtransformed point minus mean of occupied covariance voxel
[in]c_invcovariance of occupied covariance voxel
[in]compute_hessianflag to calculate hessian, unnessissary for step calculation.

Definition at line 357 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_gradient_, and pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_hessian_.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeDerivatives(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
void pcl::NormalDistributionsTransform< PointSource, PointTarget >::updateHessian ( Eigen::Matrix< double, 6, 6 > &  hessian,
Eigen::Vector3d &  x_trans,
Eigen::Matrix3d &  c_inv 
)
protected

Compute individual point contirbutions to hessian of probability function w.r.t.

the transformation vector.

Note
Equation 6.13 [Magnusson 2009].
Parameters
[in,out]hessianthe hessian matrix of the probability function w.r.t. the transformation vector
[in]x_transtransformed point minus mean of occupied covariance voxel
[in]c_invcovariance of occupied covariance voxel

Definition at line 455 of file ndt.hpp.

References pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d1_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d2_, pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_gradient_, and pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_hessian_.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeHessian(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

template<typename PointSource , typename PointTarget >
bool pcl::NormalDistributionsTransform< PointSource, PointTarget >::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 
)
protected

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
[in,out]a_lfirst endpoint of interval $ I $, $ \alpha_l $ in Moore-Thuente (1994)
[in,out]f_lvalue at first endpoint, $ f_l $ in Moore-Thuente (1994), $ \psi(\alpha_l) $ for Update Algorithm and $ \phi(\alpha_l) $ for Modified Update Algorithm
[in,out]g_lderivative at first endpoint, $ g_l $ in Moore-Thuente (1994), $ \psi'(\alpha_l) $ for Update Algorithm and $ \phi'(\alpha_l) $ for Modified Update Algorithm
[in,out]a_usecond endpoint of interval $ I $, $ \alpha_u $ in Moore-Thuente (1994)
[in,out]f_uvalue at second endpoint, $ f_u $ in Moore-Thuente (1994), $ \psi(\alpha_u) $ for Update Algorithm and $ \phi(\alpha_u) $ for Modified Update Algorithm
[in,out]g_uderivative at second endpoint, $ g_u $ in Moore-Thuente (1994), $ \psi'(\alpha_u) $ for Update Algorithm and $ \phi'(\alpha_u) $ for Modified Update Algorithm
[in]a_ttrial value, $ \alpha_t $ in Moore-Thuente (1994)
[in]f_tvalue at trial value, $ f_t $ in Moore-Thuente (1994), $ \psi(\alpha_t) $ for Update Algorithm and $ \phi(\alpha_t) $ for Modified Update Algorithm
[in]g_tderivative 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

Definition at line 486 of file ndt.hpp.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeStepLengthMT(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::init().

Member Data Documentation

template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d1_
protected
template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::gauss_d2_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_a2_
protected

Precomputed Angular Hessian.

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

Definition at line 447 of file ndt.h.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeAngleDerivatives(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::computePointDerivatives().

template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_a3_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_b2_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_b3_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_c2_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_c3_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d1_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d2_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_d3_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e1_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e2_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_e3_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f1_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f2_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::h_ang_f3_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_a_
protected

Precomputed Angular Gradient.

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

Definition at line 441 of file ndt.h.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeAngleDerivatives(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::computePointDerivatives().

template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_b_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_c_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_d_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_e_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_f_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_g_
protected
template<typename PointSource , typename PointTarget >
Eigen::Vector3d pcl::NormalDistributionsTransform< PointSource, PointTarget >::j_ang_h_
protected
template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::outlier_ratio_
protected
template<typename PointSource , typename PointTarget >
Eigen::Matrix<double, 3, 6> pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_gradient_
protected
template<typename PointSource , typename PointTarget >
Eigen::Matrix<double, 18, 6> pcl::NormalDistributionsTransform< PointSource, PointTarget >::point_hessian_
protected
template<typename PointSource , typename PointTarget >
float pcl::NormalDistributionsTransform< PointSource, PointTarget >::resolution_
protected
template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::step_size_
protected
template<typename PointSource , typename PointTarget >
TargetGrid pcl::NormalDistributionsTransform< PointSource, PointTarget >::target_cells_
protected
template<typename PointSource , typename PointTarget >
double pcl::NormalDistributionsTransform< PointSource, PointTarget >::trans_probability_
protected

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

Definition at line 435 of file ndt.h.

Referenced by pcl::NormalDistributionsTransform< PointSource, PointTarget >::computeTransformation(), and pcl::NormalDistributionsTransform< PointSource, PointTarget >::getTransformationProbability().


The documentation for this class was generated from the following files: