【MVG】（二）RANSAC计算单应矩阵

上一篇：MVG（一）2D射影几何

---

多视图几何、orb-slam2注释版、【程序员眼中的统计学（11）】卡方分布的应用

orb-slam2中在初始化的时候会同时估计H矩阵和F矩阵，使用的都是DLT算法，即根据一组匹配点（如，八点法、四点法），直接使用SVD求得线性解，而不是使用优化的方式，所以求得H矩阵或者F矩阵，跟选取的这组点有很大关系，如果这组点全部都是内点还好，如果存在外点那么得到H矩阵就会偏差很大，而这种情况是很常见的，那么如何从包含外点的数据中计算得到最接近真实值的H矩阵呢，那么就要使用RANSAC算法了.

---

1、卡方分布

1.1 卡方分布于正态分布

若n个相互独立的随机变量 ${\xi }_1、{\xi }_2、\dots \dots 、\xi n$，均服从标准正态分布（也称独立同分布于标准正态分布），则这n个服从标准正态分布的随机变量的平方和属于卡方分布（chi-square distribution），参数n称为自由度. 正如正态分布中均值或方差不同就是另一个正态分布一样，自由度不同就是另一个卡方分布，记为 $x\sim {\chi }^2(k)$.
$$x=\sum _{i=1}^n{\xi }_i^2$$
卡方分布是由正态分布构造而成的一个新的分布，当自由度n很大时，${\chi }^2$ 分布近似为正态分布.
$x\sim {\chi }^2(k)$的概率密度函数如下：

1.2 计算检验统计量

利用卡方公式计算检验统计量，因为卡方分布定义中使用的标准正态分布（均值为0，方差为1），那么如果我们的检验对象如果不是标准正态分布，如，$x\sim N(u,{\sigma }^2)$， 那么我们需要先进行转换，即
$$x^{\prime }=\frac{x-u}{\sigma }$$
$x^{\prime }$ 将是标准正态分布，接下来就可以用来计算检验量了.
我们知道g2o中计算e->chi2()，使用的是 _error.dot(information()*_error)，information()是信息矩阵，即协方差的倒数，写成公式如下：
$$chi2=e^T{\Sigma }^{-1}e$$
咋一看以为上式在计算马式距离（去掉单位量纲），其实它使用的卡方分布定义公式 计算的是检验统计量，先将残差e转换成标准正态分布，然后求平方和，自由度是残差e的维数.

1.3 解决问题

如果检验统计量值很小，说明观察频数和期望频数之间的差别不显著，统计量越大，差别越显著，所以卡方分布主要用于解决一下问题：

• 样本某性质的比例分布与总体理论分布的拟合优度（例如某行政机关男女比是否符合该机关所在城镇的男女比）；
• 同一总体的两个随机变量是否独立（例如人的身高与交通违规的关联性）；

1.4 检测标准

根据上面的 ${\chi }^2$ 概率密度函数，以显著性水平（常用1%和5%）进行检验，卡方分布检验是单尾检验且是右尾，右尾被作为拒绝域。通过查看检验统计量是否位于右尾的拒绝域以内，来判定期望分布得出结果的可能性.
拒绝域根据自由度和显著性水平通过查表法得到，即x2分布临界值表（卡方分布）.

---

2、RANSAC 随机采样一致性

2.1 RANSAC鲁棒估计算法

算法流程如下：

1. 随机地从数据集 $S$ 中选择 $s$ 个数据点，一般取 $s$ 能够刚好组成一个最小子集，以 $s$ 的数据点构成带求模型，比如，拟合曲线的实验中，最小子集当然就是两个点喽，只要取两个点就能构成一条直线.
2. 根据上一步得到模型，我们可以遍历 $S$ 集合中每一个数据点，判断它们是否满足这个模型，也就是说是否在距离阈值 $t$ 内. 如果在，就是内点，将所有这些内点集合称为一致集 $S_i$
3. 如果$S_i$的大小（比如，内点的数目）大于某个阈值 $T$，就用$S_i$中所有点重新估计模型并结束
4. 如果$S_i$小于阈值 $T$，就选择一个新的最小子集 $s$，并重复上述过程
5. 经过N次实验选择一个最大一致集 $S_i$，并用$S_i$ 的所有点重新估计模型.

2.2 距离阈值

上面算法提到了距离阈值$t$ ，其实就是卡方分布中用于检验统计量时所使用的自由度（n）和显著性水平（1%或者5%）对应的临界值. 我们常见的F矩阵对应距离阈值是3.98（1个自由度，点到直线的距离），H矩阵对应的是5.99（2个自由度，重投影误差）等.

2.3 orb-slam2中初始化计算H矩阵和F矩阵

