LIO-SAM中extrinsicRot、extQRPY和extrinsicTrans的计算推导

1 预备的知识

1.1 两个基础知识点

• 姿态角对应的旋转轴，如下图所示，也就是yaw航向角绕Z轴旋转，pitch俯仰角绕Y轴旋转，roll翻滚角绕X轴旋转，旋转角的正值均为逆时针(或者让右手大拇指指向旋转轴，然后正方向就是四指弯曲方向)，我更喜欢用大拇指确定旋转角度的正负值。
  
• 两个坐标系中点的变换
  已知1和2两个坐标系，同时有一点$P$，现在已知它在1坐标系下的位置$P_1$,同时已知两个坐标系的变换关系$R_1^2$和$t_1^2$，希望求得$P_2$,其计算公式如下： $$P_2=R_1^2\ast P_1+t_1^2$$ 这里着重介绍一下对$R_1^2$和$t_1^2$的理解。
  请注意，接下来介绍的很重要:其中$R_1^2$的计算是通过固定2坐标系，然后以2坐标系作为固定轴坐标系，通过旋转之后与1坐标系重合的旋转矩阵，但是实现的功能是将1坐标系中的点给转换到了2坐标系下。$t_1^2$的计算是1坐标系的原点在2坐标系下的坐标值。

1.2 对1.1的内容进行举例

这里的题目借用了B站讲解slam十四讲中的博主的例子：B站视频，这里面博主对姿态角讲的很好。
题目：有两个右手系1和2,其中2系的x轴与1系的y轴方向相同，2系的y轴与1系z轴方向相反，2系的z轴与1系的x轴相反,两个坐标系原点重合。求$R_1^2$，并求出1系中(1,1,1)在2系中的坐标。
我这里直接给出答案代码，答案链接:Calculate_LIO-SAM_extrinsicRot_extQRPY。

    // 将2系进行固定轴旋转到1系
    // 始终固定2系，下面的转轴都是以原始的2系作为旋转轴，先绕X转90度，再绕Y转90度，最后得到旋转后的坐标系和1系重合
    // t_1_2是1坐标系的原点在2坐标系下的坐标值，目的是将1系下的点转换到2系下
    
    // 第一步：在2坐标系下围绕X轴旋转90度
    const double angle_x = M_PI_2; // 90度转换为弧度
    AngleAxisd rotation_x(angle_x, Vector3d::UnitX());
    // 第二步：在2坐标系(也就是原始坐标系)下围绕Y轴旋转90度
    const double angle_y = M_PI_2; // 90度转换为弧度
    AngleAxisd rotation_y(angle_y, Vector3d::UnitY());
    Matrix3d final_rotation = rotation_y.toRotationMatrix() * rotation_x.toRotationMatrix();
    std::cout << "Final rotation matrix:\n" << final_rotation << std::endl;
    Vector3d t_1_2(0, 0, 0); // 1系原点在2系下的坐标

    // 测试点 1
    Vector3d pointA(1, 1, 1); // 1坐标系下的点
    Vector3d pointA_transformed = final_rotation* pointA + t_1_2;
    std::cout << "Point A in Frame2: " << pointA_transformed.transpose() << std::endl;
    // Point A in Frame2:  1 -1 -1

    // 测试点 2
    Vector3d pointB(0, 2, 0); // 1坐标系下的点
    Vector3d pointB_transformed = final_rotation * pointB + t_1_2;
    std::cout << "Point B in Frame2: " << pointB_transformed.transpose() << std::endl;
    // Point B in Frame2:           2 1.22465e-16 1.22465e-16

2 LIO-SAM中的extrinsicRot、extQRPY和extrinsicTrans计算推导

extrinsicRot变量的服务对象是imu的三轴加速度和三轴角速度，extQRPY的服务对象是imu的磁力计，这个磁力计可以提供绝对姿态角，也就是测绘中的的$C_b^n$。平常所说的绝对姿态角的定义如下，更多的详情可以参考牛小骥的b站视频，他这里的旋转关系是从n系转到了b系：

这里面的n坐标系就是大家平时称的world坐标系，或者称为ENU坐标系，换言之也就是$R_{body}^{world}$。这里做一个说明：extrinsicRot和extQRPY两个变量都是在函数imuConverter中进行使用，目的是为了将imu的三轴加速度和三轴角速度转换到lidar坐标系，绝对姿态角转换到lidar坐标系相对于world坐标系下的姿态角。

2.1 extrinsicRot的推导

extrinsicRot变量在代码中最后给到extRot变量，它的使用如下，也就是将imu的三轴加速度和角速度的结果转换到了lidar系。

Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;

我们的目标是将imu坐标系下的点转换到lidar系，也就是希望得到$R_{imu}^{lidar}$。两个坐标系的关系，如下图所示，只关注两个坐标系的xyz轴，也就是红色和绿色坐标轴，先不要关注里面的角度。
$R_{imu}^{lidar}$旋转矩阵的定义(可以参考1.1进行类比)是：固定lidar坐标系，然后绕lidar系的y轴旋转180度，即可和imu坐标系重合。

这部分的代码实现如下：

    // LIO-SAM中的extrinsicRot的计算过程
    // 将lidar坐标系旋转到imu坐标系
    // 具体的旋转是固定lidar坐标系，将lidar坐标系旋转到imu坐标系，绕lidar系的y轴旋转180度，得到和imu坐标系一样的坐标系
    const double angle1 = M_PI; 
    AngleAxisd angle1_lio_y(angle1, Vector3d::UnitY());
    Matrix3d extrinsicRot = angle1_lio_y.toRotationMatrix(); 
    std::cout << "extrinsicRot rotation matrix:\n" << extrinsicRot << std::endl;
    // extrinsicRot rotation matrix:
    //           -1            0  1.22465e-16
    //            0            1            0
    // -1.22465e-16            0           -1

