四元数和欧拉角的转换matlab程序,四元数、欧拉角、旋转矩阵之间互相转换C++源码...

#include 

#include 

#include 

#include 

#include 

#include 

#include 

using namespace std;

using namespace Eigen;

Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)

{

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

Eigen::Quaterniond q = rollAngle yawAngle pitchAngle;

cout <

cout <

cout <

cout <

cout <

return q;

}

Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)

{

Eigen::Quaterniond q;

q.x() = x;

q.y() = y;

q.z() = z;

q.w() = w;

Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);

cout <

cout <

cout <

cout <

}

Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)

{

Eigen::Quaterniond q;

q.x() = x;

q.y() = y;

q.z() = z;

q.w() = w;

Eigen::Matrix3d R = q.normalized().toRotationMatrix();

cout <

cout <

return R;

}

Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)

{

Eigen::Quaterniond q = Eigen::Quaterniond(R);

q.normalize();

cout <

cout <

cout <

cout <

cout <

return q;

}

Eigen::Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)

{

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());

Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());

Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

Eigen::Quaterniond q = rollAngle yawAngle pitchAngle;

Eigen::Matrix3d R = q.matrix();

cout <

cout <

return R;

}

Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)

{

Eigen::Matrix3d m;

m = R;

Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);

cout <

cout <

cout <

cout <

return euler;

}

int main(int argc, char **argv)

{

//this is euler2Quaternion transform function,please input your euler angle//

euler2Quaternion(0,0,0);

//this is Quaternion2Euler transform function,please input your euler angle//

Quaterniond2Euler(0,0,0,1);

//this is Quaternion2RotationMatrix transform function,please input your Quaternion parameter//

Quaternion2RotationMatrix(0,0,0,1);

//this is rotationMatrix2Euler transform function,please input your RotationMatrix parameter like following//

Eigen::Vector3d x_axiz,y_axiz,z_axiz;

x_axiz <

y_axiz <

z_axiz <

Eigen::Matrix3d R;

R <

rotationMatrix2Quaterniond(R);

//this is euler2RotationMatrix transform function,please input your euler angle for the function parameter//

euler2RotationMatrix(0,0,0);

//this is RotationMatrix2euler transform function,please input your euler angle for the function parameter//

RotationMatrix2euler(R);

cout <

}