视觉slam14讲之欧式变换与Eigen库

1.欧式变换

   在视觉slam中，相机是运动定位，构图的。而它是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化，这种变化称为欧式变换。例如: 手机抛向天空，在摔碎之前，只可能有空间位置和姿态的不同，而它自己的长度，各个面的角度等性质不会有任何变化。这样一个欧式变换由一个旋转和一个平移两部分组成。
直接给出欧式空间的坐标变换关系：

$$a' = 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;
 }

如上打出的代码段。