eigen四元素轉換

#include <iostream> #include <stdio.h> #include <Eigen/Core> #include <Eigen/Geometry> using namespace std;#define PI (3.1415926535897932346f) ::Eigen::Quaterniond Euler_to_Quaterniond(double yaw, double pitching, double droll){ ::Eigen::Vector3d ea0(yaw,pitching,droll); ::Eigen::Matrix3d R; R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ()) * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY()) * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX()); //cout << R << endl << endl; //RotationMatrix to Quaterniond ::Eigen::Quaterniond q; q = R; cout <<"x:"<<q.x()<<" "<<"y:"<<q.y()<<" "<<"z:"<<q.z()<<" "<<"w:"<<q.w()<<endl; return q;}::Eigen::Vector3d Quaterniond_to_Euler(double x, double y, double z, double w){ ::Eigen::Quaterniond q(w,x,y,z); ::Eigen::Matrix3d Rx = q.toRotationMatrix(); ::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0); cout <<ea1[0]<<" "<<ea1[1]<<" "<<ea1[2]<<endl; return ea1;}int main(int argc, char **argv) { double yaw = PI/3,pitching = PI/4,droll = PI/6; //double yaw = PI,pitching = PI,droll = PI; printf("T yaw[%f] pitching[%f] droll[%f]
",yaw,pitching,droll); ::Eigen::Quaterniond q = Euler_to_Quaterniond(yaw, pitching, droll); ::Eigen::Vector3d V = Quaterniond_to_Euler(q.x(), q.y(), q.z(), q.w()); //cout << V/PI*180 << endl; #if 0 printf("T yaw[%f] pitch[%f] droll[%f]
", yaw, pitching, droll); //EulerAngles to RotationMatrix ::Eigen::Vector3d ea0(yaw,pitching,droll); ::Eigen::Matrix3d R; R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ()) * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY()) * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX()); cout << R << endl << endl; //RotationMatrix to Quaterniond ::Eigen::Quaterniond q; q = R; cout << q.x() << endl << endl; cout << q.y() << endl << endl; cout << q.z() << endl << endl; cout << q.w() << endl << endl; ::Eigen::Quaterniond q1(q.w(),q.x(),q.y(),q.z()); //Quaterniond to RotationMatrix ::Eigen::Matrix3d Rx = q1.toRotationMatrix(); cout << Rx << endl << endl; //RotationMatrix to EulerAngles ::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0); //cout << ea1/PI*180 << endl << endl; cout << ea1 << endl << endl; std::cin.ignore(); #endif return 0; }
推薦閱讀：