《视觉SLAM十四讲精品总结》2：Eigen和Sophus
一、Eigen
提供了快速的矩阵线性代数运算，矩阵、向量、旋转矩阵、变换矩阵。
Eigen是纯头文件搭建成的库，使用时只需引入Eigen的头文件即可，不需要链接库文件target_link。
#include CMakeLists.txt需要添加头文件
include_directories("/user/include/eigen3");1、基本运算
声明部分：
//Eigen只关心三个参数：数据类型，行，列
Matrix matrix_23；
//向量3*1
Vector3d v_3d;
//矩阵3*3
Matrix3d matrix_33；
matrix_33=Matrix3d::Zero();
matrix_33=Matrix3d::Random();
matrix_33=Matrix3d::Identity();
//矩阵x*x
MatrixXd matrix_x;
//动态矩阵
Matrix matrix_dynamic; 赋值
matrix_23<<1,2,3,4,5,6;
v_3d<<1,2,3;显式转换(将原来float转为doube)
Matrix result=matrix_23.cast()*v_3d; 矩阵性质
//转置
matrix_33.transpose()
//求和
matrix_33.sum（）
//迹
matrix_33.trace（）
//逆
matrix_33.inverse（）
//行列式
matrix_33.determinate（）
//特征值和特征向量
SelfAdjointEigenSolver eigen_solver(matrix_33.transpose()*matrix_33);
eigen_solver.eigenvalues();
eigen_solver.eigenvectors();
解方程
求解Matrix_NN*x=v_Nd
定义变量
Matrix matrix_NN=MatrixXd::Random(3,3)
Matrix v_Nd=MatrixXd::Random(3,1);
1、求逆
Matrix x=matrix_NN.inverse()*v_Nd;
2、QR分解
x=matrix_NN.colPivHouseholderQr().solve(v_Nd); QR分解也叫正三角分解，矩阵分解为一个正交矩阵和上三角矩阵，通常用来求解线性最小二乘法。
计时
clock_t time_stt=clock();
1000*(clock()-time_stt)/(double)2、几何模块
旋转矩阵:3*3 Matrix3d
旋转向量: 3*1 AngleAxisd
四元数:4*1 Quaterniond
欧拉角: 3*1 Vector3d
欧式变换矩阵： 4*4 Isometry3d
仿射变换： 4*4 Affine3d
射影变换： 4*4 Projective3d
//旋转矩阵R（3*3）
Matrix3d rotation_matrix=Matrix3d::Identity()
//旋转向量，并沿Z轴转45度（3*1）
AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1))
//欧拉角(ZYX顺序，yaw,pitch,roll)（3*1）
Vector3d eular_angles=rotataion_matrix.eularAngles(2,1,0);
//变换矩阵T（4*4）
Isometry3d T=Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Vector3d(1,3,4));
cout<二、Sophus
李代数库，SO(3),SE(3),Sim（3）
#include SO(3)
//旋转向量转为旋转矩阵，沿Z轴转90度
Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
//SO(3)由旋转矩阵构造
Sophus::SO3 SO3_R(R);
//由旋转向量构造
Sophus::SO3 SO3_v(0,0,M_PI/2);
//由四元数构造
Eigen::Quaterniond q(R);//先专成四元数
Sophus::SO3 SO3_q(q)
对数映射获得李代数
//由旋转矩阵取对数得到旋转向量
Vector3d so3=SO3_R.log();
cout<<"so3="<增加扰动模型的更新
Vector3d update_so3(1e-4,0,0);
SO3 SO3_updated=SO3::exp(update_so3)*SO3_R;//左乘更新
cout<SE(3)
//平移向量t
Eigen::Vector3d t(1,0,0);
//构造SE(3)
Sophus::SE3 SE3_Rt(R,t);
Sophus::SE3 SE3_qt(q,t);
//对数映射获得李代数se3
typedef Eigen::Matrix Vector6d;
Vector6d se3=SE3_Rt.log();
cout<<"se3="<