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
到這裡，關於座標系變換的基礎理論就講明白了。