|
Point Cloud Library (PCL) 1.12.1
|
A 3D Normal Distribution Transform registration implementation for point cloud data. More...
#include <pcl/registration/ndt.h>
Public Member Functions | |
| NormalDistributionsTransform () | |
| Constructor. | |
| ~NormalDistributionsTransform () | |
| 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. | |
| 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 | getOulierRatio () const |
| Get the point cloud outlier ratio. | |
| void | setOulierRatio (double outlier_ratio) |
| Set/change the point cloud outlier ratio. | |
| double | getTransformationProbability () const |
| Get the registration alignment probability. | |
| int | getFinalNumIteration () const |
| Get the number of iterations required to calculate alignment. | |
| Public Member Functions inherited from pcl::Registration< PointSource, PointTarget, Scalar > | |
| Registration () | |
| Empty constructor. | |
| ~Registration () | |
| 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. | |
| Public Member Functions inherited from pcl::PCLBase< PointSource > | |
| PCLBase () | |
| Empty 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. | |
| IndicesPtr | getIndices () |
| Get a pointer to the vector of indices used. | |
| const PointSource & | operator[] (std::size_t pos) const |
| Override PointCloud operator[] to shorten code. | |
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. | |
| static void | convertTransform (const Eigen::Matrix< double, 6, 1 > &x, Eigen::Matrix4f &trans) |
| Convert 6 element transformation vector to transformation matrix. | |
Protected Types | |
| using | PointCloudSource |
| using | PointCloudSourcePtr = typename PointCloudSource::Ptr |
| using | PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr |
| using | PointCloudTarget |
| using | PointCloudTargetPtr = typename PointCloudTarget::Ptr |
| using | PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr |
| using | PointIndicesPtr = PointIndices::Ptr |
| using | PointIndicesConstPtr = PointIndices::ConstPtr |
| using | TargetGrid = VoxelGridCovariance<PointTarget> |
| Typename of searchable voxel grid containing mean and covariance. | |
| using | TargetGridPtr = TargetGrid* |
| Typename of pointer to searchable voxel grid. | |
| using | TargetGridConstPtr = const TargetGrid* |
| Typename of const pointer to searchable voxel grid. | |
| using | TargetGridLeafConstPtr = typename TargetGrid::LeafConstPtr |
| Typename of const pointer to searchable voxel grid leaf. | |
Protected Member Functions | |
| virtual void | computeTransformation (PointCloudSource &output) |
| Estimate the transformation and returns the transformed source (input) as output. | |
| void | computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &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 probability 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 contirbutions to derivatives of probability function w.r.t. | |
| void | computeAngleDerivatives (const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true) |
| Precompute anglular 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 probability function w.r.t. | |
| void | computeHessian (Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud, const Eigen::Matrix< double, 6, 1 > &transform) |
| Compute hessian of probability 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 contirbutions to hessian of probability 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 probability 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, ![]() | |
| 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. | |
| Protected Member Functions inherited from pcl::Registration< PointSource, PointTarget, Scalar > | |
| bool | searchForNeighbors (const PointCloudSource &cloud, int index, pcl::Indices &indices, std::vector< float > &distances) |
| Search for the closest nearest neighbor of a given point. | |
| virtual void | computeTransformation (PointCloudSource &output, const Matrix4 &guess)=0 |
| Abstract transformation computation method with initial guess. | |
| Protected Member Functions inherited from pcl::PCLBase< PointSource > | |
| bool | initCompute () |
| This method should get called before starting the actual computation. | |
| bool | deinitCompute () |
| This method should get called after finishing the actual computation. | |
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. | |
| double | gauss_d1_ |
| The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. | |
| 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]. | |
| 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. | |
| Protected Attributes inherited from pcl::Registration< PointSource, PointTarget, Scalar > | |
| 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_ |
| The number of iterations the internal optimization ran for (used internally). | |
| int | max_iterations_ |
| The maximum number of iterations the internal optimization should run for. | |
| int | ransac_iterations_ |
| 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_ |
| The maximum difference between two consecutive transformations in order to consider convergence (user defined). | |
| double | transformation_rotation_epsilon_ |
| 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_ |
| The inlier distance threshold for the internal RANSAC outlier rejection loop. | |
| bool | converged_ |
| Holds internal convergence state, given user parameters. | |
| int | min_number_correspondences_ |
| 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_ |
| Variable that stores whether we have a new target cloud, meaning we need to pre-process it again. | |
| bool | source_cloud_updated_ |
| Variable that stores whether we have a new source cloud, meaning we need to pre-process it again. | |
| bool | force_no_recompute_ |
| A flag which, if set, means the tree operating on the target cloud will never be recomputed. | |
| bool | force_no_recompute_reciprocal_ |
| 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. | |
| Protected Attributes inherited from pcl::PCLBase< PointSource > | |
| 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. | |
A 3D Normal Distribution Transform registration implementation for point cloud data.
| using pcl::NormalDistributionsTransform< PointSource, PointTarget >::ConstPtr |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
| using pcl::NormalDistributionsTransform< PointSource, PointTarget >::Ptr = shared_ptr<NormalDistributionsTransform<PointSource, PointTarget>> |
|
protected |
|
protected |
|
protected |
|
protected |
| pcl::NormalDistributionsTransform< PointSource, PointTarget >::NormalDistributionsTransform | ( | ) |
Constructor.
Sets outlier_ratio_ to 0.35, step_size_ to 0.05 and resolution_ to 1.0
Definition at line 47 of file ndt.hpp.
References gauss_d1_, gauss_d2_, pcl::Registration< PointSource, PointTarget, Scalar >::max_iterations_, outlier_ratio_, pcl::Registration< PointSource, PointTarget, Scalar >::reg_name_, resolution_, step_size_, target_cells_, trans_probability_, and pcl::Registration< PointSource, PointTarget, Scalar >::transformation_epsilon_.
|
inline |
|
inlineprotected |
Auxiliary function derivative used to determine endpoints of More-Thuente interval.

