視覺slam14講之歐式變換與Eigen庫

1.歐式變換

   在視覺slam中，相機是運動定位，構圖的。而它是一個剛體運動，它保證了同一個向量在各個座標系下的長度和夾角都不會發生變化，這種變化稱爲歐式變換。例如: 手機拋向天空，在摔碎之前，只可能有空間位置和姿態的不同，而它自己的長度，各個面的角度等性質不會有任何變化。這樣一個歐式變換由一個旋轉和一個平移兩部分組成。
直接給出歐式空間的座標變換關係：

a′=Ra+t$$a^{\prime }=Ra+t$$

    矩陣R描述了旋轉本身，故又稱爲旋轉矩陣。
    t 則代表平移t。
   其中R的推導如下：
        對於某一單位（e1, e2, e3）經過一次旋轉變成（e1’ , e2’ , e3’）。對於同一個向量a, 它在兩個座標下的座標爲[a1, a2, a3]和[a1’ ,a2’ ,a3’]. 則有：

      爲了很好的對比兩個座標之間的關係，我們對上述等式的左右同右乘[e1T, e2T, e3T]。則

        這個R就稱之爲旋轉矩陣，可知旋轉矩陣有些特別的性質，它是一個行列式爲1的正交矩陣。反之，行列式爲1的正交矩陣也是一個旋轉矩陣。

2.Eigen庫

   Eigen是一個C++開源線性代數庫，它提供了快速的有關線性代數運算，還有解方程等功能。許多上層的軟件庫也是使用Eigen進行矩陣運算，包括g2o, Sophus等。

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

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

#define MATRIX_SIZE 50

 int main(int argc, char **argv)
 {
    //Eigen 以矩陣爲基本數據單位。它是一個模板類。前三個參數爲：數據類型，行，列
    //聲明一個2x3的float矩陣
    Eigen::Matrix<float,2,3> matrix_23;
    /*Eigen通過typedef提供了許多內置類型，不過底層仍然是Eigen::Matrix
     *例如：Vector3d 實質上是Eigen::Matrix<double,3,1>
     */
    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;
    Eigen::Matrix<double,2,1> result = matrix_23.cast<double>()*v_3d;
    cout << result << endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl <<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;

    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 << "time use in normal invers is " << 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl;

    time_stt = clock();
    x = matric_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in Qr compsition is " << 1000* (clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl;

    return 0;
 }

如上打出的代碼段。