Eigen Rotations

main.cpp

#include <random>

#include <ros/ros.h>

#include <Eigen/Geometry>
#include <tf_conversions/tf_eigen.h>


int main(int argc, char* argv[]) {

  std::random_device rnd;
  const double roll = ( (int)(rnd()%36) - 18 ),
              pitch = ( (int)(rnd()%36) - 18 ),
                yaw = ( (int)(rnd()%360) - 180 );

  ROS_INFO("(roll, pitch, yaw) = (%lf, %lf, %lf)", roll, pitch, yaw);
  std::vector<std::string> ORDER { "RPY", "RYP", "YRP", "YPR", "PRY", "PYR" };

  // tf
  tf::Matrix3x3 tfMat;
  tfMat.setEulerYPR(yaw * M_PI/180.0, pitch * M_PI/180.0, roll * M_PI/180.0);
  Eigen::Vector3d rpyTF;
  tfMat.getEulerYPR(rpyTF.z(), rpyTF.y(), rpyTF.x());
  rpyTF *= 180.0 / M_PI;
  ROS_INFO("tf : (%lf, %lf, %lf)", rpyTF.x(), rpyTF.y(), rpyTF.z());

  // Eigen
  for ( const auto& order : ORDER ) {
    Eigen::Vector3d v;
    const Eigen::AngleAxisd r(roll * M_PI/180.0, Eigen::Vector3d::UnitX());
    const Eigen::AngleAxisd p(pitch* M_PI/180.0, Eigen::Vector3d::UnitY());
    const Eigen::AngleAxisd y(yaw* M_PI/180.0, Eigen::Vector3d::UnitZ());

    if ( order == "RPY" ) {
      Eigen::Quaterniond rot = r*p*y;
      v = Eigen::Matrix3d(rot).eulerAngles(0,1,2);
    } else if ( order == "RYP" ) {
      Eigen::Quaterniond rot = r*y*p;
      v = Eigen::Matrix3d(rot).eulerAngles(0,2,1);
    } else if ( order == "YRP" ) {
      Eigen::Quaterniond rot = y*r*p;
      v = Eigen::Matrix3d(rot).eulerAngles(1,2,0);
    } else if ( order == "YPR" ) {
      Eigen::Quaterniond rot = y*p*r;
      v = Eigen::Matrix3d(rot).eulerAngles(2,1,0);
    } else if ( order == "PRY" ) {
      Eigen::Quaterniond rot = p*r*y;
      v = Eigen::Matrix3d(rot).eulerAngles(1,0,2);
    } else if ( order == "PYR" ) {
      Eigen::Quaterniond rot = p*y*r;
      v = Eigen::Matrix3d(rot).eulerAngles(2,0,1);
    }

    v *= 180.0/M_PI;
    ROS_INFO("%s : (%lf, %lf, %lf)", order.c_str(), v.x(), v.y(), v.z());

  }

  return 0;
}

stdout

>
[ INFO] [1550560980.507326642]: (roll, pitch, yaw) = (4.000000, -18.000000, 148.000000)
[ INFO] [1550560980.507386066]: tf : (4.000000, -18.000000, 148.000000)
[ INFO] [1550560980.507401379]: RPY : (4.000000, -18.000000, 148.000000)
[ INFO] [1550560980.507411862]: RYP : (4.000000, 148.000000, -18.000000)
[ INFO] [1550560980.507421806]: YRP : (21.191105, 148.515785, -7.241337)
[ INFO] [1550560980.507432936]: YPR : (148.000000, -18.000000, 4.000000)
[ INFO] [1550560980.507442763]: PRY : (162.000000, 176.000000, -32.000000)
[ INFO] [1550560980.507461223]: PYR : (148.210755, -5.567181, 15.266155)