| [in] | g_a | function gradient at step length a, ![]() |
| [in] | g_0 | initial function gradient, ![]() |
| [in] | mu | the step length, constant ![]() |
Definition at line 463 of file ndt.h.
Referenced by computeStepLengthMT().
|
inlineprotected |
Auxiliary function used to determine endpoints of More-Thuente interval.

| [in] | a | the step length, ![]() |
| [in] | f_a | function value at step length a, ![]() |
| [in] | f_0 | initial function value, ![]() |
| [in] | g_0 | initial function gradient, ![]() |
| [in] | mu | the step length, constant ![]() |
Definition at line 449 of file ndt.h.
Referenced by computeStepLengthMT().
|
protected |
Precompute anglular components of derivatives.
| [in] | transform | the current transform vector |
| [in] | compute_hessian | flag to calculate hessian, unnessissary for step calculation. |
Definition at line 235 of file ndt.hpp.
References angular_hessian_, and angular_jacobian_.
Referenced by computeDerivatives().
|
protected |
Compute derivatives of probability function w.r.t.
the transformation vector.
| [out] | score_gradient | the gradient vector of the probability function w.r.t. the transformation vector |
| [out] | hessian | the hessian matrix of the probability function w.r.t. the transformation vector |
| [in] | trans_cloud | transformed point cloud |
| [in] | transform | the current transform vector |
| [in] | compute_hessian | flag to calculate hessian, unnessissary for step calculation. |
Definition at line 184 of file ndt.hpp.
References computeAngleDerivatives(), computePointDerivatives(), pcl::PCLBase< PointSource >::input_, resolution_, target_cells_, and updateDerivatives().
Referenced by computeStepLengthMT(), and computeTransformation().
|
protected |
Compute hessian of probability function w.r.t.
the transformation vector.
| [out] | hessian | the hessian matrix of the probability function w.r.t. the transformation vector |
| [in] | trans_cloud | transformed point cloud |
Definition at line 414 of file ndt.hpp.
References computePointDerivatives(), pcl::PCLBase< PointSource >::input_, resolution_, target_cells_, and updateHessian().
Referenced by computeHessian(), and computeStepLengthMT().
|
inlineprotected |
Compute hessian of probability function w.r.t.
the transformation vector.
| [out] | hessian | the hessian matrix of the probability function w.r.t. the transformation vector |
| [in] | trans_cloud | transformed point cloud |
| [in] | transform | the current transform vector |
Definition at line 328 of file ndt.h.
References computeHessian(), and pcl::utils::ignore().
|
protected |
Compute point derivatives.
| [in] | x | point from the input cloud |
| [in] | compute_hessian | flag to calculate hessian, unnessissary for step calculation. |
Definition at line 320 of file ndt.hpp.
References angular_hessian_, angular_jacobian_, point_hessian_, and point_jacobian_.
Referenced by computeDerivatives(), and computeHessian().
|
protected |
Compute line search step length and update transform and probability derivatives using More-Thuente method.
| [in] | transform | initial transformation vector, ![]() ![]() |
| [in] | step_dir | descent direction, ![]() ![]() |
| [in] | step_init | initial step length estimate, ![]() ![]() |
| [in] | step_max | maximum step length, ![]() |
| [in] | step_min | minimum step length, ![]() |
| [out] | score | final score function value, ![]() ![]() |
| [in,out] | score_gradient | gradient of score function w.r.t. transformation vector, ![]() ![]() |
| [out] | hessian | hessian of score function w.r.t. transformation vector, ![]() ![]() |
| [in,out] | trans_cloud | transformed point cloud, ![]() ![]() |
Definition at line 649 of file ndt.hpp.
References auxilaryFunction_dPsiMT(), auxilaryFunction_PsiMT(), computeDerivatives(), computeHessian(), convertTransform(), pcl::Registration< PointSource, PointTarget, Scalar >::final_transformation_, pcl::PCLBase< PointSource >::input_, pcl::transformPointCloud(), trialValueSelectionMT(), and updateIntervalMT().
Referenced by computeTransformation().
|
inlineprotectedvirtual |
Estimate the transformation and returns the transformed source (input) as output.
| [out] | output | the resultant input transformed point cloud dataset |
Definition at line 238 of file ndt.h.
References computeTransformation().
Referenced by computeTransformation().
|
overrideprotected |
Estimate the transformation and returns the transformed source (input) as output.
| [out] | output | the resultant input transformed point cloud dataset |
| [in] | guess | the initial gross estimation of the transformation |
Definition at line 73 of file ndt.hpp.
References computeDerivatives(), computeStepLengthMT(), pcl::Registration< PointSource, PointTarget, Scalar >::converged_, convertTransform(), pcl::Registration< PointSource, PointTarget, Scalar >::final_transformation_, gauss_d1_, gauss_d2_, pcl::PCLBase< PointSource >::input_, pcl::Registration< PointSource, PointTarget, Scalar >::max_iterations_, pcl::Registration< PointSource, PointTarget, Scalar >::nr_iterations_, outlier_ratio_, point_hessian_, point_jacobian_, pcl::Registration< PointSource, PointTarget, Scalar >::previous_transformation_, resolution_, step_size_, pcl::Registration< PointSource, PointTarget, Scalar >::target_, trans_probability_, pcl::Registration< PointSource, PointTarget, Scalar >::transformation_, pcl::Registration< PointSource, PointTarget, Scalar >::transformation_epsilon_, pcl::Registration< PointSource, PointTarget, Scalar >::transformation_rotation_epsilon_, pcl::transformPointCloud(), and pcl::Registration< PointSource, PointTarget, Scalar >::update_visualizer_.
|
inlinestatic |
Convert 6 element transformation vector to affine transformation.
| [in] | x | transformation vector of the form [x, y, z, roll, pitch, yaw] |
| [out] | trans | affine transform corresponding to given transfomation vector |
Definition at line 194 of file ndt.h.
Referenced by computeStepLengthMT(), computeTransformation(), and convertTransform().
|
inlinestatic |
Convert 6 element transformation vector to transformation matrix.
| [in] | x | transformation vector of the form [x, y, z, roll, pitch, yaw] |
| [out] | trans | 4x4 transformation matrix corresponding to given transfomation vector |
Definition at line 208 of file ndt.h.
References convertTransform().
|
inline |
Get the number of iterations required to calculate alignment.
Definition at line 184 of file ndt.h.
References pcl::Registration< PointSource, PointTarget, Scalar >::nr_iterations_.
|
inline |
Get the point cloud outlier ratio.
Definition at line 157 of file ndt.h.
References outlier_ratio_.
|
inline |
Get voxel grid resolution.
Definition at line 130 of file ndt.h.
References resolution_.
|
inline |
Get the newton line search maximum step length.
Definition at line 139 of file ndt.h.
References step_size_.
|
inline |
Get the registration alignment probability.
Definition at line 175 of file ndt.h.
References trans_probability_.
|
inlineprotected |
Initiate covariance voxel structure.
Definition at line 252 of file ndt.h.
References resolution_, pcl::Registration< PointSource, PointTarget, Scalar >::target_, and target_cells_.
Referenced by setInputTarget(), and setResolution().
|
inlineoverridevirtual |
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).
| [in] | cloud | the input point cloud target |
Reimplemented from pcl::Registration< PointSource, PointTarget, Scalar >.
Definition at line 105 of file ndt.h.
References init(), and pcl::Registration< PointSource, PointTarget, Scalar >::setInputTarget().
|
inline |
Set/change the point cloud outlier ratio.
| [in] | outlier_ratio | outlier ratio |
Definition at line 166 of file ndt.h.
References outlier_ratio_.
|
inline |
Set/change the voxel grid resolution.
| [in] | resolution | side length of voxels |
Definition at line 115 of file ndt.h.
References init(), pcl::PCLBase< PointSource >::input_, and resolution_.
|
inline |
Set/change the newton line search maximum step length.
| [in] | step_size | maximum step length |
Definition at line 148 of file ndt.h.
References step_size_.
|
protected |
Select new trial value for More-Thuente method.






