ORB_SLAM2 双目稀疏立体匹配算法

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

主要在frame.cc文件中

主要过程:

  1. ID自增
1
mnId=nNextId++;
  1. 计算图像金字塔的参数
1
2
3
4
5
6
7
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
  1. 对左右图像提取ORB特征点, 使用双线程进行提取
1
2
3
4
thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
thread threadRight(&Frame::ExtractORB,this,1,imRight);
threadLeft.join();
threadRight.join();
  1. 用opencv的矫正函数,内参对提取到的特征点进行矫正
1
UndisortKeyPoints();
  1. 计算双目见特征点的匹配,只有匹配成功的特征点才会计算深度,深度存放在mvDepth中;
1
ComPuteStereoMatches();
  1. 计算去畸变后边界

双目特征点匹配

image-20200515110530658

主要对应函数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$

亚像素插值

1
2
3
4
5
6
7
8
9
// 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;

亚像素插值方法:

image-20200515113540723

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

  1. 最优视差值。深度选择
  2. 删除离群点(Outliers)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// 快匹配相似度阈值判断,快意话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;
}
}
  • © 2019-2022 guoben
  • PV: UV:

微信