>
[ INFO] [1550560981.864333517]: (roll, pitch, yaw) = (14.000000, 0.000000, 93.000000)
[ INFO] [1550560981.864394690]: tf : (14.000000, 0.000000, 93.000000)
[ INFO] [1550560981.864425180]: RPY : (14.000000, -0.000000, 93.000000)
[ INFO] [1550560981.864435020]: RYP : (14.000000, 93.000000, 0.000000)
[ INFO] [1550560981.864445310]: YRP : (180.000000, 87.000000, -166.000000)
[ INFO] [1550560981.864452835]: YPR : (93.000000, 0.000000, 14.000000)
[ INFO] [1550560981.864460656]: PRY : (180.000000, 166.000000, -87.000000)
[ INFO] [1550560981.864471077]: PYR : (93.000000, 14.000000, 0.000000)

>
[ INFO] [1550560983.020657733]: (roll, pitch, yaw) = (-9.000000, 13.000000, 1.000000)
[ INFO] [1550560983.020721992]: tf : (-9.000000, 13.000000, 1.000000)
[ INFO] [1550560983.020736478]: RPY : (171.000000, 167.000000, -179.000000)
[ INFO] [1550560983.020745274]: RYP : (171.000000, 179.000000, -167.000000)
[ INFO] [1550560983.020752598]: YRP : (12.839355, -1.041674, -8.995310)
[ INFO] [1550560983.020759311]: YPR : (1.000000, 13.000000, -9.000000)
[ INFO] [1550560983.020765884]: PRY : (13.000000, -9.000000, 1.000000)
[ INFO] [1550560983.020773563]: PYR : (3.013375, -8.542759, 13.146411)

>
[ INFO] [1550560984.169804621]: (roll, pitch, yaw) = (-15.000000, 10.000000, 39.000000)
[ INFO] [1550560984.169868625]: tf : (-15.000000, 10.000000, 39.000000)
[ INFO] [1550560984.169883689]: RPY : (165.000000, 170.000000, -141.000000)
[ INFO] [1550560984.169893376]: RYP : (165.000000, 141.000000, -170.000000)
[ INFO] [1550560984.169904632]: YRP : (11.933772, 35.791121, -22.266961)
[ INFO] [1550560984.169911887]: YPR : (39.000000, 10.000000, -15.000000)
[ INFO] [1550560984.169919151]: PRY : (10.000000, -15.000000, 39.000000)
[ INFO] [1550560984.169926889]: PYR : (40.608238, -8.588118, 7.844226)

>
[ INFO] [1550560985.007435057]: (roll, pitch, yaw) = (16.000000, -15.000000, 56.000000)
[ INFO] [1550560985.007498959]: tf : (16.000000, -15.000000, 56.000000)
[ INFO] [1550560985.007511982]: RPY : (16.000000, -15.000000, 56.000000)
[ INFO] [1550560985.007521159]: RYP : (16.000000, 56.000000, -15.000000)
[ INFO] [1550560985.007531425]: YRP : (157.454164, 130.456760, -145.935279)
[ INFO] [1550560985.007539769]: YPR : (56.000000, -15.000000, 16.000000)
[ INFO] [1550560985.007548065]: PRY : (165.000000, 164.000000, -124.000000)
[ INFO] [1550560985.007557663]: PYR : (57.418383, 3.439046, -8.336751)

>
[ INFO] [1550560985.963240507]: (roll, pitch, yaw) = (-14.000000, -13.000000, 133.000000)
[ INFO] [1550560985.963297493]: tf : (-14.000000, -13.000000, 133.000000)
[ INFO] [1550560985.963316091]: RPY : (166.000000, -167.000000, -47.000000)
[ INFO] [1550560985.963328361]: RYP : (166.000000, 47.000000, 167.000000)
[ INFO] [1550560985.963340205]: YRP : (17.218241, 137.507445, -26.176570)
[ INFO] [1550560985.963354641]: YPR : (133.000000, -13.000000, -14.000000)
[ INFO] [1550560985.963380521]: PRY : (167.000000, -166.000000, -47.000000)
[ INFO] [1550560985.963394870]: PYR : (136.090368, -23.288012, 9.614823)