jgoenetxea
7/20/2017 - 3:32 PM

Yaw-pitch-roll from rotation matrix

Different types of matrix transformations

matrix_transforms.cpp
content_copyfile_download
// 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));
}