Autoware 中的坐标系问题

首先在ROS中，常用的坐标系包括：map、odom、base_link、base_footprint、base_laser坐标系。
map地图坐标系
一般设该坐标系为固定坐标系（fixed frame），一般与机器人所在的世界坐标系一致。
base_link机器人本体（基座）坐标系
与机器人中心重合，坐标系原点一般为机器人的旋转中心。
base_footprint坐标系
原点为base_link原点在地面的投影，和base_link坐标系存在z轴上的位移。
odom里程计坐标系
这里要区分开odom topic，这是两个概念。一个是坐标系，一个是根据编码器（或者视觉等）计算的里程计。
但是两者也有关系，odom topic 转化的位姿矩阵是odom–>base_link的tf关系。
base_laser激光雷达坐标系
与激光雷达的安装点有关，其与base_link的tf为固定的。

世界坐标系：world；地图坐标系：map；车的坐标系base_link；传感器坐标系：velodyne
坐标系的转换关系：（参考）
1.base_link->velodyne,激光雷达距离机器人中心的相对位置。
2.map->base_link，ndt算法内实时计算的，车在map坐标系中的位置。
3.map->gps，gps为gnss计算出来的实际位置，gps在map坐标系中的位置。
4.world->map，world坐标系下底图，点云，gps将会有同一个起始点。

从world到map的坐标系转换与从base_link到velodyne的坐标系转换是固定的，用ROS的TF即可。
从map到base_link的映射就需要scan-to-map的算法，autoware在建图采用的是ndt matching。

scan-to-map
已知上一时刻的的栅格地图submap，将scan得到的数据映射到submap中，和submap中的已知点匹配，匹配度最高的情况下的位姿即为当前机器人的位姿。在匹配时，一般使用里程计给出估计位姿，在估计位姿附近去搜索最佳位姿。最终将最佳位姿下的scan数据加入到submap中！

NDT(Normal-Distributions Transform)(参考文章)
autoware的激光点云配准算法是NDT(Normal-Distributions Transform)，相比ICP算法，它能更快速高效地确定两个大型点云的刚性变换。
(1)用第一帧laser scan，按照上述过程计算初始NDT分布
(2)初始化位姿变换参数T，全零或者用odometry初始化
(3)根据位姿变换参数T，将第二帧的laser scan点云转换到第一帧坐标系下
(4)为第二帧的每个2D点分配网格，计算点云对应的四种概率分布
(5)累计点云的概率分布，计算总体得分
(6)用牛顿法优化参数T (任意非线性优化算法均可)，使得score得分最高 [对于牛顿法，迭代步长的要点是求score函数的一阶和二阶偏导数，此处不扩展]
(7)回到步骤(3)，重复上述步骤，优化算法收敛

---