loam详细代码解析与公式推导(基础理论知识)
一、基础坐标变换
loam中欧拉角解算都采用R P Y 的解算方式，即先左乘R， 再左乘P， 最后左乘Y，用矩阵表示为： R = Ry * Rp * Rr。用个很简单的示例表示，在坐标系Oxyz中经过旋转后得到坐标系O'xyz，可以把坐标系Oxyz当成全局坐标系{world_link}，坐标系O'xyz当成局部坐标系{local_link}；有局部坐标系上的点Pl，在全局坐标系下的位置Pw是：
Pw = R * Pl = （Ry * Rp * Rr）* Pl
上式将局部坐标系上的点变换到全局坐标系即：
R = Ry * Rp * Rr
{world_link} ------------------------> {link_link}
其中：Rr是agv的翻滚角，与loam坐标系中的rot_x对应
rot_x = [1, 0, 0; 0, crx, -srx; 0 , srx, crx];
Ry是agv的偏航角，与loam坐标系中的rot_y对应
rot_y = [cry, 0, sry; 0, 1, 0; -sry , 0, cry];
Rp是agv的俯仰角，与loam坐标系中的rot_z对应
rot_z = [crz, -srz, 0; srz, crz, 0; 0 , 0, 1];
二、loam对应的欧拉变换顺序
loam中坐标变换通常分为从{link_world}到{world_link}和从{world_link}到{link_world}的变换，其分别对应的欧拉拉变换方式为：Rzxy 和 其逆矩阵inv(Rzxy) = R-y-x-z，其中
Rzxy = rot_z * rot_x * rot_y = [ cry*crz + srx*sry*srz, crz*srx*sry - cry*srz, crx*sry] [crx*srz, crx*crz, -srx] [ cry*srx*srz - crz*sry, sry*srz + cry*crz*srx, crx*cry]
inv(Rzxy) = -rot_y * -rot_x * -rot_z = [ cry*crz + srx*sry*srz, crx*srz, cry*srx*srz - crz*sry] [ crz*srx*sry - cry*srz, crx*crz, sry*srz + cry*crz*srx] [ crx*sry, -srx, crx*cry]
三、imu角度变换计算
loam中imu的坐标系方向是前左上，假设在current的imu坐标系下的各个方向加速度为
A = [ax; ay; az]
在前左上坐标系中欧拉角的解算方式是：Rxyz = rot_z * rot_x * rot_y，现在需要得到重力加速度在current坐标系下各个方向的分量，应为imu开始坐标系下G=[0; 0; -9.8]；现在相当于求全局imu坐标系下的向量在局部imu坐标系下向量值G'，故
G' = inv(Rxyz) * G
其中
inv(Rxyz) = [ cry*crz, cry*srz, -sry] [ crz*srx*sry - crx*srz, crx*crz + srx*sry*srz, cry*srx] [ srx*srz + crx*crz*sry, crx*sry*srz - crz*srx, crx*cry]
因此
A’= A + inv(Rxyz) * G
到这里，关于坐标系变换的基础理论就讲明白了。