Eigen 部分功能介绍-CFANZ编程社区

Matrix4d

#include <iostream>
#include <Eigen/Dense>
 
using namespace Eigen;
using namespace std;
 
int main()
{
  Matrix4d m = Matrix4d::Random();
  Matrix4d m1 = Matrix4d::Identity();

  cout << "m =" << endl << m << endl;
  cout << "m1 =" << endl << m1 << endl;
  cout << m1.block<3, 3>(1, 1) << endl;
}

运行：

m =
  0.680375   0.823295  -0.444451  -0.270431
 -0.211234  -0.604897    0.10794  0.0268018
  0.566198  -0.329554 -0.0452059   0.904459
   0.59688   0.536459   0.257742    0.83239
m1 =
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
1 0 0
0 1 0
0 0 1

Quaterniond的初始化：

Eigen::Quaterniond q1(w, x, y, z);// 第一种方式
 
Eigen::Quaterniond q2(Vector4d(x, y, z, w));// 第二种方式
 
Eigen::Quaterniond q2(Matrix3d(R));// 第三种方式

以上两种方式是不同的，在Quaternion内部的保存中，虚部在前，实部在后，

如果以第一种方式构造四元数，则实部是w，虚部是[x,y,z]；

对于第二种方式，则实部是w，虚部是[x,y,z]；

对于第三种方式，则是用3x3的旋转矩阵初始化四元数。

Eigen中四元数可以用Eigen::Quaterniond(double类型)或者Eigen::Quaternionf(float类型)表示. 在Eigen中的四元数可以通过如下方式构造:

Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)

w是四元数的实部, x,y,z 分别是四元数的虚部( PS : 构造的时候w在前面 ). 但是,这里有一个坑--------虽然构造的时候w在前面, 但是在实际的内存中, 四元数四个元素的顺序却是 x, y, z, w . 可以通过下面的代码去提取四元数的元素验证:

    Eigen::Vector4d q = q_AB.coeffs();

所以,当我们使用 Eigen::Map 去对某个数组操作的时候, 就需要去留意这个四元数在内存中的具体位置了.

#include <iostream>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Core>
using namespace std;
using namespace Eigen;

int main() {
        cout << "Quaternion from vector4d(1, 2, 3, 4) is:\n"        
                << Quaterniond(Vector4d(1, 2, 3, 4)).coeffs().transpose() << endl;   
        cout << "Quaternion from (1, 2, 3, 4) is:\n"        
                << Quaterniond(1, 2, 3, 4).coeffs().transpose() << endl;   
        return 0;
}

// 输出：

Quaternion from vector4d(1, 2, 3, 4) is:
1 2 3 4
Quaternion from (1, 2, 3, 4) is:
2 3 4 1

Eigen : 四元数、欧拉角、旋转矩阵相互转换

四元数->旋转矩阵

 Eigen::Quaterniond quaternion( w, x, y, z  );
 Eigen::Matrix3d rotation_matrix;
 rotation_matrix=quaternion.toRotationMatrix();

四元数->欧拉角

Eigen::Quaterniond quaternion( w, x, y, z  );
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

旋转矩阵->四元数

Eigen::Matrix3d rotation;
    for(int i=0;i<3;i++)
    {
        rotation(0,i)=currentPoseRota(0,i);
        rotation(1,i)=currentPoseRota(1,i);
        rotation(2,i)=currentPoseRota(2,i);
    }
 Eigen::Quaterniond quaternion(rotation);
 cout<<"quaternion"<<quaternion.w()<<" "<<quaternion.x()<<"  "<<quaternion.y()<<"  "<<quaternion.z()<<endl;

旋转矩阵->欧拉角

  Eigen::Matrix3d rotation;
    for(int i=0;i<3;i++)
    {
        rotation(0,i)=currentPoseRota(0,i);
        rotation(1,i)=currentPoseRota(1,i);
        rotation(2,i)=currentPoseRota(2,i);
    }
    // 旋转矩阵to欧拉角 ZYX顺序，0表示X轴,1表示Y轴,2表示Z轴
    Eigen::Vector3d euler_angles = rotation.eulerAngles(2, 1, 0);//z-y-x

[注意：euler_angles(0)表示绕z轴转]

欧拉角->旋转矩阵

    Eigen::Vector3d euler(yaw,pitch,roll);;  // 对应 z y x
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitZ()) *
                      Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitX());

欧拉角->四元数

Eigen::Vector3d eulerAngle(yaw,pitch,roll);
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); 
Eigen::Quaterniond quaternion=yawAngle*pitchAngle*rollAngle;