快速入门Eigen

demo.cpp

#include<iostream>
using namespace std;

#include<Eigen\Core>
#include<Eigen\Dense>

#include<Eigen/Geometry>

const double M_PI = 3.1415926535;

void Test1()
{
    Eigen::Matrix<float, 2, 3> matrix_23 = Eigen::Matrix<float, 2, 3>::Ones();
    Eigen::Vector3d v_3d = Eigen::Vector3d::Zero();// 3x1
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//3x3
    // 动态size矩阵
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    // 更简单的表达形式
    Eigen::MatrixXd matrix_x;

    // operate

    matrix_23 << 1, 2, 3, 4, 5, 6;
    v_3d << 3, 2, 1;
    cout << matrix_23 << endl;
    cout << endl;
    cout << v_3d << endl;
    cout << endl;
    // 乘法 float -> double
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;
    cout << endl;

    matrix_33 = Eigen::Matrix3d::Random();//  伪随机
    cout << matrix_33 << endl;
    cout << endl;

    cout << matrix_23.transpose() << endl;  cout << endl;
    cout << matrix_23.sum() << endl;        cout << endl;
    cout << matrix_33.trace() << endl;      cout << endl;
    cout << 10 * matrix_33 << endl;         cout << endl;
    cout << matrix_33.inverse() << endl;    cout << endl;
    cout << matrix_33.determinant() << endl; cout << endl;

    // 特征值
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    cout << "values = " << eigen_solver.eigenvalues() << endl;   cout << endl;
    cout << "vectors = " << eigen_solver.eigenvectors() << endl; cout << endl;

    // 解方程 A * x = b
    Eigen::Matrix<double, 50, 50> A = Eigen::Matrix<double, 50, 50>::Random();
    Eigen::Matrix<double, 50, 1 > b = Eigen::Matrix<double, 50, 1 >::Random();

    // 如果无解， eigen 是怎么解决这个问题的？？？？？？
    // 法一、x = inv(A)*b
    cout << A.inverse() * b << endl;  cout << endl;

    // 法二、QR分解
    cout << A.colPivHouseholderQr().solve(b) << endl;  cout << endl;
}

void Test2()
{
    Eigen::Matrix3d R_mat = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd R_vec(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 绕Z轴旋转Π/4
    // AngleAxisd -> Matrix3d
    R_mat = R_vec.matrix();
    cout << R_vec.toRotationMatrix() << endl; cout << endl;
    cout << R_vec.matrix() << endl;           cout << endl;

    // 坐标旋转：point2 = R_vec * point1
    Eigen::Vector3d point1(1, 0, 0);
    Eigen::Vector3d point2 = R_vec * point1; // point2 = R_mat * point1
    cout << point2 << endl; cout << endl;

    // 旋转矩阵 -> 欧拉角
    Eigen::Vector3d ZYX_angle = R_mat.eulerAngles(2, 1, 0);// Z Y X 顺序；
    cout << ZYX_angle << endl; cout << endl;

    // 初始化一个T = (R|t)
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(R_vec);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    cout << "T = " << endl;
    cout << T.matrix() << endl; cout << endl;

    // point3 = T * point1
    Eigen::Vector3d point3 = T * point1;
    cout << point3 << endl; cout << endl;

    // 初始化一个四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(R_vec);
    cout << "q = " << endl;
    cout << q.coeffs() << endl; cout << endl;

    // point4 = q * point1
    Eigen::Vector3d point4 = q * point1;
    cout << point4 << endl; cout << endl;
}

/**********************************************************************************************************

 功能：创建一个齐次变换矩阵 T = (R|t)；

 输入：roll(X轴)、pitch(Y轴)、yaw(Z轴)：欧拉角；
        (x, y, z):位移；

 输出：齐次变换矩阵 T

 返回：...

**********************************************************************************************************/
Eigen::Isometry3d CreateT(const double& roll, const double& pitch, const double& yaw,
    const double& x, const double& y, const double& z)
{
    // <1> 初始化R
    Eigen::Matrix3d R;
    // 按照 ZYX 顺序旋转
    R = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());

    // <2> 初始化t
    Eigen::Vector3d t(x, y, z);

    // <3> 构建T = (R|t)
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(R);
    T.pretranslate(t);
    return T;
}



int main()
{
    // <1> 由欧拉角 + 位姿 初始化姿态; Pose1 -> 齐次变换矩阵 T1
    Eigen::Isometry3d T1 = CreateT((30.0 / 180.0) * M_PI, (25.0 / 180.0) * M_PI, (27.0 / 180.0) * M_PI, 1.2, 0.234, 2.3);
    Eigen::Isometry3d T2 = CreateT((23.0 / 180.0) * M_PI, (33.0 / 180.0) * M_PI, (89.0 / 180.0) * M_PI, 0.1, 0.4, 0.1);

    cout << "<1> 由欧拉角 + 位姿 初始化姿态; Pose1 -> 齐次变换矩阵 T1" << endl;
    cout << "T1 = " << endl; cout << T1.matrix() << endl;
    cout << "T2 = " << endl; cout << T2.matrix() << endl;
    cout << endl;

    // <2> Pose1求逆（等价于T1求逆）
    Eigen::Isometry3d T1Invert = T1.inverse();
    Eigen::Isometry3d T2Invert = T2.inverse();

    cout << "<2> Pose1求逆（等价于T1求逆）" << endl;
    cout << "T1Invert = " << endl; cout << T1Invert.matrix() << endl;
    cout << "T2Invert = " << endl; cout << T2Invert.matrix() << endl;
    cout << endl;

    // <3> 齐次变换矩阵 T1 -> 欧拉角 + 位姿 Pose1
    cout << "<3> 齐次变换矩阵 T1 -> 欧拉角 + 位姿 Pose1" << endl;
    cout << "Pose1 = ";
    cout << T1.translation().transpose() << " " << T1.rotation().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << endl;
    cout << endl;

    // <4> Pose12 = Pose1 * Pose2 等价于 T12 = T1 * T2
    Eigen::Isometry3d T12 = T1 * T2;

    cout << "<4> Pose12 = Pose1 * Pose2 等价于 T12 = T1 * T2" << endl;
    cout << "T12 = " << endl; cout << T12.matrix() << endl;
    cout << endl;

    // <5> Pose1(旋转部分) -> 四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(T1.rotation());

    cout << " <5> Pose1(旋转部分) -> 四元数" << endl;
    cout << "q = " << endl; cout << q.coeffs().transpose() << endl; // coeffs 中实部在最后

    // <6> 四元数 -> Pose1__
    cout << " <6> 四元数 -> Pose1__(旋转部分)" << endl;
    cout << "Pose1__ = " << endl; cout << q.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << endl;

    return 1;
}

运行结果：