[MVG] 鲁棒估计: RANSAC & 鲁棒核函数

在VSLAM中，我们首先构造3D-3D、3D-2D、2D-2D匹配，然后据此去估计相机的运动。完美估计需要完美的匹配，但实际的匹配中往往存在很多错误。如何消除或者降低错误匹配的影响呢？一种方法是选择那些正确的匹配进行估计（RANSAC），另一种是降低那些错误匹配的权重（鲁棒核函数），下面分别介绍。

1. RANSAC随机采样一致算法

这种方法的目的是，从所有数据中选择正确的数据，用于估计。为了方便，先给几个定义。

点：每一个数据，SLAM里指的是匹配的点对野值/外点：错误的点内点：正确的点内点集：内点的集合外点集：外点的集合模型：带估计的参数：估计模型所需要的最小点数：所有的点形成的点集

显然，内点集就是我们想要找的正确数据。RANSAC的想法是从点集中随机的选择出 $s$ 个点，估计出一个模型，查看剩余点是否符合这个模型。如果大部分点都符合这个模型，就相当于我们找到了一个合适的模型，自然的符合模型的点就是内点啦。由于是随机选择，一次可能难以找到正确的模型，因此需要多次才可以。下面给出完整的RANSAC算法。

RANSAC 算法步骤：

从数据集中随机选择 $s$ 个点作为样本，估计模型。
计算所有点到模型的距离，得到内点集 $I$ 。
如果 $|I|>T$ ，用全部内点重新估计一个更精确的模型。
如果 $|I|\leq T$ ，则重复步骤 1。
经过 $N$ 次试验后，选择最大的内点集，并用其中的全部点重新估计模型。

RANSAC里面有三个重要的参数需要确定，分别是 $t$ 、 $T$ 、 $N$ 。

1.1 内点阈值 $t$

根据模型，每个点可以计算出一个距离的平方 $d^2$ ，利用阈值 $t^2$ 就可以识别该点为内点还是外点了。 $d^2$ 的选择就要根据具体模型去选了，一般直线方程估计中采用点到线的距离的平方，基本矩阵估计中也使用点到极线的距离的平方，单应和相机内参估计中使用点到点的距离的平方。m个独立标准正太分布的平方和服从m自由度的卡方分布，选择阈值 $t$ 使得点为内点的概率是 $\alpha$ 。就可以基于下式判断是否为内点

