SLAM十四讲第2章编程题

文件存放

.cpp/CMakeLists.txt/png/build都放在主文件夹里，所以执行文件和cmake ../make都在build里

---

2.熟悉Eigen矩阵运算

 QR_Cholesky.cpp

#include <iostream>
using namespace std;
#include <ctime>
//eigen部分
#include <Eigen/Core>
//稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
using namespace Eigenc;

#define MATRIX_SIZE 100


int main( int argc, char** argv)
{
   MatrixXd A;
   MatrixXd B;
   MatrixXd X1;
   MatrixXd X2;
   A = MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);//A
   B = MatrixXd::Random(MATRIX_SIZE,1);//b
   
   //X = A.inverse() * B;求解Ax = B;
   //QR
   clock_t time_stt = clock(); // 计时1
   X1 = A.colPivHouseholderQr().solve(B);
   cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

   //Cholesky
   time_stt = clock(); // 计时2
   X2 = A.llt().solve(B);
   cout <<"time use in Cholesky decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
   
   //output
   cout << "Here is the matix A:\n" << A << endl;
   cout << "Here is the matix B:\n" << B << endl;
   cout << "The X1 soulution is :\n" << X1 << endl;
   cout << "The X2 soulution is :\n" << X2 << endl;
   
   return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( useEigen )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )

add_executable( eigenMatrix QR_Cholesky.cpp )

---

4.几何运算问题

 eigenGeometry.cpp

#include <iostream>
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

using namespace std;

int main(int argc, char *argv[])
{
    Eigen::Quaterniond q_wr(0.55, 0.3, 0.2, 0.2);
    Eigen::Quaterniond q_rb(0.99, 0.0, 0.0, 0.01);
    Eigen::Quaterniond q_bl(0.3, 0.5, 0.0, 20.1);
    Eigen::Quaterniond q_bc(0.8, 0.2, 0.1, 0.1);

    //四元数归一化
    q_wr.normalize();
    q_rb.normalize();
    q_bl.normalize();
    q_bc.normalize();

    Eigen::Vector3d t_wr(0.1, 0.2, 0.3);
    Eigen::Vector3d t_rb(0.05, 0, 0.5);
    Eigen::Vector3d t_bl(0.4, 0, 0.5);
    Eigen::Vector3d t_bc(0.5, 0.1, 0.5);

    Eigen::Vector3d p_c(0.3, 0.2, 1.2);

    Eigen::Isometry3d T_wr(q_wr);
    T_wr.pretranslate(t_wr);
    Eigen::Isometry3d T_rb(q_rb);
    T_rb.pretranslate(t_rb);
    Eigen::Isometry3d T_bl(q_bl);
    T_bl.pretranslate(t_bl);
    Eigen::Isometry3d T_bc(q_bc);
    T_bc.pretranslate(t_bc);

    Eigen::Vector3d p_l;
    Eigen::Vector3d p_w;

    p_l = T_bl.inverse() * T_bc * p_c;
    p_w = T_wr * T_rb * T_bc * p_c;

    cout << "p_c在相机坐标系下的坐标: " ;
    cout << p_c.transpose() << endl; 

    cout << "p_c在激光坐标系下的坐标: " ;
    cout << p_l.transpose() << endl;

    cout << "p_c在世界坐标系下的坐标: " ;
    cout << p_w.transpose() << endl;
    

    return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( geometry )

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

add_executable( eigenGeometry eigenGeometry.cpp )