视觉SLAM十四讲 实践笔记（一）

标题# 视觉SLAM十四讲 实践笔记 （一）

这篇文章用于记录我在练习《视觉SLAM十四讲：从理论到实践》中代码部分时，我认为比较重要的语句和我遇到问题和解决方式。
	代码从第三章开始

实践：Eigen

#include<Eigen/Core>
using namespace Eigen;
Matrix<float,2,2> = Matrix22;//定义一个 2x2的矩阵，元素都是float
Matrix3d matrix33; //定义一个3x3矩阵，元素都是double
Matrix22<<1,2,3,4;//初始化的一种方式
Matrix3d matrix33 = Matrix3d::Zero();//
Matrix3d matrix33 = Matrix3d::Random();//

cout<<Matrix22.cast<double>()<<endl;//元素类型的显式转换
cout<<Matrix22.transpose()<<endl;//矩阵转置
cout<<Matrix22.inverse()<<endl;//逆
cout<<Matrix22.determinant()<<endl;//行列式
cout<<Matrix22.trace()<<endl;//迹
cout<<Matrix22.sum()<<endl;//元素和

SelfAdjointEigenSolver<Matrix3d> eigen_solver(Matric33.transpose()*matrix33) //求特征值，
cout <<eigen_solver.eigenvalues()<<endl;//输出特征值
cout <<eigen_solver.eigenvectors()<<endl;//特征向量

//解方程
Matrix< double, MATRIX_SIZE, MATRIX_SIZE > A;
A = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE ); //定义一个 NxN的随机矩阵 A

Matrix< double, MATRIX_SIZE,  1> B;
B = Eigen::MatrixXd::Random( MATRIX_SIZE,1 ); // 定义一个随机向量B 

Matrix<double,MATRIX_SIZE,1> X; 	//定义X

//求 X ，使AX=B
Matrix<double,MATRIX_SIZE,1> x = A.inverse()*B;//方法1，直接求逆矩阵
X = A.colPivHouseholderQr().solve(B);//方法2，用QR分解求解，就类似化成上三角矩阵那种 ，比法1快

实践Eigen/Geometry

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
//定义旋转向量，旋转矩阵，四元数，欧式变换矩阵
Matrix3d rotation_matrix = Matrix3d::Identity(); //旋转矩阵就是3X3矩阵
AngleAxisd rotation_vector( M_PI/4, Eigen::Vector3d ( 0,0,1 ));//旋转向量为旋角度+旋转轴
AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
Quaterniond q = Eigen::Quaterniond ( rotation_vector ); //利用旋转向量初始化四元数
Quaterniond q1(0.1,0.1,0.1,0.1);//直接定义四元数

Isometry3d T1=Isometry3d::Identity(); //定义一个欧式变换矩阵
T1.rotate ( rotation_vector ); //用旋转向量初始化旋转部分
T1.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );//用一个向量初始化平移部分

Isometry3d T2=Isometry3d::Identity(); //定义一个欧式变换矩阵
T1(q1); //用四元数初始化旋转部分
T1.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );//用一个向量初始化平移部分

实际坐标变换例子

3.6.2里举了一个清晰简单的转换例子：

已知两个观测者A,B,他们在世界坐标系(指向我的箭头，我的右手，我头顶)里，需要两个量来描述其姿态：
a）位置坐标 p(x,y,z)
b) 在p点处的姿态 (即旋转) ，可以用四元数来描述（s,v）,s是旋转的角度，是个标量，v是旋转轴的方向，是个矢量。

现在有一在天上飞的西瓜P,这两个观测者观察的角度，位置不一样，他们对这个西瓜的位置感觉肯定也不一样。A认为西瓜在PA方向，B认为西瓜在PB方向。PA，PB，是以他们为起点，西瓜为终点的向量。

此时我，站在世界的中心(我的观测角度是世界坐标系)，看到的西瓜也和他们不一样，除非他们此时站在我身上。

一个神奇的矩阵T,是一个欧式变换阵，需要两个元素来初始化:某个观测者的位置，某个观测者的姿态
T_Aw 表示用观测者A的位置和姿态初始化的欧式变换阵。
T_Bw 表示用观测者B的位置和姿态初始化的欧式变换阵。
T_Aw和T_Bw都是4x4的矩阵。

T_Aw 的意思是 world to A Transform，世界坐标系到A坐标系的转换，是从右往左读的。
那么，(T_Aw)^-1 即转换矩阵的逆矩阵，就是反过来的意思年。

所以有如下转换：
PA = T_Aw * P // 我看到的 左乘 T_Aw ,就变成 A 看到的。
P = (T_Aw)^-1 PA //A看到的 左乘 (T_Aw)^-1 ,就变成 我 看到的。

这到例题是，已知A 看到西瓜在PA, 问B看到了什么？

Anwser : PB = T_Bw * P = T_Bw * (T_Aw)^-1 PA

今天的笔记先到这里。