Eigen과 GTSAM을 활용한 Rtation의 XYZ ZYX Convention 분석

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <iostream>


void getRPYFromEigenMatrix3d(const Eigen::Matrix3d& mat, double& roll, double& pitch, double& yaw, const std::string order="zyx") {
  if (order == "zyx") {
    Eigen::Vector3d rpy = mat.eulerAngles(2, 1, 0); 
    yaw = rpy[0];
    pitch = rpy[1];
    roll = rpy[2];
  } else if (order == "xyz") {
    Eigen::Vector3d ypr = mat.eulerAngles(0, 1, 2); 
    roll = ypr[0];
    yaw = ypr[2];
    pitch = ypr[1];
  } 
}

void mat2rpy(const Eigen::Matrix3d& eigen_mat, double& roll, double& pitch, double& yaw) {
  // Convert Eigen::Matrix3d to cv::Mat
  cv::Mat cv_mat(3, 3, CV_64F);
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
        cv_mat.at<double>(i, j) = eigen_mat(i, j);
    }
  }

  // Use Rodrigues to convert rotation matrix to rotation vector
  cv::Mat rot_vec;
  cv::Rodrigues(cv_mat, rot_vec);

  // Extract roll, pitch, yaw from rotation vector
  roll  = rot_vec.at<double>(0);
  pitch = rot_vec.at<double>(1);
  yaw   = rot_vec.at<double>(2);
}
Eigen::Vector3d R2ypr(const Eigen::Matrix3d& R) {
  Eigen::Vector3d n = R.col(0);
  Eigen::Vector3d o = R.col(1);
  Eigen::Vector3d a = R.col(2);

  Eigen::Vector3d ypr(3);
  double y = atan2(n(1), n(0));
  double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
  double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
  ypr(0) = y;
  ypr(1) = p;
  ypr(2) = r;
  return ypr;
}

Eigen::Matrix3d generateRandomRotationMatrix() {
  // Random number generator for rotation angle and axis
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<> dis(0.0, 1.0);

  double x = dis(gen) * 2.0 - 1.0;
  double y = dis(gen) * 2.0 - 1.0;
  double z = dis(gen) * 2.0 - 1.0;

  Eigen::Vector3d axis(x, y, z);
  axis.normalize();

  double angle = dis(gen) * 2.0 * M_PI;

  Eigen::AngleAxisd angleAxis(angle, axis);

  Eigen::Matrix3d rotationMatrix = angleAxis.toRotationMatrix();

  return rotationMatrix;
}

int main() {
  for (size_t i = 0; i < 10; ++i) {
    Eigen::Matrix3d rotation = generateRandomRotationMatrix();
    gtsam::Rot3 R = gtsam::Rot3(rotation);

    double roll, pitch, yaw;

    std::cout << "----------------------------------------" << std::endl;
    getRPYFromEigenMatrix3d(rotation, roll, pitch, yaw);
    std::cout << "[Eigen]  Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
    getRPYFromEigenMatrix3d(rotation, roll, pitch, yaw, "xyz");
    std::cout << "[Eigen]  Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;

    mat2rpy(rotation, roll, pitch, yaw);
    std::cout << "[OpenCV] Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
    auto vec = R.ypr();
    std::cout << "[GTSAM]  Roll: " << vec(2) << ", Pitch: " << vec(1) << ", Yaw: " << vec(0) << std::endl;
    auto vec2 = R.rpy();
    std::cout << "[GTSAM]  Roll: " << vec2(0) << ", Pitch: " << vec2(1) << ", Yaw: " << vec2(2) << std::endl;
    auto vec3 = R2ypr(rotation.matrix());
    std::cout << "[Ours]   Roll: " << vec3(2) << ", Pitch: " << vec3(1) << ", Yaw: " << vec3(0) << std::endl;
    std::cout << "----------------------------------------" << std::endl;
  }
  return 0;
}

 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
  
 #ifndef EIGEN_EULERANGLES_H
 #define EIGEN_EULERANGLES_H
  
 #include "./InternalHeaderCheck.h"
  
 namespace Eigen { 
  
 template<typename Derived>
 EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
 MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
 {
   EIGEN_USING_STD(atan2)
   EIGEN_USING_STD(sin)
   EIGEN_USING_STD(cos)
   /* Implemented from Graphics Gems IV */
   EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
  
   Matrix<Scalar,3,1> res;
   typedef Matrix<typename Derived::Scalar,2,1> Vector2;
  
   const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
   const Index i = a0;
   const Index j = (a0 + 1 + odd)%3;
   const Index k = (a0 + 2 - odd)%3;
   
   if (a0==a2)
   {
     res[0] = atan2(coeff(j,i), coeff(k,i));
     if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0)))
     {
       if(res[0] > Scalar(0)) {
         res[0] -= Scalar(EIGEN_PI);
       }
       else {
         res[0] += Scalar(EIGEN_PI);
       }
       Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
       res[1] = -atan2(s2, coeff(i,i));
     }
     else
     {
       Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm();
       res[1] = atan2(s2, coeff(i,i));
     }
     
     // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles,
     // we can compute their respective rotation, and apply its inverse to M. Since the result must
     // be a rotation around x, we have:
     //
     //  c2  s1.s2 c1.s2                   1  0   0 
     //  0   c1    -s1       *    M    =   0  c3  s3
     //  -s2 s1.c2 c1.c2                   0 -s3  c3
     //
     //  Thus:  m11.c1 - m21.s1 = c3  &   m12.c1 - m22.s1 = s3
     
     Scalar s1 = sin(res[0]);
     Scalar c1 = cos(res[0]);
     res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j));
   } 
   else
   {
     res[0] = atan2(coeff(j,k), coeff(k,k));
     Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm();
     if((odd && res[0]<Scalar(0)) || ((!odd) && res[0]>Scalar(0))) {
       if(res[0] > Scalar(0)) {
         res[0] -= Scalar(EIGEN_PI);
       }
       else {
         res[0] += Scalar(EIGEN_PI);
       }
       res[1] = atan2(-coeff(i,k), -c2);
     }
     else
       res[1] = atan2(-coeff(i,k), c2);
     Scalar s1 = sin(res[0]);
     Scalar c1 = cos(res[0]);
     res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j));
   }
   if (!odd)
     res = -res;
   
   return res;
 }
  
 } // end namespace Eigen
  
 #endif // EIGEN_EULERANGLES_H

// In https://github.com/borglab/gtsam/blob/164c35424b13057c10a261eb4c8b6343125dccc9/gtsam/geometry/Rot3.cpp
pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
  // x is roll
  const double x = -atan2(-A(2, 1), A(2, 2));
  const auto Qx = Rot3::Rx(-x).matrix();
  const Matrix3 B = A * Qx;
  // y is pitch
  const double y = -atan2(B(2, 0), B(2, 2));
  const auto Qy = Rot3::Ry(-y).matrix();
  const Matrix3 C = B * Qy;
  // z is yaw
  const double z = -atan2(-C(1, 0), C(1, 1));
  const auto Qz = Rot3::Rz(-z).matrix();
  const Matrix3 R = C * Qz;
  ...

Vector3 Rot3::ypr(OptionalJacobian<3, 3> H) const {
  Vector3 q = xyz(H);
  if (H) H->row(0).swap(H->row(2));

  return Vector3(q(2),q(1),q(0));
}