pcl NDT配准

一、配准

两个机器人坐标系之间的空间映射T由下式给出
（tx，ty）t描述两个帧之间的平移和φ旋转。配准的目的是用两个位置扫描下的激光数据来恢复这些参数。给出两次扫描（第一和第二次扫描）后，用如下方法：
1）创建第一次扫描的NDT（参数化）。
2）初始化参数的估计值（用0或使用里程计数据）。
也就是初始化rt矩阵的参数，并将参数给到3,
3）对于第二次扫描的每个样本：根据参数将构造的点映射到第一次扫描的坐标系中。
4）确定每个映射点的对应正态分布。
5）通过评估每个映射点的分布并求和结果来确定参数的分数。
6）优化计算新参数估计。通过最优化方法牛顿法完成。
7）转到3，直到满足收敛准则。

二、代码

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_new;//初始化
ndt_new.setTransformationEpsilon(trans_epsilon);//设置收敛 0.1- 0.001
ndt_new.setStepSize(step_size);//设置步长 0.1
ndt_new.setResolution(resolution);//设置栅格化大小1-2m最佳
ndt_new.setMaximumIterations(max_iterations);//最大迭代步数50-200
ndt_new.setInputTarget(set_InputTarget);//配准的目标 对准的目标
ndt_newer.setInputSource(set_InputSource);//配准的输入 对准的输入
ndt_newer.align(*transform_points, initial_pose_matrix);//赋初值

结果获取

Eigen::Matrix4f result_pose_matrix = ndt_newer.getFinalTransformation()；//四元数矩阵 位姿变换矩阵
transform_probability = ndt_newer.getTransformationProbability();//概率

三、原理
https://zhuanlan.zhihu.com/p/96908474