| [in] | a_l | first endpoint of interval ![]() ![]() |
| [in] | f_l | value at first endpoint, ![]() |
| [in] | g_l | derivative at first endpoint, ![]() |
| [in] | a_u | second endpoint of interval ![]() ![]() |
| [in] | f_u | value at second endpoint, ![]() |
| [in] | g_u | derivative at second endpoint, ![]() |
| [in] | a_t | previous trial value, ![]() |
| [in] | f_t | value at previous trial value, ![]() |
| [in] | g_t | derivative at previous trial value, ![]() |
Definition at line 536 of file ndt.hpp.
Referenced by computeStepLengthMT().
|
protected |
Compute individual point contirbutions to derivatives of probability function w.r.t.
the transformation vector.
| [in,out] | score_gradient | the gradient vector of the probability function w.r.t. the transformation vector |
| [in,out] | hessian | the hessian matrix of the probability function w.r.t. the transformation vector |
| [in] | x_trans | transformed point minus mean of occupied covariance voxel |
| [in] | c_inv | covariance of occupied covariance voxel |
| [in] | compute_hessian | flag to calculate hessian, unnessissary for step calculation. |
Definition at line 366 of file ndt.hpp.
References gauss_d1_, gauss_d2_, point_hessian_, and point_jacobian_.
Referenced by computeDerivatives().
|
protected |
Compute individual point contirbutions to hessian of probability function w.r.t.
the transformation vector.
| [in,out] | hessian | the hessian matrix of the probability function w.r.t. the transformation vector |
| [in] | x_trans | transformed point minus mean of occupied covariance voxel |
| [in] | c_inv | covariance of occupied covariance voxel |
Definition at line 456 of file ndt.hpp.
References gauss_d1_, gauss_d2_, point_hessian_, and point_jacobian_.
Referenced by computeHessian().
|
protected |
Update interval of possible step lengths for More-Thuente method, 


