ORB_SLAM2 双目稀疏立体匹配算法

本节主要学习ORB_SLAM2中的双目立体匹配算法的实现过程

主要在frame.cc文件中

主要过程：

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. 计算去畸变后边界

双目特征点匹配

主要对应函数Frame::ComputeStereoMatches()。
输入：两帧立体矫正后的图像对应的ob特征点集
过程

1. 行特征点统计
2. 粗匹配
3. 精确匹配SAD.
4. 亚像素精度优化
5. 最有视差值/深度选择
6. 删除离群点（ outliers)

输出：稀疏特征点视差图/深度图和匹配结果

视差公式

z:深度 d:视差（disparity）f:焦距 b：(baseline) 基线

$z=frac{fb}{d},d=u_L-u_R$

亚像素插值

// Sub-pixel match (Parabola fitting)
const float dist1 = vDists[L+bestincR-1];
const float dist2 = vDists[L+bestincR];
const float dist3 = vDists[L+bestincR+1];

const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));

if(deltaR<-1 || deltaR>1)
    continue;

亚像素插值方法：

亚像素的误差在一个像素以内，所以修正量大一1时鉴定为误匹配。

5. 最优视差值。深度选择
6. 删除离群点（Outliers）

// 快匹配相似度阈值判断，快意话sad最小，不代表就是匹配的，比如光照变化，若纹理，无纹理都会造成误匹配
//误匹配判断条件 norm_sad > 1.5*1.4*median
sort(vDistIdx.begin(),vDistIdx.end()); //对dist进行排序
const float median = vDistIdx[vDistIdx.size()/2].first; //根据中值计算阈值
const float thDist = 1.5f*1.4f*median;

for(int i=vDistIdx.size()-1;i>=0;i--)
{
    if(vDistIdx[i].first<thDist)
        break;
    else
    {
        mvuRight[vDistIdx[i].second]=-1;
        mvDepth[vDistIdx[i].second]=-1;
    }
}