SLAM或VO中两帧间匹配的方法汇总

2D-2D:对极几何

A.对极约束

@输入:参考帧和当前帧的2D点坐标对(一般8对),内参
@输出:运动恢复R t

对极几何约束
参考帧和当前帧的二维像素点通过特征点法拿到了匹配关系,那么从各自光心出发的两条射线交点即3D点,P和两个光心构成了极平面,$e1$ \ $e2$为极点,$l1$ \ $l2$是极线(Epipolar line)。
对极几何必须要有正确的匹配关系,否则当前帧无法找到参考帧$p1$对应的点在$p2$,那样的话就要用极线搜索了。
对极几何求R t的步骤是:
1.根据匹配的像素点对和内参求出基础矩阵F或者本质矩阵E。E=t^ R,只和运动有关,所以是我们求的本质,平移和旋转各3自由度,单目尺度歧义性自由度-1,所以E有5个自由度,一般用八点法,8对点求;F还包括内参$$F=K^{-T}EK^{-1}$$.
2.根据E或者R恢复R t。这里一般用SVD分解恢复出4个解,然后取点在相机前方的那个解。

B.单应矩阵

@输入:参考帧和当前帧的2D点坐标对(一般4对),内参
@输出:运动恢复R t
Homography描述两个平面之间的映射关系,自由度为8。可以通过4对不共线的点得出。一般配合RANSAC。
退化(degenerate):特征点共面or纯旋转,基础矩阵的自由度下降(比如平移那块没了),就出现了退化。
ORB中H 和F都算,取好的。

没有特征匹配下的2D-2D:极线搜索

极限搜索
深度滤波器:用多次三角测量让深度估计收敛
直接法时候没有特征点匹配,所以需要用极限搜索,如DSO中ImmaturePoint::traceOn()。DSO论文中没有过多提及,深度滤波器部分可以看SVO比较详细。
@输入:Frame_ref, Frame_curr, R, t, 参考帧点像素位置和深度估计区间, 内参
@输出:当前帧对应点像素位置,点的深度和不确定度。

这时候的前提是已知Rt(可以是估计出来的,也可以是静止模型、匀速模型、随机模型之类的),思路是:
这时候只知道参考帧的点像素坐标和深度区间,也知道当前帧的极线,$p1$深度区间投过来后限定了极线的搜索范围,于是在范围内用块匹配之类的方法评估NCC(Normalized Cross Correlation)找到相似度最高的点,如果只把这个点当作最终结果是不行的,很容易陷入局部极值。所以需要用滤波的方法多次搜索直到收敛。所以过程是:
1.假设像素的深度满足高斯分布,通过极限搜索确定当前帧像素点对应位置
2.根据几何关系三角化计算深度和几何不确定性(DSO还加上了光度不确定性)
3.根据深度和不确定性融合进上一次的估计中,DSO中是GN迭代,直到深度收敛。

三角测量Triangulation

@输入:参考帧和当前帧的2D点坐标对,恢复出的R t
@输出:空间点的位置(即参考帧和当前帧点对应的深度s1 s2)
在用2D-2D的方法恢复出Rt后,还需要用运动估计点的空间位置。
三角化
理论上两个射线会交于点P,但无论是匹配误差还是运动估计误差等噪声都会导致无法相交,所以一般用最小二乘的思路解点的位置。最后求得两帧系下的位置深度s1和s2。三角测量必须要有一定的平移。(CSDN里面 \^{}打不出反协号)
$$s_1x_1 ^ \nu x_1=0=s_2 x_1 ^\nu R s_2+x_1^\nu t$$

3D-2D:PnP

@输入:参考帧下的特征点像素坐标和3D坐标,匹配到当前帧的2D点像素坐标,内参
@输出:运动估计R t
Perspective-n-Point,当知道了n个空间点和其投影位置的情况下求解位姿。3D-2D比2D-2D简单的多,最少只需要3对点,不过他需要特征点的3D位置,可以由三角化or RGB-D or lidar提供,因此在双目和RGB-D中就很方便,单目中要先初始化后才能用PnP。需要注意的是:3D点的位置应该是在参考帧坐标系下的,实际上等价于在世界坐标系下已知(因为参考帧对世界系的T已知),而当前帧的空间坐标是不知道的,只知道参考帧特征点对应的2D像素坐标,否则就成了3D-3D ICP了。
PnP有很多方法,一般先用P3P/EPnP估计一个初值,然后用非线性优化的方法即BA refine:
pnp
当前已知p1 p2坐标和P在左图下的3D点,这时候BA优化重投影误差,不断更新R t,让P在右图的投影p2^尽量接近p2,即可用G-N L-M等优化方法求出R t。(这里可以看看其中的雅可比推导),BA的目标函数:
$$ \xi^* = \mathop{\arg\min}_{\xi} \ \ \frac{1}{2} \sum | u_i - \frac{1}{s_i}Kexp(\xi^\nu)P_i |^2_2 $$
即重投影误差,u是当前帧像素坐标即p2,Pi是参考帧下的3D点齐次坐标,这里的1/s实际上就是在K投影前将当前系下估计出的3D坐标归一化,所以变量只有李代数。

3D-3D ICP

@输入:配对好的一组3D点对,即那坨点云分别在参考帧下和当前帧下的坐标
@输出:运动恢复R t
这时候不用输入相机内参,3D-3D和相机模型无关,所以lidar也可以直接用,视觉和lidar的ICP区别是:视觉可以通过特征点去匹配2D点然后找到对应的3D点对,但是lidar有时比较难匹配特征点对,就认为距离最近的两个点是同一个点,即最近邻,当然这个点也是有一定的几何特征的特征点;当然lidar也可以用强度匹配之类的。
BA的目标函数:p p’为两对输入,变量为李代数
$$ \xi^* = \mathop{\arg\min}_{\xi} \ \ \frac{1}{2} \sum | p_i - exp(\xi^\nu)p_i ‘ |^2_2 $$