\left\{ {\begin{array}{l} {Inlier}&{{d^2} < {t^2}} \\ {Outlier}&{{d^2} \geqslant {t^2}} \end{array}, \ where,{t^2} = {F^{ - 1}}} \right.(\alpha ){\sigma ^2} \\

一般取 $\alpha=95\%$ 。 $t^2$ 的取值如下表

文章配图

1.2 采样次数 $N$

最多采样多少次就结束呢？一个点为内点的概率为 $w$ ，为外点的概率为 $\varepsilon = 1-w$ 。可以根据概率计算一下。我们可以要求

事件A：在次采样中，至少要有一次选取的子集内没有外点的概率。事件A的逆事件为：次采样中，所有选取的子集内至少存在一个外点。这个概率为。那么

N= \frac{log(1-p)}{log(1-w^s)} =\frac{log(1-p)}{log(1-(1-\varepsilon)^s)} \ (1) \\

需要注意：

采样次数与内、外点的比例有关系，而与数据的量级无关系。采样次数随着给最小点数的增大而增大。

1.3 内点数量阈值 $T$

当我们发现内点的数量足够多的时候就可以提前结束采样。内点数量的阈值 $T$ 如何决定呢？我们一般认为，在给定外点的比例 $\varepsilon$ 时，对于数据个数为 $n$ , $T=(1-\varepsilon) n$ 。这个比例可以估计的稍微保守一些。

1.4 自适应的决定采样次数

从式(1)可以看出，采样次数 $N$ 与内点概率 $w$ 有关系。但实际情况是，一组数据中有多少内点，事先是不知道的。

开始时可以设 $w=0$ ，这时候 $N=\infty$ 。经过一次采样后，设获得的内点个数为 $n_i$ ,可以更新内点概率为 $w=\frac{n_i}{n}$ ，从而可以再更新一次 $N$ 。如此，每次迭代，如果发现内点个数增加，就更新 $N$ 。详细算法如下：

w = 0
sample_count = 0
while sample_count < N
    随机选取一个样本，并计算内点数 n_i
    if n_i 增加
        w = n_i / n
        根据 w 更新 N
    sample_count = sample_count + 1
end

1.5 实例-拟合2D直线

模型

直线的一般方程

Ax+By+C=0 \\

两边同时除以 $C$ 变成

A^\ast x+B ^\ast y +1 = 0 \\

两个点 $(X_1, Y_1,), (X_2, Y_2)$ 可以确定一条直线。带入到上式有

\left[ {\begin{array}{c} {{X_1}}&{{Y_1}} \\ {{X_2}}&{{Y_2}} \end{array}} \right]\left[ {\begin{array}{c} {{A^ * }} \\ {{B^ * }} \end{array}} \right] = \left[ {\begin{array}{c} { - 1} \\ { - 1} \end{array}} \right]\\

可以解出来

\begin{aligned} \left[\begin{array}{c}A^*\\B^*\end{array}\right] &= \left[\begin{array}{cc}X_1&Y_1\\X_2&Y_2\end{array}\right]^{-1} \left[\begin{array}{c}-1\\-1\end{array}\right] \\ &= \frac{1}{X_1Y_2-X_2Y_1} \left[\begin{array}{cc}Y_2&-Y_1\\-X_2&X_1\end{array}\right] \left[\begin{array}{c}-1\\-1\end{array}\right] \\ &= \frac{1}{X_1Y_2-X_2Y_1} \left[\begin{array}{c}Y_1-Y_2\\X_2-X_1\end{array}\right] \end{aligned}

注意到 $A，B，C$ 是up to scale的，所以可以令

\begin{aligned} A &= Y_1-Y_2 \\ B &= X_2-X_1 \\ C &= X_1Y_2-X_2Y_1 \end{aligned}

这就是两点确定一个条直线一般方程的方法。

内点距离

内点的距离取点到直线的距离

{d^2} = \frac{{{{\left\| {Ax + By + C} \right\|}^2}}}{{{A^2} + {B^2}}}\\

阈值 $t^2=3.84 \times \sigma^2$

代码

我们可以制造一些数据，来测试一下RANSAC。具体代码如下

ydsf16/MVG_Algorithm

2. 鲁棒核函数

上述RANSAC方法进行模型估计，实际上分了两步，首先选出局内点，然后再进行一步优化。鲁棒核函数只需要一步，直接优化求解模型参数，通过降低外点的权重，来降低错误数据的影响。一般，我们会优化如下的代价函数

argmin \sum\limits_i^{} {{{\left\| {{d_i}} \right\|}^2}} \\

外点造成的误差/距离 $d_i$ 一般较大，导致最终求解的结果有很大的误差，甚至错误。引入鲁棒核函数对误差在做一步处理，人为降低 $d_i$ 过大的误差项。例如，Huber核函数将代价函数变为

\arg \min \sum\limits_i^{} {\rho \left( {{d_i}} \right)} = \left\{ {\begin{array}{l} {{{\left\| {{d_i}} \right\|}^2},if\left| {{d_i}} \right| < t} \\ {2t\left| {{d_i}} \right| - {t^2},else} \end{array}} \right. \\

当距离 $d_i$ 较小的时候，还是保持原来的代价函数，当 $d_i$ 太大的时候，由原来的二次函数变为线性函数，降低了错误数据的影响。为了便于实现，可以将Huber函数直接转换为误差项的权重。即我们要找一个权重 $w_i$ 使得

{\left( {{w_i}{d_i}} \right)^T}\left( {{w_i}{d_i}} \right) = \rho \left( {{d_i}} \right) \\

那么

{w_i} = \frac{{\sqrt {\rho \left( {{d_i}} \right)} }}{{\left\| {{d_i}} \right\|}} \\

参考资料

[1] Multiple View Geometry

[2] g2o: A general Framework for (Hyper) Graph Optimization

----相关代码----

ydsf16 - Overview

----更多SLAM文章----

杨小东：[ORB-SLAM2] ORB-SLAM中的ORB特征（提取）

杨小东：SLAM轨迹真值获取装置

杨小东：[PnP]PnP问题之EPnP解法

杨小东：[PnP] PnP问题之DLT解法

杨小东：[ORB-SLAM2]卡方分布(Chi-squared)外点(outlier)剔除

杨小东：[ORB-SLAM2] ORB特征提取策略对ORB-SLAM2性能的影响

杨小东：[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM

杨小东：[PR-2] PF 粒子滤波/蒙特卡罗定位