c_cpp 旋转矩阵的偏航俯仰滚动

Posted 2021-05-14

// How to transform rotation matrix to yaw pitch roll
// This type of rotation vector is a z-y-x euler angle definition
void fromMatToYawPitchRoll(const cv::Matx33f& rotMat, 
                           float *_yaw, float *_pitch, float *_roll) {
  *_pitch = -atan2(rotMat(2, 1), rotMat(2, 2));
  *_yaw = -atan2(-rotMat(2, 0),
                 sqrt((rotMat(2, 1) * rotMat(2, 1)) +
                      (rotMat(2, 2) * rotMat(2, 2))));
  *_roll = atan2(rotMat(1, 0), rotMat(0, 0));
}