基于liosam先验地图与NDT配准的全局重定位的一些个人理解

全局重定位：在有先验地图的前提条件下，将机器人重新投入先验地图对应的环境中时，通过采集实时点云，与先验地图做配准，从而知道机器人在全局地图下自身的位姿。

首先，我们做全局重定位第一步就是需要得到先验地图，项目中我们采用的是用Lio-sam开源算法做的全局先验地图，然后采用NDT来做点云配准。

具体步骤如下：

1.首先在做配准之前判断有没有先验地图，有就将其作为目标点云来填充NDT，顺便填充一下NDT中其他参数，例如步长什么的（NDT算法直接调用pcl库）。

2.得到当前实时帧点云scan1后，首先体素降采样，删除某些距离有问题的点或者NaN点，做个预处理，得到scan2。

3.由于我们此时得到的点云位姿是在lidar坐标系(lidar_frame)下的位姿，但是我们全局重定位核心就是:求出世界坐标系(map_frame)下的base_link坐标（即让map_frame下的点云与base_link_frame下的点云做配准得到转换矩阵），因此我们要将原属于lidar_frame的点云通过TF转换（lidar->base_link，一般来说这是个固定矩阵）转到base_link_frame下得到scan3（注意这里的scan2与scan3其实点云都是一模一样的，只不过是坐标系变了），接着将scan3作为源点云来做填充NDT。

4.NDT匹配一般来说是需要当前帧的初始估计位姿intial_guess（为了结果更精确），初始估计位姿的确定也是通过Lio-sam算法得到，从而通过NDT匹配得到scan3到先验地图的转换矩阵。

5.接下来这点纯属于个人理解：先说结论，如果世界坐标系下的原点位姿为（0，0，0，0，0，0，）这里得到的转换矩阵就为当前实时帧的位姿矩阵。点云配准的实质就是坐标系的转换，将A坐标系下的点云转换到B坐标系下，就可以得到A到B的转换矩阵，同理，我们这里是将base_link坐标系下的点云转换到世界坐标系下，世界坐标系原点我们这里定义的为（0，0，0，0，0，0），因此我们此时得到的旋转矩阵就是base_link在世界坐标系下的位姿矩阵。

以上都属于个人理解（特别是第五点），不知道是否正确，如有不对的地方望指正，欢迎评论区讨论，由于代码涉及到项目问题本人就不公开了，感谢理解。

补充：第4点中有一个当前帧的初始估计位姿，通过实时帧与局部地图的scan2Map优化得到的初始估计位姿