上面打印的1.22465e-16接近于0，上述结果和params.yaml文件中的旋转矩阵一致：

  extrinsicRot: [-1, 0, 0,
                  0, 1, 0,
                  0, 0, -1]

2.2 extQRPY的推导

我们通过代码来反推extQRPY这个变量到底是什么含义。代码如下：

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
......
// 这是一个九轴imu，因此还会有姿态信息
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY;

上面代码中的q_final是$R_{lidar}^{world}$，也就是想得到lidar坐标系在世界坐标系中的姿态。在这个代码中q_from是imu的磁力计(Magnetometer)提供的三个姿态角，代码中是四元数表示的，也就是$R_{Magnetometer}^{world}$。 那么extQRPY自然就是$R_{lidar}^{Magnetometer}$。也就是下面的公式：$$R_{lidar}^{world}=R_{Magnetometer}^{world}\ast R_{lidar}^{Magnetometer}$$可是作者的imu和平时使用的imu不一样，他的imu中的磁力计观测到的yaw，roll和pitch的定义如下：yaw的定义是绕z轴旋转，但是yaw的正方向和z轴不一致；pitch确是绕x旋转，正方向没错，但是我们一般的定义是绕y轴旋转；roll是绕y轴旋转，正方向没错，但是我们一般的定义是绕x轴旋转。这里再强调一遍，一般情况下yaw航向角绕Z轴旋转，pitch俯仰角绕Y轴旋转，roll翻滚角绕X轴旋转。
因此我们根据图中三个姿态角的定义(注意关注图中imu的黄色的三个姿态角)还原出来常规的xyz的坐标轴方向，还原出来的xyz轴用黄色坐标轴表示，请注意标记出来的黄色坐标系是磁力计(Magnetometer)的真实xyz的坐标轴。
我们现在已经拿到了磁力计的真实坐标系，那么就可以求$R_{lidar}^{Magnetometer}$。
$R_{lidar}^{Magnetometer}$的定义(可以参考1.1进行类比)：以磁力计为固定坐标系，绕黄色坐标系的Z轴旋转-90度，即可和lidar坐标系完全平行。

解算的代码如下：

    // LIO-SAM中的extQRPY的计算过程，也就是R_mag_lidar
    // 将Magnetometer坐标系旋转到lidar坐标系
    // 固定Magnetometer坐标系，绕Magnetometer坐标系的z轴旋转-90度，得到和lidar坐标系一样的坐标系
    const double angel2 = -M_PI_2; 
    AngleAxisd angle2_lio_z(angel2, Vector3d::UnitZ());
    Matrix3d extQRPY = angle2_lio_z.toRotationMatrix(); 
    std::cout << "extQRPY rotation matrix:\n" << extQRPY << std::endl;
    // extQRPY rotation matrix:
    // 6.12323e-17           1           0
    //          -1 6.12323e-17           0
    //           0           0           1

上面打印的6.12323e-17接近于0，上述结果和params.yaml文件中的旋转矩阵一致：

  extrinsicRPY: [0,  1, 0,
                 -1, 0, 0,
                  0, 0, 1]

2.3 extrinsicTrans的推导

extrinsicTrans的推导相对简单，可以看以下代码：

    gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
    gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
	......
     // 将lidar的位姿转移到imu坐标系下
     prevPose_ = lidarPose.compose(lidar2Imu);

从代码中可知，通过lidar2Imu变量将lidar的位姿给转到了imu坐标系下，而作者用的lidar2Imu是通过gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()))构造的，也就是说构造了一个单位阵的旋转矩阵和extrinsicTrans的x，y，z。再结合下面的公式:$$P_{imu}=R_{lidar}^{imu}\ast P_{lidar}+t_{lidar}^{imu}$$ 那么$t_{lidar}^{imu}$，也就是extrinsicTrans。
$t_{lidar}^{imu}$的定义(可以参考1.1进行类比)为：lidar坐标系的原点在imu坐标系下的坐标值。

2.4 小结

上面完整的实现代码见我的github链接: Calculate_LIO-SAM_extrinsicRot_extQRPY。
extrinsicRot变量是imu中的六轴(加速度计和陀螺仪)观测量转换到LiDAR坐标系的旋转矩阵，也就是$R_{imu}^{lidar}$。
extrinsicRPY变量是将九轴imu中的磁力计坐标系转换到LiDAR坐标系。extrinsicTrans是lidar坐标系的原点在imu坐标系下的坐标值。
extrinsicTrans是$t_{lidar}^{imu}$，即为：lidar坐标系的原点在imu坐标系下的坐标值。
因此在使用LIO-SAM的时候，如果是六轴imu，那么需要将extrinsicRot进行配置，extrinsicRPY可以直接置为单位阵。如果是九轴imu，需要额外配置extrinsicRPY，也就是$R_{lidar}^{Magnetometer}$。不过值得注意的是，一般情况下磁力计会和imu的加速度计和陀螺仪坐标系一致，如果不一致，就需要像LIO-SAM中的作者进行配置了。
另外，以上两个变量作者的标定非常简单，只是物理位置上大致对齐了，然后就计算出了两个旋转矩阵，也可以将imu和LiDAR进行精细标定。

[1]: 参考链接: LVI-SAM坐标系外参分析与代码修改，以适配各种数据集