Eigen/Geometry 模块提供了各种旋转和平移的表示
一、表示:
旋转矩阵直接使用 Matrix3d 或 Matrix3f:
Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();//初始化为一个单位阵。
旋转向量使用 AngleAxis:
Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1)); //角+轴:沿 Z 轴旋转 45 度
欧拉角:
Eigen::Vector3d ea0(yaw,pitching,droll);
二、转化:
旋转向量->旋转矩阵:rotationMatrix=rotation_vector.toRotationMatrix();
旋转向量->四元数:Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
旋转矩阵->四元数:Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_matrix );
四元素->旋转矩阵:Eigen::Matrix3d Rx = q.toRotationMatrix();
旋转向量->欧拉角:Eigen::Vector3d eulerAngle=rotationVector.matrix().eulerAngles(0,1,2);
旋转矩阵->欧拉角:Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序,即roll pitch yaw顺序
三、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 << “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;
-
}
测试结果
参考:
https://blog.csdn.net/zhuoyueljl/article/details/70789472