SLAM之数学基础篇

Eigen库的使用及三维空间刚体运动的变换关系

本例程使用cmake编译工具，分为两个文件夹：源文件src文件夹和编译文件夹build。编译过程如下：

cd build
cmake ..
make

CMakeList.txt源码如下：

 cmake_minimum_required(VERSION 2.8)
 
 project (Eigen)
 
 add_executable(eigen src/eigen.cpp)
  
 include_directories("/usr/include/eigen3")

eigen.cpp源码如下：

#include <iostream>
#include <ctime>
using namespace std;

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

#define MATRIX_SIZE 50

int main (int argc, char** argv)
{
    Eigen::Matrix<float, 2, 3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    matrix_23 << 1,2,3,4,5,6;
    cout << matrix_23 << endl;

    for (int i=0; i<1; i++)
            for (int j=0; j<2; j++)
                    cout << matrix_23(i,j) << endl;

    v_3d << 3,2,1;
    cout << v_3d << endl;

    Eigen::Matrix<double , 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl;

    cout << matrix_33.transpose() << endl;      //转置
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;         //迹
    cout << 10*matrix_33 << endl;
    cout << matrix_33.inverse() << endl;       //逆
    cout << matrix_33.determinant() << endl;   //行列式

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

    //求解Matrix_NN * x = v_Nd;
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock();

    //直接求逆
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << CLOCKS_PER_SEC << endl;
    cout << "time use in normal inverse is " << 1000 * (clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl;

    //矩阵分解，QR分解
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in Qr composition is " << 1000 * (clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl;

    //同一点在在不同坐标系下的坐标位置换算
    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
    q1.normalize();
    q2.normalize();

    Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
    Eigen::Vector3d p1(0.5, 0, 0.2);

    //四元数变换旋转矩阵
    Eigen::Matrix3d q1Rotation_Matrix = Eigen::Matrix3d::Identity();
    q1Rotation_Matrix = q1.toRotationMatrix();
    //欧式变换矩阵
    Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();
    Tc1w.rotate(q1Rotation_Matrix);
    Tc1w.pretranslate(t1);

    //pc = Tcw * pw
    Eigen::Matrix<double, 3, 1> pw = Tc1w.inverse() * p1;

    Eigen::Matrix3d q2Rotation_Matrix = Eigen::Matrix3d::Identity();
    q2Rotation_Matrix = q2.toRotationMatrix();
    Eigen::Isometry3d Tc2w = Eigen::Isometry3d::Identity();
    Tc2w.rotate(q2Rotation_Matrix);
    Tc2w.pretranslate(t2);

    Eigen::Matrix<double, 3, 1> p2 = Tc2w * pw;

    cout << "该向量在q2坐标系下的坐标" << p2 << endl;

    return 0;
}