Skip to content

Instantly share code, notes, and snippets.

@ginrou
Created February 19, 2019 07:24
Show Gist options
  • Save ginrou/d745a063764f1f2298631315de5d7361 to your computer and use it in GitHub Desktop.
Save ginrou/d745a063764f1f2298631315de5d7361 to your computer and use it in GitHub Desktop.
Eigen Rotations
#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;
}
>
[ 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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment