using Ptr = shared_ptr< NormalDistributionsTransform < PointSource, PointTarget, Scalar > >
using ConstPtr = shared_ptr< const NormalDistributionsTransform < PointSource, PointTarget, Scalar > >
using Vector3 = typename Eigen::Matrix< Scalar, 3, 1 >
using Matrix4 = typename Registration < PointSource, PointTarget, Scalar >::Matrix4
using Affine3 = typename Eigen::Transform< Scalar, 3, Eigen::Affine >
using Matrix4 = Eigen::Matrix< Scalar, 4, 4 >
using Ptr = shared_ptr< Registration < PointSource, PointTarget, Scalar > >
using ConstPtr = shared_ptr< const Registration < PointSource, PointTarget, Scalar > >
using CorrespondenceRejectorPtr = pcl::registration::CorrespondenceRejector::Ptr
using KdTree = pcl::search::KdTree < PointTarget >
using KdTreePtr = typename KdTree::Ptr
using KdTreeReciprocal = pcl::search::KdTree < PointSource >
using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr
using PointCloudSource = pcl::PointCloud < PointSource >
using PointCloudSourcePtr = typename PointCloudSource::Ptr
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr
using PointCloudTarget = pcl::PointCloud < PointTarget >
using PointCloudTargetPtr = typename PointCloudTarget::Ptr
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr
using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr
using TransformationEstimation = typename pcl::registration::TransformationEstimation < PointSource, PointTarget, Scalar >
using TransformationEstimationPtr = typename TransformationEstimation::Ptr
using TransformationEstimationConstPtr = typename TransformationEstimation::ConstPtr
using CorrespondenceEstimation = pcl::registration::CorrespondenceEstimationBase < PointSource, PointTarget, Scalar >
using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr
using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr
using UpdateVisualizerCallbackSignature = void(const pcl::PointCloud < PointSource > &, const pcl::Indices &, const pcl::PointCloud < PointTarget > &, const pcl::Indices &)
The callback signature to the function updating intermediate source point cloud position during it's registration to the target point cloud.
using PointCloud = pcl::PointCloud < PointSource >
using PointCloudPtr = typename PointCloud::Ptr
using PointCloudConstPtr = typename PointCloud::ConstPtr
using PointIndicesPtr = PointIndices::Ptr
using PointIndicesConstPtr = PointIndices::ConstPtr
NormalDistributionsTransform ()
Constructor.
~NormalDistributionsTransform () override=default
Empty destructor.
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 to).
void setResolution (float resolution)
Set/change the voxel grid resolution.
void setMinPointPerVoxel (unsigned int min_points_per_voxel)
Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
float getResolution () const
Get voxel grid resolution.
double getStepSize () const
Get the newton line search maximum step length.
void setStepSize (double step_size)
Set/change the newton line search maximum step length.
double getOutlierRatio () const
Get the point cloud outlier ratio.
double getOulierRatio () const
Get the point cloud outlier ratio.
void setOutlierRatio (double outlier_ratio)
Set/change the point cloud outlier ratio.
void setOulierRatio (double outlier_ratio)
Set/change the point cloud outlier ratio.
double getTransformationLikelihood () const
Get the registration alignment likelihood.
double getTransformationProbability () const
Get the registration alignment probability.
int getFinalNumIteration () const
Get the number of iterations required to calculate alignment.
const TargetGrid & getTargetCells () const
Get access to the VoxelGridCovariance
generated from target cloud containing point means and covariances.
Registration ()
Empty constructor.
~Registration () override=default
destructor.
void setTransformationEstimation (const TransformationEstimationPtr &te)
Provide a pointer to the transformation estimation object.
void setCorrespondenceEstimation (const CorrespondenceEstimationPtr &ce)
Provide a pointer to the correspondence estimation object.
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)
PointCloudSourceConstPtr const getInputSource ()
Get a pointer to the input point cloud dataset target.
PointCloudTargetConstPtr const getInputTarget ()
Get a pointer to the input point cloud dataset target.
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.
KdTreePtr getSearchMethodTarget () const
Get a pointer to the search method used to find correspondences in the target cloud.
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).
KdTreeReciprocalPtr getSearchMethodSource () const
Get a pointer to the search method used to find correspondences in the source cloud.
Matrix4 getFinalTransformation ()
Get the final transformation matrix estimated by the registration method.
Matrix4 getLastIncrementalTransformation ()
Get the last incremental transformation matrix estimated by the registration method.
void setMaximumIterations (int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
int getMaximumIterations ()
Get the maximum number of iterations the internal optimization should run for, as set by the user.
void setRANSACIterations (int ransac_iterations)
Set the number of iterations RANSAC should run for.
double getRANSACIterations ()
Get the number of iterations RANSAC should run for, as set by the user.
void setRANSACOutlierRejectionThreshold (double inlier_threshold)
Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
double getRANSACOutlierRejectionThreshold ()
Get the inlier distance threshold for the internal outlier rejection loop as set by the user.
void setMaxCorrespondenceDistance (double distance_threshold)
Set the maximum distance threshold between two correspondent points in source <-> target.
double getMaxCorrespondenceDistance ()
Get the maximum distance threshold between two correspondent points in source <-> target.
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.
double getTransformationEpsilon ()
Get the transformation epsilon (maximum allowable translation squared difference between two consecutive transformations) as set by the user.
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.
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).
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.
double getEuclideanFitnessEpsilon ()
Get the maximum allowed distance error before the algorithm will be considered to have converged, as set by the user.
void setPointRepresentation (const PointRepresentationConstPtr &point_representation)
Provide a boost shared pointer to the PointRepresentation to be used when comparing points.
bool registerVisualizationCallback (std::function< UpdateVisualizerCallbackSignature > &visualizerCallback)
Register the user callback function which will be called from registration thread in order to update point cloud obtained after each iteration.
double getFitnessScore (double max_range=std::numeric_limits< double >::max())
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
double getFitnessScore (const std::vector< float > &distances_a, const std::vector< float > &distances_b)
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target) from two sets of correspondence distances (distances between source and target points)
bool hasConverged () const
Return the state of convergence after the last align run.
void align (PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output .
void align (PointCloudSource &output, const Matrix4 &guess)
Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output .
const std::string & getClassName () const
Abstract class get name method.
bool initCompute ()
Internal computation initialization.
bool initComputeReciprocal ()
Internal computation when reciprocal lookup is needed.
void addCorrespondenceRejector (const CorrespondenceRejectorPtr &rejector)
Add a new correspondence rejector to the list.
std::vector< CorrespondenceRejectorPtr > getCorrespondenceRejectors ()
Get the list of correspondence rejectors.
bool removeCorrespondenceRejector (unsigned int i)
Remove the i-th correspondence rejector in the list.
void clearCorrespondenceRejectors ()
Clear the list of correspondence rejectors.
PCLBase ()
Empty constructor.
PCLBase (const PCLBase &base)
Copy constructor.
virtual ~PCLBase ()=default
Destructor.
virtual void setInputCloud (const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloudConstPtr const getInputCloud () const
Get a pointer to the input point cloud dataset.
virtual void setIndices (const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void setIndices (const IndicesConstPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void setIndices (const PointIndicesConstPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void setIndices (std::size_t row_start, std::size_t col_start, std::size_t nb_rows, std::size_t nb_cols)
Set the indices for the points laying within an interest region of the point cloud.
IndicesPtr getIndices ()
Get a pointer to the vector of indices used.
IndicesConstPtr const getIndices () const
Get a pointer to the vector of indices used.
const PointSource & operator[] (std::size_t pos) const
Override PointCloud operator[] to shorten code.
virtual void computeTransformation (PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
void computeTransformation (PointCloudSource &output, const Matrix4 &guess) override
Estimate the transformation and returns the transformed source (input) as output.
void init ()
Initiate covariance voxel structure.
double computeDerivatives (Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud, const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Compute derivatives of likelihood function w.r.t.
double updateDerivatives (Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian=true) const
Compute individual point contributions to derivatives of likelihood function w.r.t.
void computeAngleDerivatives (const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Precompute angular components of derivatives.
void computePointDerivatives (const Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
void computeHessian (Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud)
Compute hessian of likelihood function w.r.t.
void updateHessian (Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const
Compute individual point contributions to hessian of likelihood function w.r.t.
double computeStepLengthMT (const Eigen::Matrix< double, 6, 1 > &transform, Eigen::Matrix< double, 6, 1 > &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, PointCloudSource &trans_cloud)
Compute line search step length and update transform and likelihood derivatives using More-Thuente method.
bool updateIntervalMT (double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t) const
Update interval of possible step lengths for More-Thuente method, in More-Thuente (1994)
double trialValueSelectionMT (double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, double g_t) const
Select new trial value for More-Thuente method.
double auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu=1.e-4) const
Auxiliary function used to determine endpoints of More-Thuente interval.
double auxilaryFunction_dPsiMT (double g_a, double g_0, double mu=1.e-4) const
Auxiliary function derivative used to determine endpoints of More-Thuente interval.
bool searchForNeighbors (const PointCloudSource &cloud, int index, pcl::Indices &indices, std::vector< float > &distances)
Search for the closest nearest neighbor of a given point.
bool initCompute ()
This method should get called before starting the actual computation.
bool deinitCompute ()
This method should get called after finishing the actual computation.
TargetGrid target_cells_
The voxel grid generated from target cloud containing point means and covariances.
float resolution_ {1.0f}
The side length of voxels.
double step_size_ {0.1}
The maximum step length.
double outlier_ratio_ {0.55}
The ratio of outliers of points w.r.t.
double gauss_d1_ {0.0}
The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009].
double gauss_d2_ {0.0}
union {
double trans_probability_
double trans_likelihood_ {0.0}
};
The likelihood score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009].
Eigen::Matrix< double, 8, 4 > angular_jacobian_
Precomputed Angular Gradient.
Eigen::Matrix< double, 15, 4 > angular_hessian_
Precomputed Angular Hessian.
Eigen::Matrix< double, 3, 6 > point_jacobian_
The first order derivative of the transformation of a point w.r.t.
Eigen::Matrix< double, 18, 6 > point_hessian_
The second order derivative of the transformation of a point w.r.t.
std::string reg_name_
The registration method name.
KdTreePtr tree_
A pointer to the spatial search object.
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
int nr_iterations_ {0}
The number of iterations the internal optimization ran for (used internally).
int max_iterations_ {10}
The maximum number of iterations the internal optimization should run for.
int ransac_iterations_ {0}
The number of iterations RANSAC should run for.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
double transformation_epsilon_ {0.0}
The maximum difference between two consecutive transformations in order to consider convergence (user defined).
double transformation_rotation_epsilon_ {0.0}
The maximum rotation difference between two consecutive transformations in order to consider convergence (user defined).
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.
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
double inlier_threshold_ {0.05}
The inlier distance threshold for the internal RANSAC outlier rejection loop.
bool converged_ {false}
Holds internal convergence state, given user parameters.
unsigned int min_number_correspondences_ {3}
The minimum number of correspondences that the algorithm needs before attempting to estimate the transformation.
CorrespondencesPtr correspondences_
The set of correspondences determined at this ICP step.
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud.
std::vector< CorrespondenceRejectorPtr > correspondence_rejectors_
The list of correspondence rejectors to use.
bool target_cloud_updated_ {true}
Variable that stores whether we have a new target cloud, meaning we need to pre-process it again.
bool source_cloud_updated_ {true}
Variable that stores whether we have a new source cloud, meaning we need to pre-process it again.
bool force_no_recompute_ {false}
A flag which, if set, means the tree operating on the target cloud will never be recomputed.
bool force_no_recompute_reciprocal_ {false}
A flag which, if set, means the tree operating on the source cloud will never be recomputed.
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the target point cloud.
PointCloudConstPtr input_
The input point cloud dataset.
IndicesPtr indices_
A pointer to the vector of point indices to use.
bool use_indices_
Set to true if point indices are used.
bool fake_indices_
If no set of indices are given, we construct a set of fake indices that mimic the input PointCloud .
template<typename PointSource, typename PointTarget, typename Scalar = float>
class pcl::NormalDistributionsTransform< PointSource, PointTarget, Scalar >
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 66 of file ndt.h .