gici-open示例数据运行（ground_truth坐标的转换）

1. 坐标系转换说明

涉及的两个坐标转换：

• nmea_pose_to_pose ：激光IMU中心到数据集IMU中心，主要是杆臂误差，转换关系为：

  //======坐标转换的主要步骤(若发现有错误的地方，请评论指出)======
  //定义激光IMU和数据集IMU之间的杆臂
  const Eigen::Vector3d t_AB_A(-0.00509546,    0.320372,   0.0669864);
  //定义激光IMU和数据集IMU之间的姿态角（应该是以数据集右前下坐标系为基准）
  const Eigen::Vector3d rot_AB(179.928, -0.55273, -179.265);
  //激光IMU到数据集IMU之间的转换四元数
  Transformation T_AB(t_AB_A, eulerAngleToQuaternion(rot_AB * D2R));
  //激光IMU为原点的载体系下的enu转换到到导航
  Transformation T_WA(p_enu, q);
  //将激光IMU为原点转换到以数据集IMU为原点
  Transformation T_WB = T_WA * T_AB;
  

  根据代码里的设置值为：(-0.00509546,    0.320372,   0.0669864)
  而从 gici-open-dataset/intrinsics_and_extrinsics.yaml中看到的参数值为：

      数值上存在一定的差异。

• nmea_pose_to_position:由数据集IMU中心到GPS天线相位中心的杆臂补偿，转换到导航系。主要转换公式为：

  //数据集IMU和GNSS APC之间的杆臂
  const Eigen::Vector3d t_SR_S(0.354, -0.042, -0.029);
  //将杆臂转换到导航系下，并将改正数加到导航系下的坐标NEU里。
  p_enu = p_enu.eval() + q * t_SR_S;
  

  代码中的杆臂为：(0.354, -0.042, -0.029)
  而从 gici-open-dataset/intrinsics_and_extrinsics.yaml中看到的参数值为：

• 
  两者也存在数值上的差异。

2. 杆臂误差处理（载体系投影到导航系）

导航系(n)：站心坐标系，一般以运动起始时刻作为站心原点，坐标轴：北东地

载体系(b):以IMU或GNSS为中心，坐标轴：前右下

 [北东地——前右下]坐标系下欧拉角旋转顺序：
       航向角（绕Z轴）—俯仰角（绕Y轴）—横滚角（绕X轴）：
  第一个转动角度：航向角（绕Z轴旋转ψ角）
  第二个转动角度：俯仰角（绕Y轴旋转 θ 角）
  第三个转动角度：横滚角（绕X轴旋转ϕ 角）
注意 俯仰角和横滚角：方向与对应轴向转动方向一致，绕对应轴正转，相应姿态为正；绕对应轴反转，相应姿态为负。X轴指北时，航向角为零。北偏东顺时针依次从0变为360°
  对应的旋转矩阵为：

  

最后一定注意欧拉角旋转顺序与定义啊兄弟们，别搞混了，坐标系选取不同定义也不同的！！！！！！

 2. 欧拉角和四元数的关系原理

文章来源：无处不在的小土-导航坐标系与姿态描述方法