| [in,out] | a_l | first endpoint of interval ![]() ![]() |
| [in,out] | f_l | value at first endpoint, ![]() ![]() ![]() |
| [in,out] | g_l | derivative at first endpoint, ![]() ![]() ![]() |
| [in,out] | a_u | second endpoint of interval ![]() ![]() |
| [in,out] | f_u | value at second endpoint, ![]() ![]() ![]() |
| [in,out] | g_u | derivative at second endpoint, ![]() ![]() ![]() |
| [in] | a_t | trial value, ![]() |
| [in] | f_t | value at trial value, ![]() ![]() ![]() |
| [in] | g_t | derivative at trial value, ![]() ![]() ![]() |
Definition at line 491 of file ndt.hpp.
Referenced by computeStepLengthMT().
|
protected |
Precomputed Angular Hessian.
The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
Definition at line 502 of file ndt.h.
Referenced by computeAngleDerivatives(), and computePointDerivatives().
|
protected |
Precomputed Angular Gradient.
The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].
Definition at line 495 of file ndt.h.
Referenced by computeAngleDerivatives(), and computePointDerivatives().
|
protected |
The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009].
Definition at line 484 of file ndt.h.
Referenced by computeTransformation(), NormalDistributionsTransform(), updateDerivatives(), and updateHessian().
|
protected |
Definition at line 484 of file ndt.h.
Referenced by computeTransformation(), NormalDistributionsTransform(), updateDerivatives(), and updateHessian().
|
protected |
The ratio of outliers of points w.r.t.
a normal distribution, Equation 6.7 [Magnusson 2009].
Definition at line 480 of file ndt.h.
Referenced by computeTransformation(), getOulierRatio(), NormalDistributionsTransform(), and setOulierRatio().
|
protected |
The second order derivative of the transformation of a point w.r.t.
the transform vector, 
Definition at line 510 of file ndt.h.
Referenced by computePointDerivatives(), computeTransformation(), updateDerivatives(), and updateHessian().
|
protected |
The first order derivative of the transformation of a point w.r.t.
the transform vector, 
Definition at line 506 of file ndt.h.
Referenced by computePointDerivatives(), computeTransformation(), updateDerivatives(), and updateHessian().
|
protected |
The side length of voxels.
Definition at line 473 of file ndt.h.
Referenced by computeDerivatives(), computeHessian(), computeTransformation(), getResolution(), init(), NormalDistributionsTransform(), and setResolution().
|
protected |
The maximum step length.
Definition at line 476 of file ndt.h.
Referenced by computeTransformation(), getStepSize(), NormalDistributionsTransform(), and setStepSize().
|
protected |
The voxel grid generated from target cloud containing point means and covariances.
Definition at line 470 of file ndt.h.
Referenced by computeDerivatives(), computeHessian(), init(), and NormalDistributionsTransform().
|
protected |
The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009].
Definition at line 488 of file ndt.h.
Referenced by computeTransformation(), getTransformationProbability(), and NormalDistributionsTransform().