ORB SLAM2 双目稀疏立体匹配学习

Posted 2021-02-19 guoben

本节学习自6哥的ORBSLAM2解读
这部分主要在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;
    }
}