Orientation transformations

The main orientation types are:

• Rotation matrix
• Euler Angles
• Pitch-Yaw-Roll (XYZ euler angle case)
• Vector Axis (exponential map?)
• Queaternion

From Rotation Matrix to Pitch-Yaw-Roll

You can use similar functions with other Euler Angle representations: Out euler elements are defined in radians.

void fromRotMat2PitchYawRoll(const cv::Matx33d& rotMat,
                             float *_pitch, float *_yaw, float *_roll) {
    double q0 = sqrt(1 + rotMat(0, 0) + rotMat(1, 1) +
                     rotMat(2, 2)) / 2.0;
    double q1 = (rotMat(2, 1) - rotMat(1, 2)) / (4.0*q0);
    double q2 = (rotMat(0, 2) - rotMat(2, 0)) / (4.0*q0);
    double q3 = (rotMat(1, 0) - rotMat(0, 1)) / (4.0*q0);

    *_yaw   = static_cast<float>(asin(2.0 * (q0*q2 + q1*q3)));
    *_pitch = static_cast<float>(atan2(2.0 * (q0*q1-q2*q3),
                                       q0*q0-q1*q1-q2*q2+q3*q3));
    *_roll  = static_cast<float>(atan2(2.0 * (q0*q3-q1*q2),
                                       q0*q0+q1*q1-q2*q2-q3*q3));
}

From Pitch-Yaw-Roll to rotation matrix

You can use similar functions with other Euler Angle representations. All euler elements are defined in radians.

void fromPitchYawRoll2RotMat(const float _pitch, const float _yaw,
                             const float _roll, cv::Matx33f *_rotMat) {
    double pitchD = static_cast<double>(_pitch);
    double yawD = static_cast<double>(_yaw);
    double rollD = static_cast<double>(_roll);

    // Calculate rotation about x axis
    cv::Matx33f R_x(1,       0,              0,
                    0,       cos(pitchD),   -sin(pitchD),
                    0,       sin(pitchD),   cos(pitchD));

    // Calculate rotation about y axis
    cv::Matx33f R_y(cos(yawD),    0,      sin(yawD),
                    0,               1,      0,
                    -sin(yawD),   0,      cos(yawD));

    // Calculate rotation about z axis
    cv::Matx33f R_z(cos(rollD), -sin(rollD), 0,
                    sin(rollD), cos(rollD),  0,
                    0,          0,           1);

    // Combined rotation matrix
    *_rotMat = (R_x * R_y * R_z);
}

Rotation matrix representation

void drawOrientationInImage(cv::Mat *_img, const cv::Point2f &_centroid,
                            const cv::Mat &_rotMat) {
    double axisScale = 100.;
    cv::Mat xAxis = _rotMat.col(0) * axisScale;
    cv::Mat yAxis = _rotMat.col(1) * axisScale;
    cv::Mat zAxis = _rotMat.col(2) * axisScale;

    cv::Point2f xAxisProj = _centroid;
    xAxisProj.x += xAxis.at<float>(0);
    xAxisProj.y += xAxis.at<float>(1);

    cv::Point2f yAxisProj = _centroid;
    yAxisProj.x -= yAxis.at<float>(0);
    yAxisProj.y -= yAxis.at<float>(1);

    cv::Point2f zAxisProj = _centroid;
    zAxisProj.x -= zAxis.at<float>(0);
    zAxisProj.y -= zAxis.at<float>(1);

    cv::line(*_img, _centroid, xAxisProj, cv::Scalar(0, 0, 255));
    cv::line(*_img, _centroid, yAxisProj, cv::Scalar(0, 255, 0));
    cv::line(*_img, _centroid, zAxisProj, cv::Scalar(255, 0, 0));
}