激光雷达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中的相对位姿定义相同。
坐标转换与IMU融合
1、 transformToStartIMUskillfully
注册点云时（MultiScanRegistration.cpp中），当判断有IMU数据时，会进⾏⼀步坐标转换的预处理，体现在函数transformToStartIMU中。这个函数进⾏了三步处理：
美国总统辩论（1） rotateZXY(point, _ll, _imuCur.pitch, _imuCur.yaw);
将原始点云从当前Lidar系转换到global IMU系下；
（2）补偿了_imuPositionShift，也即估算的IMU位置漂移；
（3） rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_ll);
将global IMU系下的点云转换到Start时刻的Lidar系下。
经过这个函数的处理，点云的position部分处于当前Lidar系下⼀个相对准确的位置上（基于扫描周期内匀速运动的假设），但点云的Rotation部分是Start时刻Lidar系下观察所得的，⽽⾮处于当前Lidar系下。更清晰地来说，即此时观察到的点云坐标，是以当前Lidar的坐标（⼀个估计值）为原点，⽽坐标轴是与Start时刻的Lidar系的坐标轴对齐的。
2、 OD初始化：
adverly根据第⼀次开始扫描时的IMU pitch与roll，作为累积位姿的初始值。
__x += _imuPitchStart;
__z += _imuRollStart;
长沙编程培训
Yaw⾓度和pos部分都未赋初值，即假设开始时刻的偏航⾓为0，位于global系下的原点位置。
3、运动估计初值：
南昌电脑培训万物网（1） _transform.pos -= _imuVeloFromStart * _scanPeriod;
其中，imuVeloFromStart的计算，可知imuVeloFromStart为Start时刻Lidar系下的速度变化⽮量：
imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;
rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_ll);
对于匀速运动假设的⼀个补偿，并且基于运动曲线的连续性，做了递推形式的计算，可能乘以1/2会更合适？
（2） _transform的rotation部分未赋初值，认为为0。
4、 transformToStart
长春日语培训在进⾏KDTree最近点搜索前，⾸先将进⾏畸变处理后的点云转换到每⼀次扫描的开始时刻。
先根据匀速运动假设计算出当前点时刻Lidar的位移和旋转。
但这⾥其实是和前⾯的转换transformToStartIMU有些冲突的。因为transformToStartIMU之后点云所处的坐标轴是与Start时刻的Lidar系的坐标轴对齐的。现在⼜通过_transform将点云转换到start时刻。有⼀种转了两次的感觉。
有⼀种解释：认为k次扫描中的旋转部分⼤部分由IMU积分获得，_transform中的旋转估算的只是⼀个⼩量。
5、 transformToEnd
（1）先进⾏transformToStart，此时点云处于start时刻的Lidar系下；在线翻译网页
（2）通过_transform转换到end时刻的Lidar系下；
（3） rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart);
转换到global系下
（4） rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd);
转换到end时刻的Lidar系下。
总结点云的旋转过程从1->5, 可⽤公式表⽰为：
nsf
6、accumulateRotationwhy not
该函数的作⽤是将计算的两帧之间的位姿“累加”起来，获得相对于第⼀帧的旋转矩阵，具体公式如下：
7、 pluginIMURotation
该函数与accumulateRotation，联合起来完成了更新_transformSum的rotation部分的⼯作。该函数可视为transformToEnd的下部分的逆过程。具体公式如下：
