(01)ORB-SLAM2源码无死角解析-(28) 双目Stereo相机立体匹配,SAD算法→深度求解

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

该篇博客主要对双目相机的立体匹配进行一个讲解，也就是通过双目相机提供的信息求解深度。详细讲解之前，我们需要了解一下双目相机的基本属性，下面是双目相机的一个原理视图: 其上 Left 与 Right 分别表示两个相机的光心，他们的之间的距离也叫做基线 baseline (一般以米为单位)，从上图可以看到真实世界上的一个黑点，映射成图像坐标分别为 ( x , y ) (x,y) (x,y), ( x ′ , y ) (x',y) (x′,y)，那么其视差disparity为 x ′ − x x'-x x′−x(一般以像素为单位)。这里记 baseline为 b b b，disparity 为 d d d。

那么根据上图，可以明显的知道 Z b = f d \frac{Z}{b}=\frac{f}{d} bZ​=df​, 其中的 f f f 表示相机焦距，该公式还是十分简单的，主要利用了一个等比性质，这里就不再进行详细的讲解了。该公式化简之后为 Z = f b d Z=\frac{fb}{d} Z=dfb​。也就是说，知道一个特征点在两个相机成像位置之后，就可以求解视差 d d d，另外 f f f, b b b 一般为已知量，即可求解特征点对应的深度。

上图可以看到其最大视差的点为黄线与绿线的交点。因为这个3D点在左相机成像 x x x 坐标是最大的，再右相机成像 x ′ x' x′ 是最小的。此时他们具备最大视差。注意一个点，就是成像平面是关于 ( 0 , 0 ) (0,0) (0,0) 成像原点对称，也就是说单左成像面与右成像平面在3维空间接触的时，最大误差为 x ′ − x = b ( b a s e l i n e ) x'-x=b(baseline) x′−x=b(baseline)。那么我们再来看看最小视察,如下图所示:

可以看到，当两条黄线平行的时候，可以很明显的知道此时的视差 x ′ − x = 0 x'-x=0 x′−x=0，但是需要注意一个点，那就是此次两黄线是平行线，也就是 3维 真实世界没有对应的点能真正的投影成 0 视差。可以想象得到，假设左下的黄线往右旋转无穷小，或者右上的黄线往左旋转无穷小，那么两条黄线将相较于无穷远处，也就是此时存在了交点。

从前面的第一张图像，可以看到，特征点成像 ( x , y ) (x,y) (x,y), ( x ′ , y ) (x',y) (x′,y) 坐标，他们纵坐标是一样的，因为一般来说，双目相机都是固定的。简单的说，就是现实世界中的一点，投影到两个相机成像之后，连接两个成像点产生的直线，永远与基线 baseline 平行。换而言之，同一世界点成像坐标的 y 轴永远是相等的(建立在相机工艺完美的前提下)，如下图所示： 可以看到左图的一个特征点，对应于右图的可能存在的位置，其 y 轴坐标是相同的。也就是说，知道左图相机的一个特征代点，在进行特征匹配的时候，只需要跟右图 y 轴相同的像素进行匹配即可，二不需要匹配所有的像素点，这样就极度提高了特征匹配的效率。

 

二、源码概括

通过上面的讲解，已经大致明白了求解特征点深度的过程，其难点主要还在在于特征匹配。下面就来说说源码是如何进行特征匹配的。对应的源码为 src/Frame.cc 中的函数 void Frame::ComputeStereoMatches()

( 1 ) : \color{blue}{(1):} (1): 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断.

( 2 ) : \color{blue}{(2):} (2): 粗匹配. 根据步骤1的结果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi

( 3 ) : \color{blue}{(3):} (3): 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果

( 4 ) : \color{blue}{(4):} (4): 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差

( 5 ) : \color{blue}{(5):} (5): 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。

( 6 ) : \color{blue}{(6):} (6): 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配   根据上面的总结，下面再对其上上的难点进行细致的分析，主要对源码进行细致的讲解(后面有较全的源码注释，大家可以参考)

( 1 ) : \color{blue}{(1):} (1): 首先统计按行统计右图特征点坐标位置，存储于vRowIndices 变量之中，比如 vRowIndices[i], 其中 i 表示行数(行坐标)，mvKeysRight[i] 为一个数组，数组中的值，即为多个特征点的列数(列坐标)。但是这样还不够的，为了减少误差(由于双目相机制造工艺)，需要为 vRowIndices 的每一行，额外添加一些来自其他行特征点的列坐标。代码中由变量 const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave]; 决定了偏移范围。

( 2 ) : \color{blue}{(2):} (2): 计算出最小视差与最大视差，因为在左图中的特征点，可能在右图是看不见的。也就是说，前面根据双目成像原理，已经把特征匹配缩小至右图的一行所有特征点(加上偏移范围的特征点)。另外，进一步根据视察的原理，继续缩小匹配范围。然后再进行特征匹配，找到最好的匹配点

( 3 ) : \color{blue}{(3):} (3): 通过前面两点，进行了粗匹配，那么接下来还要进行一个精细匹配。分别以右图特征点， 以上面第(2)获得最好的特征点为中心，分别提取半径为w的图像块patch，然后利用滑窗口匹配→左边覆盖区域减去右边覆盖区域，并求出所有像素点差的绝对值的和(SAD值)。找到这个范围内SAD值最小的窗口，即找到了左边图像的最佳匹配的像素块。

( 4 ) : \color{blue}{(4):} (4): 根据(3)，已经计算出了左右图像块patch中相似度最高的位置。但是如果该位置两边的相似度差值较大，则认为是误匹配，否则进行修复。比如 dist2 表示最高相似度，dist1，dist3表示其旁边的现实度，那么通过公式 deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2))，可以计算出需要修复的量，如果修复量太大了，说明 dist1，dist3 与 dist2之间的差异较大，则认为是误匹配，具体的可以参考 opencv sgbm源码中的亚像素插值公式，或者论文 公式7。

