Lego-LOAM IMU坐标系变换的详细记录

最近看了Lego-LOAM 的IMU部分，没看懂IMU的坐标系变换。看其它的博客，看的让我更混淆了，因此决定抛开其他人的理解，故写下这篇博客。

0 基础知识

1. IMU的数据欧拉角表示的时从当前时刻的坐标系到初始坐标系的旋转关系（内旋变换），且旋转顺序为RPY( yaw(z) - > pitch(y) -> roll(x) ), 即：
   R = R(yaw)*R(pitch)*R(roll)；
2. IMU的欧拉角的值旋转顺时针（从负轴看向正轴）为负，本人实测。这也解释了第一点为什么IMU的欧拉角表示的是从当前时刻的坐标系到初始坐标系的原因。
3. 旋转矩阵 R = R(yaw)*R(pitch)*R(roll) -----------------------------------------------------------------------(1)
   

5 IMU坐标系方向：IMU RPY顺时针为正，加速度计

1. IMU 重力加速度消除

首先，一般重力加速度再Z轴为正，至少本人IMU测试重力加速度再Z轴为正。有了上面的基础，再来看IMU的重力加速度消除就简单，减去重力加速度在xyz轴的分量即可。
1.1 假设在初始坐标系下IMU的重力加速度在x，y，z方向的分量表示为ginit=[0; 0 ; 9.8]
1.2 Rinit_imu =R(yaw)R(pitch)R(roll)表示从当前时刻的IMU坐标系转换到初始坐标系
1.3 因此有，ginit = Rinit_imu * gimu，可得重力加速度在当前IMU坐标系下的xyz重力分量为：
               gimu = R^Tinit_imu * ginit
                 = [-9.8sin(pitch); 9.8sin(roll)cos(pitch); 9.8*cos(roll)cos(pitch)];
1.4 IMU数据减去重力分量，并且转换到相机坐标系下：

[accZ; accX; accY] = [imu_acc_x; imu_accy; imu_acc_z] - g imu

旋转到相机坐标系下，RPY和相机坐标系的关系：

2 相机坐标系(camera)到初始坐标系(camera_init)的转换

上面我们将IMU转换到相机坐标系下了。在Lego-LOAM 中计算位移和速度以初始相机坐标系（camera_init）为参考，因此需要转换到初始相机坐标系（camera_init）。
2.1 从当前时刻的相机坐标系转换到初始相机坐标系的旋转关系，应该和IMU的旋转关系一致，即

R init_camera = R init_imu

acc_wrold = R init_camera * [accZ; accX; accY] = R init_imu *( [imu_acc_x; imu_accy; imu_acc_z] - g imu );

  上面等式也描述了为什么两个旋转矩阵相等，右边是IMU的旋转到初始IMU坐标系位置，左边是相机坐标系旋转到初始坐标系位置。并且从相机坐标系变换会初始坐标系时不是：z(roll) --> y(yaw)–>x(pitch)，而是(y_cam)yaw -> (x_cam)pitch -> (z_cam)roll，即：
    acc_wrold = R2init_camera * [accX; accY; accZ];
    R2init_camera = R(yaw)*R(pitch)*R(roll) = R(y_cam)*R(x_cam)*R(z_cam)

2.2 最后将上述等式乘起来就是Lego-LOAM中AccumulateIMUShiftAndRotation()的坐标系变换。
2.3 Lego-LOAM旋转变换都是遵循RPY(Y->P->R)顺序，只是Yaw 对应的到底是R(x),R(y),R(z)的哪个旋转矩阵，要根据IMU的RPY坐标系和待旋转坐标轴确定。例如在相机坐标系下yaw对应的就是y轴，因此对应R(y)的旋转矩阵。明白了这些，在Lego-LOAM中其他地方的IMU坐标变换便迎刃而解。