ORB-SLAM2 双目构造函数 + 匹配函数

双目相机的构造函数

ORBSLAM2对双目帧处理的主要步骤：

• ID自增
• 计算图像金字塔的参数
• 对左右图像提取ORB特征点, 使用双线程进行提取
• 用opencv的矫正函数，内参对提取到的特征点进行矫正
• 计算双目见特征点的匹配，只有匹配成功的特征点才会计算深度，- 深度存放在mvDepth中;
• 计算去畸变后边界

具体步骤

1. ID自增

   mnId=nNextId++;

2. 计算图像金字塔的参数

mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();    
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

3. 对左右图像提取ORB特征点, 使用双线程进行提取

   thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
    thread threadRight(&Frame::ExtractORB,this,1,imRight);
    threadLeft.join();
    threadRight.join();

4. 用opencv的矫正函数，内参对提取到的特征点进行矫正

 UndisortKeyPoints();

5. 计算双目见特征点的匹配，只有匹配成功的特征点才会计算深度，深度存放在mvDepth中;

 ComPuteStereoMatches();

6. 计算去畸变后边界

---

双目匹配函数 void Frame::ComputeStereoMatches()

接下来主要介绍双目匹配函数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
详解

• 经过去畸变操作，原则上左图上某个特征点在右图匹配的点应该是在同一行上，但考虑到尺度金字塔特征，一个特征点可能存在于多行，而非唯一的一行，所以根据特征点所在层的缩放因子，计算得到一个行的范围。
• 但是在寻找匹配点的时候，不需要遍历行范围内的所有特征点，而是在一定的列范围内。
• 粗匹配是指用左图的某个特征点与右图中可能匹配的所有特征点逐个比较，得到最相似匹配点的相似度和索引。其中有一个所在金字塔层数的判断，如果左右两点所在的层数相差比较大，则认为不是匹配的。
• 精匹配过程是计算SAD，块匹配，在右图中设定一个以要判断的特征点为中心的patch，然后在一定范围内横向移动，用左图patch内的每个像素减去每次移动后右图patch的像素值，选择绝对值和最小的，保留下此时的最下SAD和对应的偏移量
• 亚像素匹配，主要是为了提高匹配精度
  
  这部分就是根据这个公式计算的，得到亚像素精度偏移量delta调整最佳匹配索引，根据更新后的视差计算对应的深度值，最后保存这个点对应的深度值、匹配的右特征点u坐标以及归一化SAD最小相似度
• 上一步结束之后，得到了所有左图特征点根据SAD值计算得到的匹配的右图特征点，虽然这些特征点匹配的SAD值最小，但也不一定就是正确的，比如光照变化，若纹理，无纹理都会造成误匹配。所以最后一步是对这些匹配做个筛选。
  筛选条件是：认为SAD值小于thDist才是正确的匹配。

const float median = vDistIdx[vDistIdx.size()/2].first;
const float thDist = 1.5f*1.4f*median;