视觉SLAM十四讲：三维空间刚体旋转平移相关课后程序理解

1. Eigen库基础应用

//
// Created by g214-j on 18-7-12.
//

#include <iostream>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

using namespace std;

#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;

    // 错误的数据类型
    //Eigen::Matrix<double,2,1> result_wrong_type = matrix_23*v_3d;
    // 应该先进行显式的转换
    Eigen::Matrix<double,2,1> result = matrix_23.cast<double>()*v_3d;

    // 错误的维度
    //Eigen::Matrix<double,2,3> result_wrong_dimension = matrix_23.cast<double>()*v_3d;

    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;
    cout<<x.transpose()<<endl;

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

    time_stt = clock();
    x = matrix_NN.llt().solve(v_Nd);
    cout<<"time use in ldlt is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
    cout<<x.transpose()<<endl;

    // 解方程
    // 我们求解 matrix_dynamic * x = v_dynamic 这个方程
    // 创建动态矩阵
    Eigen::MatrixXd matrix_100_100;
    Eigen::MatrixXd matrix_100_1;

    matrix_100_100 = Eigen::MatrixXd::Random(100,100);
    matrix_100_1 = Eigen::MatrixXd::Random(100,1);

    Eigen::MatrixXd matrix_100_1_result;

    // QR分解
    time_stt=clock();
    matrix_100_1_result = matrix_100_100.colPivHouseholderQr().solve(matrix_100_1);
    cout<<matrix_100_1_result.transpose()<<endl;
    cout<<"time use in Qr(100*100) decomposition is "<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;

    // Chlesky分解
    time_stt=clock();
    matrix_100_1_result = matrix_100_100.llt().solve(matrix_100_1);
    cout<<matrix_100_1_result.transpose()<<endl;
    cout<<"time use in Cholesky(100*100) decomposition is "<<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    Eigen::MatrixXd matrix_5_5 = Eigen::MatrixXd::Random(5,5);
    cout<<matrix_5_5<<endl<<endl;

    // 块操作
    matrix_5_5.block(1,1,3,3)<<1,0,0,
                                0,1,0,
                                0,0,1;
    cout<<matrix_5_5<<endl;

    return 0;
}

2. Eigen库几何模块应用

//
// Created by g214-j on 18-7-12.
//

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

using namespace std;


int main(int argc,char** argv){

    // 旋转矩阵和旋转轴的创建和初始化
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector( M_PI/4, Eigen::Vector3d(0,0,1) );
    cout.precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix()<<endl;        // 以旋转矩阵的形式输出旋转轴
    rotation_matrix = rotation_vector.toRotationMatrix();               // 调用模板类内的函数实现旋转轴转旋转矩阵

    // 用旋转轴实现旋转变幻
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 用旋转矩阵实现旋转
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角<-->旋转矩阵
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); // 欧拉角顺序为 ZYX
    cout<<"yaw-pitch-roll = "<<euler_angles<<endl;

    // 欧式变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();        // 虽然看起来像是3*3矩阵，实质上是4*4矩阵
    T.rotate(rotation_vector);              // 按照旋转轴进行旋转
    T.pretranslate(Eigen::Vector3d(1,3,4));     // 设置平移向量为（1，3，4）
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;

    // 使用变换矩阵实现坐标变换(看一下这里)
    Eigen::Vector3d v_transformed = T * v;
    cout<<"v transformed = "<<v_transformed.transpose()<<endl;

    // 仿射和射影变换使用Eigen::Affine3d和Eigen::Projectived3d

    // 四元数
    // 旋转轴<-->四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;  // q的输出顺序是（x,y,z,w），实部在后
    // 旋转矩阵<-->四元数
    q = Eigen::Quaterniond(rotation_matrix);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;

    // 使用四元数旋转向量
    v_rotated = q * v;  // 这里重载了，实际上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}

3. 课后程序小萝卜位姿问题

//
// Created by g214-j on 18-7-15.
//

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

using namespace std;

int main(int argc, char** argv){

    // 创建旋转矩阵
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 创建旋转轴
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));

    cout.precision(3);
    // 旋转轴以旋转矩阵形式输出
    cout<<"rotation matrix = \n"<<rotation_vector.matrix()<<endl;
    // 将旋转轴转换成旋转矩阵
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用旋转轴进行旋转变换
    Eigen::Vector3d v(1,0,0);   // 待旋转的点
    Eigen::Vector3d v_rotation = rotation_vector * v;   // 旋转之后点的坐标
    cout<<"(1,0,0) after rotation = "<<v_rotation.transpose()<<endl;
    // 用旋转矩阵进行旋转变换
    v_rotation = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotation.transpose()<<endl;


    // 将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);  // ZYX顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧式变换矩阵T
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();        // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate(rotation_vector);                  // 旋转部分R：按照rotation_vector进行旋转
    T.pretranslate(Eigen::Vector3d(1,3,4));     // 平移部分t：把平移向量设成(1,3,4)
    cout<<"Transform matrix = \n"<<T.matrix()<<endl;    // 以变换矩阵的形式输出

    // 用变换矩阵进行变换
    Eigen::Vector3d v_transformed = T*v;            // 相当于进行了R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 由旋转轴得到四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;      // 这里coeffs输出的顺序是（x,y,z,w）
    // 由旋转矩阵得到四元数
    q = Eigen::Quaterniond(rotation_matrix);
    cout<<"quaternion = \n"<<q.coeffs()<<endl;
    // 使用四元数进行旋转操作
    v_rotation = q*v;
    cout<<"(1,0,0) after rotation = "<<v_rotation.transpose()<<endl;

    // 小萝卜问题
    // 已知:  q1=[ 0.55, 0.3, 0.2, 0.2],t1=[0.7, 1.1, 0.2]      注:q的第一项为实部
    //       p1=[0.5, -0.1, 0.2]
    //        q2=[ -0.1, 0.3, -0.7, 0.2],t2=[-0.1, 0.4, 0.8]
    // 求解:  p2

    Eigen::Quaterniond q1(0.55, 0.3, 0.2, 0.2);
    Eigen::Quaterniond q2(-0.1, 0.3, -0.7, 0.2);
    q1 = q1.normalized();
    q2 = q2.normalized();
    Eigen::Vector3d t1(0.7, 1.1, 0.2);
    Eigen::Vector3d t2(-0.1, 0.4, 0.8);
    Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();     // Tc1w:世界坐标系到相机1坐标系的变换关系
    Eigen::Isometry3d Tc2w = Eigen::Isometry3d::Identity();     // Tc2w:世界坐标系到相机2坐标系的变换关系
    Tc1w.rotate(q1);
    Tc1w.pretranslate(t1);
    cout << "Transform matrix = \n" << Tc1w.matrix() << endl;
    Tc2w.rotate(q2);
    Tc2w.pretranslate(t2);
    cout << "Transform matrix = \n" << Tc1w.matrix() << endl;

    Eigen::Vector3d p1(0.5, -0.1, 0.2);
    p2 = Tc2w * Tc1w.inverse() * p1;                        //p1=Tc1w*pw+t1; p2=Tc2w*pw+t2;
    cout<<"在相机2下的坐标为: "<<p2.transpose()<<endl;

    return  0;
}