loam中各种旋转各种坐标变换

欧拉角

1. 从旋转所相对的坐标系来看，分成两种：
   extrinsic：每一次旋转，都相对于固定的基坐标系进行
   intrinsic：每一次旋转，都相对于旋转后的新坐标系进行
2. 从旋转轴类型上，分成两类：
   Proper Euler angles：有重复旋转的轴
   Tait–Bryan angles： 没有重复旋转的轴
   总结成下表：
   
   从loam的程序来看，可能选择的是上表中$Y_1{X}_2{Z}_3$这种类型

AccumulateRotation函数

假设基坐标系——也就是第一帧的系为0系，上一帧得到的车辆坐标系为1系，本帧车辆坐标系为2系，本次laserOdometry求解后得到1系和2系的相对旋转关系${}_2^{1}R$。则，本函数，计算的是：
$${}_2^{0}R{=}_1^{0}R{\cdot }_2^{1}R$$

通过下面的matlab符号计算可以进行公式验证

syms cx cy cz lx ly lz ox oy oz R1 R2 R3;
R1 = [cos(cy),0,sin(cy);0,1,0;-sin(cy),0,cos(cy)] * ...
    [1,0,0;0,cos(cx),-sin(cx);0,sin(cx),cos(cx)] * ...
    [cos(cz),-sin(cz),0;sin(cz),cos(cz),0;0,0,1]

R2 = [cos(ly),0,sin(ly);0,1,0;-sin(ly),0,cos(ly)] * ...
    [1,0,0;0,cos(lx),-sin(lx);0,sin(lx),cos(lx)] * ...
    [cos(lz),-sin(lz),0;sin(lz),cos(lz),0;0,0,1]

R3 = R1 * R2

TransformToStartIMU函数

该函数在去畸变函数中，作用是将一个特定时刻的激光点的坐标变换到，当前帧第一个点对应的车辆坐标系下。假设，imu给出的rollpitchyaw角度相对于一个固定的设计坐标系0系。当前激光点对应的车辆坐标系为2系，当前帧第一个激光点对应的车辆坐标系为1系
以下代码实现${}^{0}p{=}_2^{0}R{\cdot }^{2}p$

/********************************************************************************
    Ry*Rx*Rz*Pl, transform point to the global frame
  *********************************************************************************/
  //绕z轴旋转(imuRollCur)
  float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;
  float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
  float z1 = p->z;

  //绕x轴旋转(imuPitchCur)
  float x2 = x1;
  float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
  float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;

  //绕y轴旋转(imuYawCur)
  float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
  float y3 = y2;
  float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;

以下代码实现${}^{1}p{=}_0^{1}R{\cdot }_2^{0}R{\cdot }^{2}p$

//绕y轴旋转(-imuYawStart)
  float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
  float y4 = y3;
  float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;

  //绕x轴旋转(-imuPitchStart)
  float x5 = x4;
  float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
  float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;

  //绕z轴旋转(-imuRollStart)，然后叠加平移量
  p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;
  p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
  p->z = z5 + imuShiftFromStartZCur;