激光雷达slam之LOAM中的坐标转换与IMU融合

需要用到的一些知识和假设：

（1）  来源于 github中的讨论：

由于IMU累积推算位置的误差大，程序中粗略地计算了IMU的位置漂移。

_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

上式成立的前提是认为一个扫描周期内，Lidar的运动是匀速的，上式计算出了非线性误差部分。

 

（2） X、Y、Z轴对应俯仰（pitch）、航向（yaw）、横滚（roll）机动，可知Lidar坐标系为“右下前”坐标系。

 

（3）  从Lidar系到global IMU系，类似于惯导系统中的C（b->n）,即载体系到地理系的转换。

旋转顺序为：横滚->俯仰->航向

rotateZXY(point, roll, pitch, yaw);

从global IMU系到Lidar系，旋转顺序正好相反。

rotateYXZ(point, -yaw, -pitch, -roll);

 

（4）  transform代表将k时刻的点云转换到k+1时刻下，与视觉slam中的相对位姿定义相同。