下面是计算H矩阵和F矩阵socre的公式，$d^2$ 即对应上面说的 $chi2=e^T{\Sigma }^{-1}e$.
$$\begin{array}{c}{S}_M={\sum }_i\left({\rho }_M\left(d_{cr}^2\left({\mathbf{x}}_c^i,{\mathbf{x}}_r^i,M\right)\right)+{\rho }_M\left(d_{rc}^2\left({\mathbf{x}}_c^i,{\mathbf{x}}_r^i,M\right)\right)\right)\\ {\rho }_M\left(d^2\right)=\{\begin{array}{ll}\Gamma -d^2& \text{ if }{d}^2<T_M\\ 0& \text{ if }{d}^2\ge T_M\end{array}\end{array}$$

1）$\Gamma =5.99$ 对于H矩阵和F矩阵是相同的，这是为了后面得分时，二者的基准是相同的，这样才有意义啊，得到的score才具备可比性.

2）如果初始化时的场景是平面（如上图a）或者视差很小（如上图b），这时，只能够使用H矩阵进行解释，而不能使用F矩阵解释（对极约束无法满足，推推公式便知），因为即使求得F矩阵，对其分解将出现问题，因为$t=0$，将导致$E=t^{\wedge }R$ 无法分解. 对于图a，我们能够通过对H矩阵分解得到相机运动，但对于图b，由于分解得到$t$为0，所以无法进行三角化，将拒绝此次初始化，reset初始化.

3）使用下面的公式，当 $R_H>0.45$ 时使用H矩阵，否则使用F矩阵
$$R_H=\frac{S_H}{S_H+S_F}$$

2.3.1 RANSAC求解H矩阵

orb-slam2初始化求解H矩阵先要对ReferenceFrame和CurrentFrame匹配上的特征点的坐标分别进行归一化，归一化后x', y'的均值（一阶矩）为0，一阶绝对矩为1. （至于这么做的好处，参见多视图几何：归一化DLT）.
求解H矩阵使用的是八点法，而不是最小子集数4个，我是这么理解的：虽然采用大于最小子集的 $s$，会增加采样次数N，即增加计算量，但是计算得到超定解会更精确带来好处更大远大于增加计算量带来的坏处，因为orb-slam2设置采样次数N是固定的200，与求解F矩阵使用N和 $s$ 是一样，也是考虑到一举两得带来的好处，才这么做的吧. 实现代码如下：

// 重写OpenCV中的findHomography函数                                            
for(int it=0; it<mMaxIterations; it++)                                   
{                                                                        
    // Select a minimum set                                              
    for(size_t j=0; j<8; j++)                                            
    {                                                                    
        int idx = mvSets[it][j];                                         
                                                                         
        // vPn1i和vPn2i为匹配的特征点对的坐标                                        
        vPn1i[j] = vPn1[mvMatches12[idx].first];                         
        vPn2i[j] = vPn2[mvMatches12[idx].second];                        
    }                                                                    
                                                                         
    cv::Mat Hn = ComputeH21(vPn1i,vPn2i);                                
    // 恢复原始的均值和尺度                                                        
    H21i = T2inv*Hn*T1;                                                  
    H12i = H21i.inv();                                                   
                                                                         
    // H矩阵就相当于得到一个模型，然后对该模型进行打分                                          
    // 利用重投影误差为当次RANSAC的结果评分                                             
    currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
                                                                         
    // 得到最优的vbMatchesInliers与score                                       
    if(currentScore>score)                                               
    {                                                                    
        H21 = H21i.clone();                                              
        vbMatchesInliers = vbCurrentInliers;                             
        score = currentScore;                                            
    }                                                                    
}                                                                        

注意：

• 它这里使用的阈值 $T$ 是score，而不简单是内点的数目.
• CheckFundamental() 函数中计算chi2时，使用的const float chiSquare1 = squareDist1*invSigmaSquare;，注意这个invSigmaSquare是1，是因为特征点都是在第一层提取的，参考 SearchForInitialization() 函数

2.3.2 RANSAC求解F矩阵

我们使用DLT的八点法求得F矩阵后，要对F矩阵进行验证，这个验证计算的是点到直线的距离，不能像验证H矩阵那样构造重投影约束，这是因为 $p_2^T{F}_{21}{p}_1=0$ 这个约束是对极约束，我们只知道 $p_1$ 在pKF2上的极线是 $F_{21}{p}_1$，然后计算 $p_2$ 到该极线的距离，也就是 $p_2^T{F}_{21}{p}_1$ 的大小，也即残差，这个距离符合高斯分布，距离的平方当然就符合卡方分布了，进一步，这是1自由度的卡方分布. 代码如下：

float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
{
    const int N = mvMatches12.size();

    const float f11 = F21.at<float>(0,0);
    const float f12 = F21.at<float>(0,1);
    const float f13 = F21.at<float>(0,2);
    const float f21 = F21.at<float>(1,0);
    const float f22 = F21.at<float>(1,1);
    const float f23 = F21.at<float>(1,2);
    const float f31 = F21.at<float>(2,0);
    const float f32 = F21.at<float>(2,1);
    const float f33 = F21.at<float>(2,2);

    vbMatchesInliers.resize(N);

    float score = 0;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差?）
    const float th = 3.841;
    const float thScore = 5.991;

    // sigma:1.0
    const float invSigmaSquare = 1.0/(sigma*sigma);

    for(int i=0; i<N; i++)
    {
        bool bIn = true;

        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];

        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;

        // Reprojection error in second image
        // l2=(a2,b2,c2)=F21*x1
        // F21*x1可以算出x1在图像中x2对应的线l
        //         |a2|
        // |x y 1|*|b2|=a2*x+b2*y+c2=0
        //         |c2|
        const float a2 = f11*u1+f12*v1+f13;
        const float b2 = f21*u1+f22*v1+f23;
        const float c2 = f31*u1+f32*v1+f33;

        // x2应该在l这条线上:x2点乘l = 0，(这是F矩阵的推导出来的性质:x2'*F21*x1=0)
        const float num2 = a2*u2+b2*v2+c2;

        /** @attention 此chi-squared的自由度为1，为点到直线的距离*/
        const float squareDist1 = num2*num2/(a2*a2+b2*b2); // 点到线的几何距离 的平方

        const float chiSquare1 = squareDist1*invSigmaSquare;

        if(chiSquare1>th)
            bIn = false;
        else
            score += thScore - chiSquare1;
......

注意:

• 最后使用的chi2是点到直线的距离squareDist1 = num2*num2/(a2*a2+b2*b2)，判断该检验量与距离阈值的距离，计算得分.
• thScore=5.99而不是3.84，原因见上

---

@leatherwang

二零一八年十月三十一日

---