版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/zhuoyueljl/article/details/70789472
1、源码
-
#include
<iostream>
-
#include
<Eigen/Eigen>
-
#include
<stdlib.h>
-
#include
<Eigen/Geometry>
-
#include
<Eigen/Core>
-
#include
<vector>
-
#include
<math.h>
-
-
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
<< "Euler2Quaternion result is:" <<endl;
-
cout << "x = " << q.x() <<endl;
-
cout << "
y =
" << q.y() <<endl;
-
cout << "
z =
" << q.z() <<endl;
-
cout << "
w =
" << q.w() <<endl<<endl;
-
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 << "
Quaterniond2Euler
result
is:" <<
endl;
-
cout << "x = "<< euler[2] << endl ;
-
cout << "
y =
"<< euler[1] << endl ;
-
cout << "
z =
"<< euler[0] << endl << endl;
-
}
-
-
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 << "
Quaternion2RotationMatrix
result
is:" <<
endl;
-
cout << "R = " << endl << R << endl<< endl;
-
return R;
-
}
-
-
-
Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)
-
{
-
Eigen::Quaterniond q = Eigen::Quaterniond(R);
-
q.normalize();
-
cout << "
RotationMatrix2Quaterniond
result
is:" <<
endl;
-
cout << "x = " << q.x() <<endl;
-
cout << "
y =
" << q.y() <<endl;
-
cout << "
z =
" << q.z() <<endl;
-
cout << "
w =
" << q.w() <<endl<<endl;
-
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 << "
Euler2RotationMatrix
result
is:" <<
endl;
-
cout << "R = " << endl << R << endl<<endl;
-
return R;
-
}
-
-
Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)
-
{
-
Eigen::Matrix3d m;
-
m = R;
-
Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);
-
cout << "
RotationMatrix2euler
result
is:" <<
endl;
-
cout << "x = "<< euler[2] << endl ;
-
cout << "
y =
"<< euler[1] << endl ;
-
cout << "
z =
"<< euler[0] << endl << endl;
-
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 << 1,0,0;
-
y_axiz << 0,1,0;
-
z_axiz << 0,0,1;
-
Eigen::Matrix3d R;
-
R << x_axiz,y_axiz,z_axiz;
-
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 << "
All
transform
is
done!" <<
endl;
-
}
-
2、测试结果
转载自:https://blog.csdn.net/zhuoyueljl/article/details/70789472
版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/zhuoyueljl/article/details/70789472