SLAM学习笔记(Code2)----刚体运动、Eigen库

1
2
3
4
5
6
#include <Eigen/Core>//Core:核心
#include <Eigen/Dense>//求矩阵的逆、特征值、行列式等
#include <Eigen/Geometry>//Eigen的几何模块，可以利用矩阵完成如旋转、平移
/***其他***/
#include <ctime>//可用于计时，比较哪个程序更快
#include <cmath>//包含abs(),sin(),exp(),log()等等

1
2
3
4
5
Eigen::Matrix<float, 2, 3> matrix_23;//普通矩阵，Eigen::Matrix<数值类型，行，列> 矩阵名；
Eigen::Vector3d v_3d;//向量矩阵，3d指3*1的double型阵，3f就是float型，但是最多vector4d，没有5d
Eigen::Matrix3d matrix_33;//方阵
Eigen::Martix<float,Eigen::Dynamic,Eigen::Dynamic>matrix_dynamic;//动态矩阵，行列自适应
Eigen::MartixXd matrix_xx;//也是动态矩阵

1
2
3
4
5
6
7
8
9
10
Eigen::Matrix<float,2,3>matrix_23;//定义一个2*3阵
matrix_23 = Eigen::Matrix<float,2,3>::Zero();//初始化全0
Eigen::Matrix<float,2,3>matrix_23(Eigen::Matrix<float,2,3>::Zero());//定义时初始化0，直接用等于也可以
Eigen::Vector3d v_3d(Eigen::Vector3d::Zero());
matrix_23(1,1) = 2.0;//直接访问相应位置的元素，可以利用for循环遍历
matrix_23 << 1,2,3,4,5,6;//一次性赋值，注意不要少元素
v_3d << 3,2,1;
matrix_23 = Eigen::Matrix<float,2,3>::Random();//赋随机值
Eigen::Matrix3d matrix_33(Eigen::Matrix3d::Random());
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Identity();//初始化为单位矩阵

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/***矩阵乘法***/
Eigen::Matrix<float,2,3>matrix_23(1,2,3,4,5,6);
Vector3d v_3d(1,2,3);
//直接相乘会出错，因为数据类型不一样，所以先转变成相同的数据类型
Eigen::Matrix<double,2,1>matrix_result = matrix_23.cast<double>() * v_3d;//结果的维度不要搞错了
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>matrix_result2 = matrix_23.cast<double>() * v_3d;//利用动态矩阵存放结果就不用考虑结果是几维矩阵了
//matirx_23 = matirx_23.cast<double>();是错误的
/***特殊计算***/
转置        .transpose()
各元素和    .sum()
取迹        .trace()
数乘        num * matrix
逆          .inverse()
行列式      .determinant
Eigen::Matrix3d matrix_3d(Eigen::Matrix3d::Random());
cout<<matrix_23.transpose()<<endl;
cout<<matrix_3d.sum()<<endl;
cout<<matrix_3d.trace()<<endl;
cout<<2*matrix_3d<<endl;
cout<<matrix_3d.inverse()<<endl;
cout<<matrix_3d.determinant()<<endl;
/***求特征值、特征向量***/
//如何定义一个一定可以相似对角化的矩阵？——实对称矩阵
Eigen::Matrix3d eigen_solver1 = matrix_3d * matrix_3d.transpose();//这个是是对称矩阵，但不能利用.eigenvalues()和.eigenvectors()分别求特征值和特征向量
//利用SelfAdjointEigenSolver<Matrix_type> Matrix_name;进行定义，但是这个矩阵不能直接访问
Eigen::SelfAdjointEigenSolver<Matrix3d>eigen_solver2(matrix_3d * matrix_3d.transpose();
cout<<eigen_solver2.eigenvalues()<<endl;//特征值
cout<<eigen_solver2.eigenvectors()<<endl;//特征向量

1
2
3
4
5
6
7
/***定义矩阵***/
#define MATRIX_SIZE 50;
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::VectorXd::Random(MATRIX_SIZE,1);

1
2
3
4
5
#include<ctime>
float time_1 = 0.0;
clock_t time_stt = clock();//clock_t是一个长整型的数据类型，clock()记录现阶段CPU时钟计时单元数
//假设中间运行了一段程序
time_1 = 1000(clock() - time_stt)/(double)CLOCK_PER_SEC;//CLOCK_PER_SEC表示每秒有几个计时单元，这里的单位是ms，time_1就是运行程序花费的时间

1
2
3
clock_t time_stt = clock();
Eigen::Matrix <double,MATRIXSIZE,1> x = matrix_NN.inverse() * v_Nd;
cout<<"程序花费时间为"<<endl<<1000*(clock() - time_stt)/(double)CLOCKS_PER_SEC;

1
2
3
time_stt = clock();
x = matrix_NN.ColPivHouseholderQr().solve(v_Nd);//matrix_name.ColPivHouseholderQr().solve(v_name)
cout<<"程序花费时间为"<<endl<<1000*(clock() - time_stt)/(double)CLOCKS_PER_SEC<<endl;

1
2
3
4
//想要调试程序，需要在CMakeLists中添加以下语句
set( CMAKE_BUILD_TYPE "Debug" )//这样才能实现单步运行
//程序正常运行则需要改为Release模式
set( CMAKE_BUILD_TYPE "Release" )

1
cout << matrix_23 << endl;

1
2
/***CMakeLists.txt中添加***/
include_directories("usr/include/eigen3")

1
2
include_directiories(/usr/include/库头文件夹)
target_link_libraries(myprogram path/库文件.so)

1
2
3
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIR})
target_linked_libraries(myprogram ${OpenCV_LIBRARIES})