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

最新推荐文章于 2024-05-04 09:43:15 发布

豆皮卷香菜

最新推荐文章于 2024-05-04 09:43:15 发布

阅读量1.6k

点赞数 2

分类专栏： slam 文章标签：算法自动驾驶

本文链接：https://blog.csdn.net/l898985121/article/details/114592293

版权

slam 专栏收录该内容

20 篇文章 8 订阅

订阅专栏

文章目录

欧拉角
AccumulateRotation函数
TransformToStartIMU函数

欧拉角

从旋转所相对的坐标系来看，分成两种：
extrinsic：每一次旋转，都相对于固定的基坐标系进行
intrinsic：每一次旋转，都相对于旋转后的新坐标系进行
从旋转轴类型上，分成两类：
Proper Euler angles：有重复旋转的轴
Tait–Bryan angles：没有重复旋转的轴
总结成下表：

从loam的程序来看，可能选择的是上表中 $Y_1X_2Z_3$ 这种类型

AccumulateRotation函数

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

通过下面的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 = ^0_2R \cdot ^2p$

/********************************************************************************
    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;

以下代码实现 $^1p= ^1_0R \cdot ^0_2R \cdot ^2p$

//绕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;