ORB SLAM2 雙目稀疏立體匹配學習

這部分主要在frame.cc文件中
對應函數為：
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth) :mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_cast<KeyFrame*>(NULL))

ORBSLAM2對雙目幀處理的主要步驟：

1. ID自增
2. 計算圖像金字塔的參數
3. 對左右圖像提取ORB特征點, 使用雙線程進行提取
4. 用opencv的矯正函數，內參對提取到的特征點進行矯正
5. 計算雙目見特征點的匹配，只有匹配成功的特征點才會計算深度，深度存放在mvDepth中;
6. 計算去畸變后邊界

具體步驟

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. 計算去畸變后邊界

雙目特征點匹配

本部分介紹上部分中的第5不

主要對應函數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;
    }
}