slam入门2：轮式里程计与2D激光联合标定及里程计内参校准

一、参考论文（同一个作者的两篇论文）：

1、Simultaneous maximum-likelihood calibration of odometry and sensor parameters （2008）

2、Simultaneous Calibration of Odometry and Sensor Parameters for Mobile Robots （2013）

二、开源实现：开源C++实现

三、论文简介：论文以差动模型为例（两轮）并假设里程计坐标系与机器人坐标系相同，以激光传感器估计的位姿为基准，使用激光雷达估计出的位姿变化（delta_pose）对里程计位姿变化进行优化并求解里程计的轮距和左右轮半径，并计算得出激光与里程计的位姿变换T（x, y, yaw）。具体原理如下图：

               

上图中， 为激光传感器位姿变化（是在上一帧自身坐标系下，不是在世界坐标系下，也不是在机器人坐标系下），为激光相对于机器人坐标系的变换（即激光在机器人坐标系下的位姿）， 为机器人的位姿变化（同理，是在上一帧自身坐标系下，不是在世界坐标系下）， 为机器人相对于世界坐标系的变换（即机器人在世界坐标系下的位姿）。

通过优化上述方程达到最小误差来分别求解出里程计的轮距，左右轮半径和激光相对于里程计的位姿变换。

四、注意点：

1、论文以激光pose为准，那么就要求激光pose尽量能准确估计出机器人的运动变化，所以最好在建完图之后取全局优化之后的激光pose（关键帧数据太少的话，建议取全部帧激光数据），此时的激光pose已经经过大闭环和全局优化了，位姿比较精确。

2、里程计数据直接取原始数据就行（比如里程计ticks或者累积位移也可以）。

3、在将数据喂给算法之前，要将激光pose数据与里程计数据进行时间戳对齐。

4、标定数据采集方法：原地左右各转三圈，然后向前直行3m (或5m，或10m)，然后原地旋转180°直行回到原地。原地旋转是为了标轮距，直行为了标轮半径。