41 #ifndef PCL_REGISTRATION_NDT_H_
42 #define PCL_REGISTRATION_NDT_H_
44 #include <pcl/registration/registration.h>
45 #include <pcl/filters/voxel_grid_covariance.h>
47 #include <unsupported/Eigen/NonLinearOptimization>
62 template<
typename Po
intSource,
typename Po
intTarget>
90 typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> >
Ptr;
91 typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> >
ConstPtr;
197 trans = Eigen::Translation<float, 3>(float (x (0)), float (x (1)), float (x (2))) *
198 Eigen::AngleAxis<float>(
float (x (3)), Eigen::Vector3f::UnitX ()) *
199 Eigen::AngleAxis<float>(
float (x (4)), Eigen::Vector3f::UnitY ()) *
200 Eigen::AngleAxis<float>(
float (x (5)), Eigen::Vector3f::UnitZ ());
210 Eigen::Affine3f _affine;
212 trans = _affine.matrix ();
270 Eigen::Matrix<double, 6, 6> &hessian,
272 Eigen::Matrix<double, 6, 1> &p,
273 bool compute_hessian =
true);
285 Eigen::Matrix<double, 6, 6> &hessian,
286 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv,
287 bool compute_hessian =
true);
314 Eigen::Matrix<double, 6, 1> &p);
324 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv);
341 Eigen::Matrix<double, 6, 1> &step_dir,
343 double step_max,
double step_min,
345 Eigen::Matrix<double, 6, 1> &score_gradient,
346 Eigen::Matrix<double, 6, 6> &hessian,
365 double &a_u,
double &f_u,
double &g_u,
366 double a_t,
double f_t,
double g_t);
386 double a_u,
double f_u,
double g_u,
387 double a_t,
double f_t,
double g_t);
401 return (f_a - f_0 - mu * g_0 * a);
414 return (g_a - mu * g_0);
461 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
467 #include <pcl/registration/impl/ndt.hpp>
469 #endif // PCL_REGISTRATION_NDT_H_
void setStepSize(double step_size)
Set/change the newton line search maximum step length.
virtual ~NormalDistributionsTransform()
Empty destructor.
Eigen::Vector3d h_ang_b2_
int getFinalNumIteration() const
Get the number of iterations required to calculate alignment.
double getOulierRatio() const
Get the point cloud outlier ratio.
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.
PointCloudTarget::Ptr PointCloudTargetPtr
PointIndices::Ptr PointIndicesPtr
bool updateIntervalMT(double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t)
Update interval of possible step lengths for More-Thuente method, in More-Thuente (1994) ...
Eigen::Vector3d h_ang_f1_
Eigen::Vector3d h_ang_e3_
Eigen::Vector3d h_ang_e2_
double trans_probability_
The probability score of the transform applied to the input cloud, Equation 6.9 and 6...
Eigen::Vector3d h_ang_b3_
Eigen::Vector3d h_ang_d3_
double computeStepLengthMT(const Eigen::Matrix< double, 6, 1 > &x, Eigen::Matrix< double, 6, 1 > &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, PointCloudSource &trans_cloud)
Compute line search step length and update transform and probability derivatives using More-Thuente m...
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
Eigen::Vector3d h_ang_e1_
Eigen::Vector3d h_ang_f2_
boost::shared_ptr< ::pcl::PointIndices > Ptr
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output. ...
Eigen::Vector3d j_ang_a_
Precomputed Angular Gradient.
double outlier_ratio_
The ratio of outliers of points w.r.t.
Eigen::Vector3d h_ang_f3_
TargetGrid::LeafConstPtr TargetGridLeafConstPtr
Typename of const pointer to searchable voxel grid leaf.
void init()
Initiate covariance voxel structure.
boost::shared_ptr< const NormalDistributionsTransform< PointSource, PointTarget > > ConstPtr
A 3D Normal Distribution Transform registration implementation for point cloud data.
TargetGrid * TargetGridPtr
Typename of pointer to searchable voxel grid.
void computeAngleDerivatives(Eigen::Matrix< double, 6, 1 > &p, bool compute_hessian=true)
Precompute anglular components of derivatives.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Eigen::Vector3d h_ang_a3_
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
double auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu=1.e-4)
Auxilary function used to determin endpoints of More-Thuente interval.
double step_size_
The maximum step length.
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Eigen::Matrix4f &trans)
Convert 6 element transformation vector to transformation matrix.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
PointCloudSource::ConstPtr PointCloudSourceConstPtr
VoxelGridCovariance< PointTarget > TargetGrid
Typename of searchable voxel grid containing mean and covariance.
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Eigen::Affine3f &trans)
Convert 6 element transformation vector to affine transformation.
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.
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.
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
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.
TargetGrid target_cells_
The voxel grid generated from target cloud containing point means and covariances.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
void computePointDerivatives(Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
double getStepSize() const
Get the newton line search maximum step length.
PointCloudSource::Ptr PointCloudSourcePtr
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.
void setResolution(float resolution)
Set/change the voxel grid resolution.
PointIndices::ConstPtr PointIndicesConstPtr
NormalDistributionsTransform()
Constructor.
Eigen::Vector3d h_ang_d1_
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Eigen::Matrix< double, 18, 6 > point_hessian_
The second order derivative of the transformation of a point w.r.t.
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009].
float resolution_
The side length of voxels.
Registration< PointSource, PointTarget >::PointCloudTarget PointCloudTarget
double getTransformationProbability() const
Get the registration alignment probability.
Registration represents the base registration class for general purpose, ICP-like methods...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
const TargetGrid * TargetGridConstPtr
Typename of const pointer to searchable voxel grid.
Eigen::Matrix< double, 3, 6 > point_gradient_
The first order derivative of the transformation of a point w.r.t.
Eigen::Vector3d h_ang_c3_
boost::shared_ptr< NormalDistributionsTransform< PointSource, PointTarget > > Ptr
Eigen::Vector3d h_ang_a2_
Precomputed Angular Hessian.
PointCloudConstPtr input_
The input point cloud dataset.
Eigen::Vector3d h_ang_c2_
double auxilaryFunction_dPsiMT(double g_a, double g_0, double mu=1.e-4)
Auxilary function derivative used to determin endpoints of More-Thuente interval. ...
Eigen::Vector3d h_ang_d2_
Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
void setOulierRatio(double outlier_ratio)
Set/change the point cloud outlier ratio.
float getResolution() const
Get voxel grid resolution.