quaternion is formed incorrectly in data_parser.cc

[jilinzhou]

I notice the issue as I am swapping out gnss driver with our own sensor framework.

In novatel_parser.cc, the orientation is converted from RFU to FLU. Specially, the azimuth angle (measured clockwise from North) is converted to Yaw (counterclockwise from East).

Then in data_parser.cpp, all of them are converted back to RFU and these are all fine. The problem is the yaw angle is funnily converted back to azimuth (measured from North but counterclockwise, so not in real azimuth sense) and quaternion is formed using azimuth angle which is incorrectly as my understanding (Please see application note apn037.pdf from Novatel at https://www.novatel.com/support/search/items/Application%20Note).

  // 2. orientation
  Eigen::Quaterniond q =
      Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                        Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY());

  gps_msg->mutable_orientation()->set_qx(q.x());
  gps_msg->mutable_orientation()->set_qy(q.y());
  gps_msg->mutable_orientation()->set_qz(q.z());
gps_msg->mutable_orientation()->set_qw(q.w());

There are a few consequences here:

1. In apollo::common::math::quaternion::QuternionToHeading and HeadingToQuternion, an extra PI/2 is added to offset the angle in between the East and the North. If the quaternion is formed properly in the first place, there is no need of this PI/2 offset.

inline double QuaternionToHeading(const double qw, const double qx,
                                  const double qy, const double qz) {
  EulerAnglesZXYd euler_angles(qw, qx, qy, qz);
  // euler_angles.yaw() is zero when the car is pointing North, but
  // the heading is zero when the car is pointing East.
  return NormalizeAngle(euler_angles.yaw() + M_PI_2);
}

template <typename T>
inline Eigen::Quaternion<T> HeadingToQuaternion(const double heading) {
  // Note that heading is zero at East and yaw is zero at North.
  EulerAnglesZXY<T> euler_angles(heading - M_PI_2);
  return euler_angles.ToQuaternion();
}

2. In Function localization::rtk_localization::ComposeLocalizationMsg(), this quaternion is used to rotate the angular velocity and the linear acceleration to the world frame as the following:

          // linear_acceleration:
          // convert from vehicle reference to map reference
          Vector3d orig(imu.linear_acceleration().x(),
                        imu.linear_acceleration().y(),
                        imu.linear_acceleration().z());
          Vector3d vec = common::math::QuaternionRotate(
              localization->pose().orientation(), orig);
          mutable_pose->mutable_linear_acceleration()->set_x(vec[0]);
          mutable_pose->mutable_linear_acceleration()->set_y(vec[1]);
          mutable_pose->mutable_linear_acceleration()->set_z(vec[2]);

...
          // angular_velocity:
          // convert from vehicle reference to map reference
          Vector3d orig(imu.angular_velocity().x(), imu.angular_velocity().y(),
                        imu.angular_velocity().z());

          Vector3d vec = common::math::QuaternionRotate(
              localization->pose().orientation(), orig);
          mutable_pose->mutable_angular_velocity()->set_x(vec[0]);
          mutable_pose->mutable_angular_velocity()->set_y(vec[1]);
          mutable_pose->mutable_angular_velocity()->set_z(vec[2]);

Luckily these world frame (map reference) acceleration and angular velocity are never used in the control node, so we do not see the actual problem. But the rotated vectors are certainly not what we are expecting.

3. Because the data_parser.cc forms the quaternion following Novatel way (Z-X-Y), it makes adding new GPS/IMU support a bit tricky

Any comments will be appreciated!

[hillbrook]

1. Applying the right-hand rule on the x, y and z axes of vehicle frame, the Euler angles (roll, pitch and yaw) are defined, respectively. Yaw is positive when the vehicle turns left; Pitch is positive when the vehicle is nose up; Roll is positive when the left side of the vehicle is up. In novatel_parser.cc, vehicle frame is FLU. The original idea is publish sensor-independent vehicle frame in driver. In data_parser.cc, vehicle frame is converted back to RFU(Localization module use RFU vehicle body frame); Because yaw is positive when the vehicle turns left, yaw is counterclockwise from North(opposite to azimuth). In the following code,

   // 2. orientation
   Eigen::Quaterniond q =
     Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                       Eigen::Vector3d::UnitZ()) *
     Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
     Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY());

   Since euler angle in ins messgae is FLU frame, the auler angle in RFU frame is below:

   yaw_rfu = ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL;
   pitch_rfu = -ins->euler_angles().y();
   roll_rfu = ins->euler_angles().x();

   Using intrinsic z-y-x sequence, quaternion is below:

   Eigen::Quaterniond q =
     Eigen::AngleAxisd(yaw_rfu,
                       Eigen::Vector3d::UnitZ()) *
     Eigen::AngleAxisd(pitch_rfu, Eigen::Vector3d::UnitX()) *
     Eigen::AngleAxisd(roll_rfu, Eigen::Vector3d::UnitY());

2. In Planning, heading is counterclockwise from x-axis (ENU frame).

[jilinzhou]

@hillbrook :

Is the sequence in the following code z-x-y (UnitZ(), UnitX(), then UnitY())?

 // 2. orientation
  Eigen::Quaterniond q =
      Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                        Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY());

[hillbrook]

The sequence is z-x-y.

[xmyqsh]

@hillbrook: Absolutely right! For clarification and most important, I paste the definition of Euler angle in apollo project below:

  // in world coordinate (East/North/Up)
  // The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
  // The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
  // The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
  // The direction of rotation follows the right-hand rule.
  optional apollo.common.Point3D euler_angles = 9;

as well as @xinwf 's good analyse: #5713

[zhixy]

@hillbrook @jilinzhou if it is z-x-y, shouldn't the rotation around z be at the right most? just like

// 2. orientation
  Eigen::Quaterniond q =
       Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY())*
      Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL,
                        Eigen::Vector3d::UnitZ());

And also, in the document (apn37 https://www.novatel.com/support/search/items/Application%20Note) from Novatel, the rotation around z is at the right most.

[JeffYoung17]

@zhixy you need to check "Relating the Body Frame to the Local Level Frame" in (https://www.novatel.com/support/search/items/Application%20Note)，the rotation matrix is formed by euler angles of intrinsic rotation in z-x-y order. And you can refer to https://en.wikipedia.org/wiki/Euler_angles.