讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人
联系方式,
点击本人照片即可显示
W
X
→
官方认证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
一、前言
该篇博客主要对双目相机的立体匹配进行一个讲解,也就是通过双目相机提供的信息求解深度。详细讲解之前,我们需要了解一下双目相机的基本属性,下面是双目相机的一个原理视图:
其上 Left 与 Right 分别表示两个相机的光心,他们的之间的距离也叫做基线 baseline (一般以米为单位),从上图可以看到真实世界上的一个黑点,映射成图像坐标分别为
(
x
,
y
)
(x,y)
(x,y),
(
x
′
,
y
)
(x',y)
(x′,y),那么其视差disparity为
x
′
−
x
x'-x
x′−x(一般以像素为单位)。这里记 baseline为
b
b
b,disparity 为
d
d
d。
那么根据上图,可以明显的知道 Z b = f d \frac{Z}{b}=\frac{f}{d} bZ=df, 其中的 f f f 表示相机焦距,该公式还是十分简单的,主要利用了一个等比性质,这里就不再进行详细的讲解了。该公式化简之后为 Z = f b d Z=\frac{fb}{d} Z=dfb。也就是说,知道一个特征点在两个相机成像位置之后,就可以求解视差 d d d,另外 f f f, b b b 一般为已知量,即可求解特征点对应的深度。
上图可以看到其最大视差的点为黄线与绿线的交点。因为这个3D点在左相机成像
x
x
x 坐标是最大的,再右相机成像
x
′
x'
x′ 是最小的。此时他们具备最大视差。注意一个点,就是成像平面是关于
(
0
,
0
)
(0,0)
(0,0) 成像原点对称,也就是说单左成像面与右成像平面在3维空间接触的时,最大误差为
x
′
−
x
=
b
(
b
a
s
e
l
i
n
e
)
x'-x=b(baseline)
x′−x=b(baseline)。那么我们再来看看最小视察,如下图所示:
可以看到,当两条黄线平行的时候,可以很明显的知道此时的视差 x ′ − x = 0 x'-x=0 x′−x=0,但是需要注意一个点,那就是此次两黄线是平行线,也就是 3维 真实世界没有对应的点能真正的投影成 0 视差。可以想象得到,假设左下的黄线往右旋转无穷小,或者右上的黄线往左旋转无穷小,那么两条黄线将相较于无穷远处,也就是此时存在了交点。
从前面的第一张图像,可以看到,特征点成像
(
x
,
y
)
(x,y)
(x,y),
(
x
′
,
y
)
(x',y)
(x′,y) 坐标,他们纵坐标是一样的,因为一般来说,双目相机都是固定的。简单的说,就是现实世界中的一点,投影到两个相机成像之后,连接两个成像点产生的直线,永远与基线 baseline 平行。换而言之,同一世界点成像坐标的 y 轴永远是相等的(建立在相机工艺完美的前提下),如下图所示:
可以看到左图的一个特征点,对应于右图的可能存在的位置,其 y 轴坐标是相同的。也就是说,知道左图相机的一个特征代点,在进行特征匹配的时候,只需要跟右图 y 轴相同的像素进行匹配即可,二不需要匹配所有的像素点,这样就极度提高了特征匹配的效率。
二、源码概括
通过上面的讲解,已经大致明白了求解特征点深度的过程,其难点主要还在在于特征匹配。下面就来说说源码是如何进行特征匹配的。对应的源码为 src/Frame.cc 中的函数 void Frame::ComputeStereoMatches()
( 1 ) : \color{blue}{(1):} (1): 行特征点统计. 统计img_right每一行上的ORB特征点集,便于使用立体匹配思路(行搜索/极线搜索)进行同名点搜索, 避免逐像素的判断.
( 2 ) : \color{blue}{(2):} (2): 粗匹配. 根据步骤1的结果,对img_left第i行的orb特征点pi,在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi
( 3 ) : \color{blue}{(3):} (3): 精确匹配. 以点qi为中心,半径为r的范围内,进行块匹配(归一化SAD),进一步优化匹配结果
( 4 ) : \color{blue}{(4):} (4): 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度,并不一定是真实视差,通过亚像素差值(抛物线插值)获取float精度的真实视差
( 5 ) : \color{blue}{(5):} (5): 最优视差值/深度选择. 通过胜者为王算法(WTA)获取最佳匹配点。
(
6
)
:
\color{blue}{(6):}
(6): 删除离群点(outliers). 块匹配相似度阈值判断,归一化sad最小,并不代表就一定是正确匹配,比如光照变化、弱纹理等会造成误匹配
根据上面的总结,下面再对其上上的难点进行细致的分析,主要对源码进行细致的讲解(后面有较全的源码注释,大家可以参考)
( 1 ) : \color{blue}{(1):} (1): 首先统计按行统计右图特征点坐标位置,存储于vRowIndices 变量之中,比如 vRowIndices[i], 其中 i 表示行数(行坐标),mvKeysRight[i] 为一个数组,数组中的值,即为多个特征点的列数(列坐标)。但是这样还不够的,为了减少误差(由于双目相机制造工艺),需要为 vRowIndices 的每一行,额外添加一些来自其他行特征点的列坐标。代码中由变量 const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave]; 决定了偏移范围。
( 2 ) : \color{blue}{(2):} (2): 计算出最小视差与最大视差,因为在左图中的特征点,可能在右图是看不见的。也就是说,前面根据双目成像原理,已经把特征匹配缩小至右图的一行所有特征点(加上偏移范围的特征点)。另外,进一步根据视察的原理,继续缩小匹配范围。然后再进行特征匹配,找到最好的匹配点
( 3 ) : \color{blue}{(3):} (3): 通过前面两点,进行了粗匹配,那么接下来还要进行一个精细匹配。分别以右图特征点, 以上面第(2)获得最好的特征点为中心,分别提取半径为w的图像块patch,然后利用滑窗口匹配→左边覆盖区域减去右边覆盖区域,并求出所有像素点差的绝对值的和(SAD值)。找到这个范围内SAD值最小的窗口,即找到了左边图像的最佳匹配的像素块。
( 4 ) : \color{blue}{(4):} (4): 根据(3),已经计算出了左右图像块patch中相似度最高的位置。但是如果该位置两边的相似度差值较大,则认为是误匹配,否则进行修复。比如 dist2 表示最高相似度,dist1,dist3表示其旁边的现实度,那么通过公式 deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2)),可以计算出需要修复的量,如果修复量太大了,说明 dist1,dist3 与 dist2之间的差异较大,则认为是误匹配,具体的可以参考 opencv sgbm源码中的亚像素插值公式,或者论文 <> 公式7。
( 5 ) : \color{blue}{(5):} (5): 根据左右图匹配到的特征点位置,计算视差,进一步求解深度。
( 6 ) : \color{blue}{(6):} (6): 删除离群点(outliers). 块匹配相似度阈值判断,归一化sad最小,并不代表就一定是正确匹配,比如光照变化、弱纹理等会造成误匹配
三、源码注解
src/Frame.cc 文件中的 void Frame::ComputeStereoMatches() 函数:
/*
* 双目匹配函数
*
* 为左图的每一个特征点在右图中找到匹配点 \n
* 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n ‘
* 这里所说的SAD是一种双目立体视觉匹配算法,可参考[https://blog.csdn.net/u012507022/article/details/51446891]
* 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对,然后利用抛物线拟合得到亚像素精度的匹配 \n
* 这里所谓的亚像素精度,就是使用这个拟合得到一个小于一个单位像素的修正量,这样可以取得更好的估计结果,计算出来的点的深度也就越准确
* 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)
*/
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
*/
// 为匹配结果预先分配内存,数据类型为float型
// mvuRight存储右图匹配点索引
// mvDepth存储特征点的深度信息
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);
// orb特征相似度阈值 -> mean ~= (max + min) / 2
const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
// 金字塔底层(0层)图像高 nRows
const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;
// 二维vector存储每一行的orb特征点的列坐标,为什么是vector,因为每一行的特征点有可能不一样,例如
// vRowIndices[0] = [1,2,5,8, 11] 第1行有5个特征点,他们的列号(即x坐标)分别是1,2,5,8,11
// vRowIndices[1] = [2,6,7,9, 13, 17, 20] 第2行有7个特征点.etc
vector<vector<size_t> > vRowIndices(nRows, vector<size_t>());
for(int i=0; i<nRows; i++) vRowIndices[i].reserve(200);
// 右图特征点数量,N表示数量 r表示右图,且不能被修改
const int Nr = mvKeysRight.size();
// Step 1. 行特征点统计。 考虑用图像金字塔尺度作为偏移,左图中对应右图的一个特征点可能存在于多行,而非唯一的一行
for(int iR = 0; iR < Nr; iR++) {
// 获取特征点ir的y坐标,即行号
const cv::KeyPoint &kp = mvKeysRight[iR];
const float &kpY = kp.pt.y;
// 计算特征点ir在行方向上,可能的偏移范围r,即可能的行号为[kpY + r, kpY -r]
// 2 表示在全尺寸(scale = 1)的情况下,假设有2个像素的偏移,随着尺度变化,r也跟着变化
const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave];
const int maxr = ceil(kpY + r);
const int minr = floor(kpY - r);
// 将特征点ir保证在可能的行号中
for(int yi=minr;yi<=maxr;yi++)
vRowIndices[yi].push_back(iR);
}
// 下面是 粗匹配 + 精匹配的过程
// 对于立体矫正后的两张图,在列方向(x)存在最大视差maxd和最小视差mind
// 也即是左图中任何一点p,在右图上的匹配点的范围为应该是[p - maxd, p - mind], 而不需要遍历每一行所有的像素
// maxd = baseline * length_focal / minZ
// mind = baseline * length_focal / maxZ
const float minZ = mb;
const float minD = 0; // 最小视差为0,对应无穷远
const float maxD = mbf/minZ; // 最大视差对应的距离是相机的基线
// 保存sad块匹配相似度和左图特征点索引
vector<pair<int, int> > vDistIdx;
vDistIdx.reserve(N);
// 为左图每一个特征点il,在右图搜索最相似的特征点ir
for(int iL=0; iL<N; iL++) {
const cv::KeyPoint &kpL = mvKeys[iL];
const int &levelL = kpL.octave;
const float &vL = kpL.pt.y;
const float &uL = kpL.pt.x;
// 获取左图特征点il所在行,以及在右图对应行中可能的匹配点
const vector<size_t> &vCandidates = vRowIndices[vL];
if(vCandidates.empty()) continue;
// 计算理论上的最佳搜索范围
const float minU = uL-maxD;
const float maxU = uL-minD;
// 最大搜索范围小于0,说明无匹配点
if(maxU<0) continue;
// 初始化最佳相似度,用最大相似度,以及最佳匹配点索引
int bestDist = ORBmatcher::TH_HIGH;
size_t bestIdxR = 0;
const cv::Mat &dL = mDescriptors.row(iL);
// Step 2. 粗配准。左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的描述子距离和索引
for(size_t iC=0; iC<vCandidates.size(); iC++) {
const size_t iR = vCandidates[iC];
const cv::KeyPoint &kpR = mvKeysRight[iR];
// 左图特征点il与待匹配点ic的空间尺度差超过2,放弃
if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
continue;
// 使用列坐标(x)进行匹配,和stereomatch一样
const float &uR = kpR.pt.x;
// 超出理论搜索范围[minU, maxU],可能是误匹配,放弃
if(uR >= minU && uR <= maxU) {
// 计算匹配点il和待匹配点ic的相似度dist
const cv::Mat &dR = mDescriptorsRight.row(iR);
const int dist = ORBmatcher::DescriptorDistance(dL,dR);
//统计最小相似度及其对应的列坐标(x)
if( dist<bestDist ) {
bestDist = dist;
bestIdxR = iR;
}
}
}
// Step 3. 图像块滑动窗口用SAD(Sum of absolute differences,差的绝对和)实现精确匹配.
if(bestDist<thOrbDist) {
// 如果刚才匹配过程中的最佳描述子距离小于给定的阈值
// 计算右图特征点x坐标和对应的金字塔尺度
const float uR0 = mvKeysRight[bestIdxR].pt.x;
const float scaleFactor = mvInvScaleFactors[kpL.octave];
// 尺度缩放后的左右图特征点坐标
const float scaleduL = round(kpL.pt.x*scaleFactor);
const float scaledvL = round(kpL.pt.y*scaleFactor);
const float scaleduR0 = round(uR0*scaleFactor);
// 滑动窗口搜索, 类似模版卷积或滤波
// w表示sad相似度的窗口半径
const int w = 5;
// 提取左图中,以特征点(scaleduL,scaledvL)为中心, 半径为w的图像块patch
cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
IL.convertTo(IL,CV_32F);
// 图像块均值归一化,降低亮度变化对相似度计算的影响
IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);
//初始化最佳相似度
int bestDist = INT_MAX;
// 通过滑动窗口搜索优化,得到的列坐标偏移量
int bestincR = 0;
//滑动窗口的滑动范围为(-L, L)
const int L = 5;
// 初始化存储图像块相似度
vector<float> vDists;
vDists.resize(2*L+1);
// 计算滑动窗口滑动范围的边界,因为是块匹配,还要算上图像块的尺寸
// 列方向起点 iniu = r0 - 最大窗口滑动范围 - 图像块尺寸
// 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1
// 此次 + 1 和下面的提取图像块是列坐标+1是一样的,保证提取的图像块的宽是2 * w + 1
// ! 源码: const float iniu = scaleduR0+L-w; 错误
// scaleduR0:右图特征点x坐标
const float iniu = scaleduR0-L-w;
const float endu = scaleduR0+L+w+1;
// 判断搜索是否越界
if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)
continue;
// 在搜索范围内从左到右滑动,并计算图像块相似度
for(int incR=-L; incR<=+L; incR++) {
// 提取右图中,以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch
cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);
IR.convertTo(IR,CV_32F);
// 图像块均值归一化,降低亮度变化对相似度计算的影响
IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);
// sad 计算,值越小越相似
float dist = cv::norm(IL,IR,cv::NORM_L1);
// 统计最小sad和偏移量
if(dist<bestDist) {
bestDist = dist;
bestincR = incR;
}
//L+incR 为refine后的匹配点列坐标(x)
vDists[L+incR] = dist;
}
// 搜索窗口越界判断
if(bestincR==-L || bestincR==L)
continue;
// Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线来得到最小sad的亚像素坐标
// 使用3点拟合抛物线的方式,用极小值代替之前计算的最优是差值
// \ / <- 由视差为14,15,16的相似度拟合的抛物线
// . .(16)
// .14 .(15) <- int/uchar最佳视差值
// .
// (14.5)<- 真实的视差值
// deltaR = 15.5 - 16 = -0.5
// 公式参考opencv sgbm源码中的亚像素插值公式
// 或论文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7
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));
// 亚像素精度的修正量应该是在[-1,1]之间,否则就是误匹配
if(deltaR<-1 || deltaR>1)
continue;
// 根据亚像素精度偏移量delta调整最佳匹配索引
float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
float disparity = (uL-bestuR);
if(disparity>=minD && disparity<maxD) {
// 如果存在负视差,则约束为0.01
if( disparity <=0 ) {
disparity=0.01;
bestuR = uL-0.01;
}
// 根据视差值计算深度信息
// 保存最相似点的列坐标(x)信息
// 保存归一化sad最小相似度
// Step 5. 最优视差值/深度选择.
mvDepth[iL]=mbf/disparity;
mvuRight[iL] = bestuR;
vDistIdx.push_back(pair<int,int>(bestDist,iL));
}
}
}
// Step 6. 删除离群点(outliers)
// 块匹配相似度阈值判断,归一化sad最小,并不代表就一定是匹配的,比如光照变化、弱纹理、无纹理等同样会造成误匹配
// 误匹配判断条件 norm_sad > 1.5 * 1.4 * median
sort(vDistIdx.begin(),vDistIdx.end());
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 {
// 误匹配点置为-1,和初始化时保持一直,作为error code
mvuRight[vDistIdx[i].second]=-1;
mvDepth[vDistIdx[i].second]=-1;
}
}
}
四、结语
通过该篇博客的介绍,我们已经知道如何通过双目相机去求解特征代点的深度。同时了解了 SAD 以及 亚像素插值算法。
本文内容来自计算机视觉life ORB-SLAM2 课程课件