Eigen——几何模块的使用方法与举例

Eigen——几何模块的使用方法

常用表示：
1、旋转矩阵（3X3）：Eigen::Matrix3d
2、旋转向量（3X1）:Eigen::AngleAxisd
旋转向量也就是所谓的轴角，定义如下：
3、四元数（4X1）:Eigen::Quaterniond
4、平移向量（3X1）:Eigen::Vector3d
5、变换矩阵（4X4）:Eigen::Isometry3d
6、绕该轴逆时针旋转angle(rad)：AngleAxis（angle, axis）angle(rad)表示旋转角度默认为：弧度制
7、变换矩阵 ：
Eigen::Isometry3d T;
T.matrix()才是变换矩阵，做运算时需加.matrix()后缀;
T.pretranslate()以及T.rotate()可以给平移部分和旋转矩阵赋值，但是若循环中使用，末尾不重置变换矩阵的话，这个设置量会累加，而不是覆盖。

#include <iostream>
#include <cmath>
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
constexpr auto M_PI = 3.14159265358979323846 ;

using namespace std;

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main(int argc, char** argv)
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
    cout.precision(3);
    cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
    cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
    T.pretranslate(Eigen::Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() << endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T * v;                              // 相当于R*v+t
    cout << "v tranformed = " << v_transformed.transpose() << endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout << "quaternion = \n" << q.coeffs() << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond(rotation_matrix);
    cout << "quaternion = \n" << q.coeffs() << endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q * v; // 注意数学上是qvq^{-1}
    cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

    return 0;
}

例子：

Eigen::Vector3d  cal_delta_distance(Eigen::Vector3d odom_pose)
{

    static Eigen::Vector3d now_pos,last_pos;
    Eigen::Vector3d d_pos;  //return value
    now_pos = odom_pose;

    //TODO:
    // -------------------------------------------------------------------------------------------------
    // d_pos = last_pos.inverse() * now_pos;
    // d_pos = now_pos - last_pos;是不对的
    d_pos = now_pos - last_pos;
    Eigen::AngleAxisd temp(last_pos(2),Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d trans=temp.matrix().inverse();
    d_pos=trans*d_pos;

    //end of TODO:


    last_pos = now_pos;

    return d_pos;
}