前端VO：

1. 初始化

相机进行平移运动，然后匹配不同帧的特征点，归一化不同帧之间的t为1，作为尺度因子。然后根据对极约束，采用八点法，估计出E（本质矩阵），F（和E只差一个相机内参），和H（单应矩阵），进而分解E得到R，t。

2. 三角测量：

两帧图像a,b。知道a图像上p1点，知道b图像上对应的 p2点，知道a图像与b图像之间的Rt关系，可以求出三维p点的坐标，即求出该点对两幅图像的深度s1和s2。

3. 3D-2D：PnP

知道n个3D空间点及其投影位置，来估计相机 位姿。如果一张图像上有3个特征点的3D位置已知，就可以估计相机运动。额外再需要一个点对验证结果。单目相机必须先进行初始化，才能使用PnP。

4. BA优化（最小化重投影误差优化）

PnP问题可以构建成一个最小化重投影误差问题。即把3D空间点投影到目标帧图像，根据特征点匹配也能已知其在图像上的真实坐标位置，这样最小化投影与真实位置的误差进行PnP的解算。

BA优化不限于两幅图像，可以多帧图像匹配的位姿进行优化，甚至可以将整个SLAM过程放进来进行优化，这种主要在后端优化时候才会使用。这里只是小型BA优化。

如果上3D-3D的位姿解算，就是ICP（迭代最近点）问题。

5. 直接法

因为特征点提取与匹配耗时，而且有些地方容易缺失。有三个思路可以处理这个问题：

1）只计算关键点，不提描述子，使用 光流法 跟踪特征点运动。其余解算不变，仍使用对极几何，PnP，ICP等算法。

2）只计算关键点，不提描述子，使用 直接法 计算特征在下一帧的位置。

3）既不计算关键点，也不提描述子，根据像素差异直接计算相机运动。

2）和3）都属于直接法。1）因为采用特征点，所以优化方法是最小化重投影误差。2）3）直接法中通过优化最小化光度误差来求解。

直接法推导的推导流程参考《SLAM十四讲》的P195。

直接法分类：（都是在把点P当作已知的空间3D点，双目可以直接给出，单目的需要进行深度估计来得到）

- 稀疏关键点： 稀疏直接法。只使用数百个像素即可。

- 半稠密直接法：像素梯度为0的点使雅可比矩阵为0，没有贡献，只考虑带梯度的像素点。

- 稠密直接法： P为所有像素点，需要GPU加速，很难实时「

 

所以VO流程：

先通过初始化相机运动，得到归一化尺度的相机位姿Rt，然后根据此Rt，采用三角测量方法得到一些特征点的3D空间坐标。有了3D空间坐标，再根据PnP方法，估计相机位姿R t，有了Rt再进行三角测量得到新的3D点，有了新的3D点又可以计算新的Rt。这样一直循环下去。

 

后端：

主要进行优化。投影过程如下：

1）把世界坐标转换到相机坐标，用到相机外参R，t

2）然后将P’投影到归一化平面，得到归一化坐标

3）考虑归一化坐标畸变情况，得到去畸变前的原始像素坐标。

4）根据内参模型，计算像素坐标

这个过程就是 观测方程。