( 5 ) : \color{blue}{(5):} (5): 根据左右图匹配到的特征点位置，计算视差，进一步求解深度。

( 6 ) : \color{blue}{(6):} (6): 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配

 

三、源码注解

src/Frame.cc 文件中的 void Frame::ComputeStereoMatches() 函数:

/*
 * 双目匹配函数
 *
 * 为左图的每一个特征点在右图中找到匹配点 \n
 * 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n ‘
 * 这里所说的SAD是一种双目立体视觉匹配算法，可参考[https://blog.csdn.net/u012507022/article/details/51446891]
 * 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对，然后利用抛物线拟合得到亚像素精度的匹配 \n 
 * 这里所谓的亚像素精度，就是使用这个拟合得到一个小于一个单位像素的修正量，这样可以取得更好的估计结果，计算出来的点的深度也就越准确
 * 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)
 */
void Frame::ComputeStereoMatches()
{
    /*两帧图像稀疏立体匹配（即：ORB特征点匹配，非逐像素的密集匹配，但依然满足行对齐）
     * 输入：两帧立体矫正后的图像img_left 和 img_right 对应的orb特征点集
     * 过程：
          1. 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断.
          2. 粗匹配. 根据步骤1的结果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi
          3. 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果
          4. 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差
          5. 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。
          6. 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配
     * 输出：稀疏特征点视差图/深度图（亚像素精度）mvDepth 匹配结果 mvuRight
     */

    // 为匹配结果预先分配内存，数据类型为float型
    // mvuRight存储右图匹配点索引
    // mvDepth存储特征点的深度信息
	mvuRight = vector(N,-1.0f);
    mvDepth = vector(N,-1.0f);

	// orb特征相似度阈值  -> mean ～= (max  + min) / 2
    const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;

    // 金字塔底层（0层）图像高 nRows
    const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;

	// 二维vector存储每一行的orb特征点的列坐标，为什么是vector，因为每一行的特征点有可能不一样，例如
    // vRowIndices[0] = [1，2，5，8, 11]   第1行有5个特征点,他们的列号（即x坐标）分别是1,2,5,8,11
    // vRowIndices[1] = [2，6，7，9, 13, 17, 20]  第2行有7个特征点.etc
    vector vRowIndices(nRows, vector());
    for(int i=0; i