Eigen几何模块

1 ubuntu安装Eigen

sudo apt install libeigen3-dev

2 Eigen 的基本使用

2.1 头文件

#include <Eigen/Core>
#include <Eigen/Dense>

2.2 数据类型和赋值

这里介绍三种： Eigen::Matrix（自定义矩阵）、 Eigen::Matrix3d（3x3的矩阵） 和 Eigen::Vector3d（3x1的向量），Vector3d 的底层数据类型也是 Matrix；给它们赋值的时候都可以用数据流符号 <<，代码如下：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main()
{
        Eigen::Matrix<double, 2, 3> matrix_23;
        matrix_23 << 1,2,3,4,5,6;
        //matrix_23 = Eigen::MatrixXd::Ones(2,3); // 全1矩阵
        std::cout << matrix_23 << std::endl;

        Eigen::Matrix3d matrix_33; // 相当于 Eigen::Matrix<double, 3, 3>; 可以直接与 Eigen::Matrix 运算
        matrix_33 << 1,2,3,4,5,6,7,8,9;
        //matrix_33 = Eigen::Matrix3d::Ones(); // 全1矩阵
        std::cout << matrix_33 << std::endl;


        Eigen::Vector3d v_3d;      // 相当于 Eigen::Matrix<double, 3, 1>; 但不可以直接与 Eigen::Matrix 运算
        v_3d << 1,2,3;
        //v_3d = Eigen::Vector3d::Ones(); // 全1矩阵
        std::cout << v_3d << std::endl;
}

2.3 矩阵的运算

首先，运算之前数据类型包保证一致，比如 Eigen::Matrix 和 Eigen::Vector3d 就不能相互运算，需要将 Eigen::Matrix 作转换，相关运算如下：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main()
{
    Eigen::Matrix<double, 2, 3> matrix_23;
    matrix_23 << 1,2,3,4,5,6;

    Eigen::Matrix3d matrix_33; // 相当于 Eigen::Matrix<double, 3, 3>;
    matrix_33 << 1,2,3,4,5,6,7,8,9;

    Eigen::Vector3d v_3d;      // 相当于 Eigen::Matrix<double, 3, 1>;
    v_3d << 1,2,3;

    /*************** 三种数据类型之间的运算 ******************/
    // 1. Eigen::Matrix 和 Eigen::Vector3d 运算
    Eigen::Matrix<double, 2,1> result1 = matrix_23.cast<double>() * v_3d;

    // 2. Eigen::Matrix3d 和 Eigen::Vector3d 运算
    Eigen::Matrix<double, 3,1> result2 = matrix_33.cast<double>() * v_3d;

    // 3. Eigen::Matrix 和 Eigen::Matrix3d 运算 (其实就是 Eigen::Matrix 之间的运算，不需要转化)
    Eigen::Matrix<double, 2,3> result3 = matrix_23 * matrix_33;


    /*************** 关于 Eigen::Matrix3d 的矩阵运算 ****************/
    // 1. 转置
    std::cout << matrix_33.transpose() << std::endl;
    // 2. 求和
    std::cout << matrix_33.sum() << std::endl;
    // 3. 迹
    std::cout << matrix_33.trace() << std::endl;
    // 4. 数乘
    std::cout << 10 * matrix_33 << std::endl;
    // 5. 逆
    std::cout << matrix_33.inverse() << std::endl;
    // 6. 行列式
    std::cout << matrix_33.determinant() << std::endl;


    /***************** 解方程 ******************/
    // 求解 matrix_99 * x = v_9d
    Eigen::Matrix<double, 9, 9> matrix_99 = Eigen::MatrixXd::Random(9,9);
    matrix_99 = matrix_99 * matrix_99.transpose(); // 创建 matrix_99，保证它半正定
    Eigen::Matrix<double, 9, 1> v_9d = Eigen::MatrixXd::Random(9, 1); // 创建 v_9d

    // 方法1：直接求逆
    Eigen::Matrix<double, 9, 1> x = matrix_99.inverse() * v_9d;
    std::cout << x.transpose() << std::endl;

    // 方法2：矩阵分解来求解
    x = matrix_99.colPivHouseholderQr().solve(v_9d);
    std::cout << x.transpose() << std::endl;

    // 方法3：对于正定矩阵，还可以用 cholesky 来分解
    x = matrix_99.ldlt().solve(v_9d);
    std::cout << x.transpose() << std::endl;
}

3 关于几何的几种旋转类型

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main()
{
    /****************** 旋转类型 ******************/
    // 1. 旋转向量 (3 x 1)
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); // 表示沿 z 轴旋转 90 度
    ///std::cout << rotation_vector.matrix() << std::endl;

    // 2. 旋转矩阵 (3 x 3) <-- 旋转向量 (3 x 1)
    Eigen::Matrix3d rotation_maxtrix = rotation_vector.toRotationMatrix();
    ///std::cout << rotation_maxtrix << std::endl;

    // 2. 旋转矩阵 (3 x 3) <-- cv::Mat 旋转矩阵 (3 x 3)
    rotation_maxtrix << R.at<cv::Vec3d>(0,0)[0],  // R 的格式为 CV_64F 1 通道
                        R.at<cv::Vec3d>(0,0)[1],
                        R.at<cv::Vec3d>(0,0)[2],
                        R.at<cv::Vec3d>(0,1)[0],
                        R.at<cv::Vec3d>(0,1)[1],
                        R.at<cv::Vec3d>(0,1)[2],
                        R.at<cv::Vec3d>(0,2)[0],
                        R.at<cv::Vec3d>(0,2)[1],
                        R.at<cv::Vec3d>(0,2)[2];

    // 3. 欧拉角 (3 x 1) <-- 旋转矩阵 (3 x 3)
    Eigen::Vector3d euler_angles = rotation_maxtrix.eulerAngles(2, 1, 0); // 这里数字表示优先级，轴旋转顺序为zyx
    ///std::cout << euler_angles.transpose() << std::endl;

    // 4. 四元数 (4 x 1) <-- 旋转向量 (3 x 1) or 旋转矩阵 (3 x 3)
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    //Eigen::Quaterniond q = Eigen::Quaterniond(rotation_maxtrix);
    ///std::cout << q.coeffs().transpose() << std::endl;

    // 5. 欧式变换矩阵 (4 x 4) <-- 旋转向量 (3 x 1) and 平移向量 (3 x 1)
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    ///std::cout << T.matrix() << std::endl;

    // 以上几种旋转的类型(除了欧拉角)都可以直接和目标相乘，从而旋转目标
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated;
    v_rotated = rotation_vector * v;
    v_rotated = rotation_maxtrix * v;
    v_rotated = q * v;
    v_rotated = T * v;
    ///std::cout << v_rotated << std::endl;
}