视觉SLAM
视觉SLAM
特征点法
特征点提取+匹配
ORB = Fast + ORiented Brief
特征点：Fast10(9/11)，即如果周围连续10个点都亮/暗于中心参考点，则该点为特征角点。
ORiented Brief Descriptor: Brief有一个固定pattern，比较pattern线段两端大小0/1，比如128线段会出128bit的descriptor，descriptor之间的相似性通过hamming distance来度量。
匹配：通常会直接暴力匹配(brute-force)，加入一些规则过滤明显距离远的来加速。
2D-2D对极几何
单目相机本身无法获取到3D点，需要通过三角化的方式获取到没有尺度的3D点，这个过程叫做初始化，几何模型与双目测距的模型相同都是用得对极几何，不同的是双目测距已知基线b，目的是直接用相似三角形解出深度z，而单目SLAM初始化利用八点法将E矩阵（3x3=9，再减去一个尺度因子）解出，再通过\(E=tR\)构造出\(R\),\(t\)的解。值得注意的是通常我们获取到的特征点pair数远大于8点，会使用RANSAC除点一些outlier，得到的inlier点数也远大于8点，再使用least square解出最终的E矩阵。E矩阵的尺度因子通常会作用于平移矩阵t，所以会对t进行尺度约束\(t = t / norm(t)\)。但整个过程还是会存在尺度漂移导致后期的尺度与前期不一致。
补充：E是本质矩阵，F是基础矩阵，\(F=K^{-T}EK^{-1}\)。
3D-2D PnP
PnP (perspective n points)，已知一组3D点与另外一组对应的2D点，求R、T变换。就用场景已经初始化过的单目SLAM或者RGBD-SLAM。存在两种线法：
(a) DLT（directly linear transform），即根据\(sx=TP\)（x为归一化坐标，T=R|T，P为对应3D点），通过6组(x, P)pair能构造12个方程，将T的3x4=12个参数解出。实际挑选6个pair很关键，通常噪声较大，只适合作为一个(b)BA(bundle adjustment)的优化初始值。
(b) BA(bundle ajustment)，即连续的几组T（so3空间下）及观察值x（像素坐标）经过重投影误差可以构造一个约束方式，通过非线性优化方法可以解出这一系列T(so3下)（通常用Gaussian Newton、Levenberg等方法）。
3D-3D ICP
ICP （iterative closet point），已经两组点云，求位姿变换T。分两种情况：
已知对应关系(RGBD-SLAM)：解法相对简单，线性解可以通过质心间向量直接算出t，再除去质心情况下使用SVD法可以直接解出R。也可以用非线性优化解出，结果一致。
不知对应关系（Lidar-SLAM）：匹配点未知，使用非线性优化解，极值不一定最小。
光流法
特征点法很经典，但存在 (a)特征点描述子计算与匹配耗时；(b)当特征点数减少时（弱纹理区域），能够形成match的pair数更快减少。
光流法基本理论支持：亮度不变假设，即同一个特征点在前后两帧亮度不近似相等。
\(I(x+dy, y+dy, t+dy) = I(x, y, t)\)
泰勒一阶展开，整理得到下式，其中\(I_{x}\), \(I_{y}\), \(I_{t}\)分别为该Pixel位置在x, y, t方向的梯度。x, y的梯度可以直接通过\((I(x+1) - I(x-1))/2\)得到，而t方向梯度就是两张图像同一个pixel位置的亮度变化。最终得到一个关于u, v的方程。
\(I_{x}u + I_{y}v + I_{t} = 0\)
同时, 该Pixel附近的点速度(u, v)应该一致，故可以在(w, w)的小窗口里面找到其它几个方程组成一个超定方程组。使用最小二乘法来解。也可以使用一些二阶的方法来快速求解。
说明
(1) 有些特征点由于相机旋转或者运动过快的原因，有可以变化非常大，这时候可以用图像金子塔等trick来增强跟踪鲁棒性。
(2) 光流法通过optical flow tracking直接获取到特征点之间的matching关系，好处是当特征点比较少的时候track到的基本就是对应的match数量。光流法获得特征点pair，后续位姿估计仍需要对极几何初始化或者PNP/ICP等步骤。
(3) 光流迭代求解过程，依赖于第一帧图像梯度，这个是不变的，一次性计算出，不用迭代每步都算。
直接法
光流法基于：(1)亮度不变假设。(2)特征patch间仅有平移(dx, dy)。
实际上特征点之间仍有scale与rotate等变换，那我们能不能直接利用相机几何变化作为前后pair的对应关系？这就引出了直接法，“直接”是说通过优化相机变换T（R|t）来直接满足“亮度不变假设”，从而不仅获得了pair之间的match关系，连后续的PNP/ICP或者bundle ajustment都省略了，直接优化出位姿。
理论模型（最小化光度误差）
优化函数：\(e = min(I(p_2) - I(p_1))\)
\(q = T x P\)
\(p_2 = \frac{1}{Z_2}Kq\)
即获得亮度误差\(e\)关于T的函数，由于T属于SO3空间，需要转换成so3的李代数使用扰动模型优化。
此外，\(\frac{\partial I}{\partial p2}\)可由像素梯度获得；\(\frac{\partial p2}{\partial q}\)可由\(Z_2p_2 = Kq\)求导获得。
直接法优点：
(1) 相对于特征点法不需要提取角点特征与Match，直接出Pose，速度快很多。
(2) 当有些没有明显角点的场景，直接法仍能work，例如白墙没有角点但有一些光线渐变的特征。
(3) 具有恢复稠密或者半稠密的深度图的能力。
直接法缺点：
(1) 依赖于像素局部梯度寻找位姿优化方向，但图像亮度变化是非凸函数，导致优化过程容易掉入sub-optimal解。
(2) 基于亮度不变假设，但有些场景存在相机过曝或者亮度明显变化的情况会失效。
一些细节问题
计算单应矩阵H至少需要多少对点？=> 4对点：因为每对点可以构造2个方程，根据单应矩阵是3x3的矩阵，除去尺度后自由度为8，4对点可以构造8个方程解出H。但E的计算需要8对点，因为每对点只能构造一个方程（简单理解）。
PnP至少需要几个点可以解？DLT的6对特征点：T=[R|t]包含12个未知参数，每对特征点可以构造两个方程，所以6对特征点可以解出T，但这里的T不属于SE3，需要将R使用QR分解投影到SO3流型上作为后处理。P3P使用3对特征点求出：利用相似三角形将出在相机2上的坐标，再使用ICP之类的方法可以算出相机1到相机2之间的变换。
使用世界坐标点转换像素坐标（有畸变的情况）？图像=>3D点：一般先将图像去畸变，再按照去畸变之后的点到3D空间的反投影关系。3D点=>图像：得到归一化坐标之后，主要考虑径向畸变（可以看作极坐标系下极径发生了变换）通常使用k1, k2, k3来拟合，切向畸变（可以看作成像平面与镜片不平行）通常使用p1, p2两个参数来拟合。最终去畸变后的坐标可以使用畸变前的坐标x, y利用k1, k2, k3, p1, p2的多项式表达式算出。
如果不rectify如何特征点的深度？=> 三角化，核心是对极几何，可以通过解出E或者H，再得到R、t。该过程是VIO的初始化部分。
工程问题
安装sophus
需要从github上下载安装fmt
由于安装ros，系统中存在两个版eigen，以ros中的为准，所以每个cmake的时候，需要 |
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
