讲解关于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→官方认证
一、前言前面两篇博客对 BoW 的构建，以及:BRIEF描述子转BoW向量进行了讲解。那么他有什么作用呢? 或者说，在什么地方可以得到应用。其实，利用到他的地方很多，比如局部建图线程搜索匹配三角化SearchForTriangulation。SearchForTriangulation 函数是在局部建图中被调用的，本来是后面才讲解的内容，但是为了更深一步了解 BoW 的应用，所以提前对其进行一个讲解。
该代码位于 src/ORBmatcher.cc 中，函数名为int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,ector &vMatchedPairs, const bool bOnlyStereo)。该函数的主要作用是: 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对。下面来做一个具体的分析。另外这里为附上一个参考图: 
( 1 ) ： \color{blue}{(1)}： (1)：计算KF1的相机中心在KF2图像平面的二维像素坐标，这个像素坐标也叫极点，对应于上图的 e 1 e_1 e1。创建变量 vbMatched2，vMatches12用来记录是否匹配成功，避免重复匹配。rotHist 用来统计旋转差直方图。
( 2 ) ： \color{blue}{(2)}： (2)：利用BoW加速匹配: 只对属于同一节点(特定层)的ORB特征进行匹配。vFeatVec1，vFeatVec2 每个元素的第一个值，存储的是所属节点 id；第二值存储的是特征点或者BRIEF描述子的索引。vFeatVec1，vFeatVec2 中的元素按照所属 id 从小到大进行排列。
( 3 ) ： \color{blue}{(3)}： (3)：遍历pKF1和pKF2所有所属节点，如果所属节点相同，则对两个节点下的特征点进行匹配。
( 4 ) ： \color{blue}{(4)}： (4)：遍历pKF1和pKF2同一所属节点下的所有特征点，进行两两匹配，如果特征点已经匹配过了，或者存在与之对应的地图点，则跳过该特征点，不进行匹配。
( 5 ) ： \color{blue}{(5)}： (5)：pKF2 上的特征点距离极点太近，则认为该离摄像头太近了，则跳过不进行特征匹配。
( 6 ) ： \color{blue}{(6)}： (6)：kp2(当前帧特征点)应该位于kp1对应的极线上，也就是说，上图中的 P 1 P_1 P1特征点应该距离 极线 l 1 l_1 l1 最近(后面有细节分析)。如果距离过远，认为匹配失败。同时记录最近的特征点，认为是最佳匹配点。
( 7 ) ： \color{blue}{(7)}： (7)：记录旋转差直方图信息，该内容在前面的博客中讲解过，主要是记录主流特征点，删除异常匹配点特征点。
另外这里额外提及一些细节的东西:SearchForTriangulation 函数中存在代码如下:
if(f1it->first == f2it->first)
......
else if(f1it->first first)
f1it = vFeatVec1.lower_bound(f2it->first);
else
f2it = vFeatVec2.lower_bound(f1it->first);
因为 vFeatVec1，与 vFeatVec2 都是按照所属节点 id 从小到大排序的。如果 pKF1 中所属节点与 pKF2 中所属节点不匹配，则有两种情况： ①.f1it->first(pKF1所属节点id) 小于 f2it->first(pKF2所属节点id) →那么就把 f1it 的节点 id 增大，赋值成与 f2it 相同的节点，那么下一次循环，他们所属节点 id 就相同了。 ②.f1it->first(pKF1所属节点id) 大于 f2it->first(pKF2所属节点id) →那么就把 f2it 的节点 增大，赋值成与 f1it 相同的节点，那么下一次循环，他们所属节点 id 就相同了。
判断关键点与极线距离函数为CheckDistEpipolarLine(kp1,kp2,F12,pKF2)，通过前面博客，根据对极几何原理，可得基本矩阵相关公式: F a b = ( K b − T E a b K a − 1 ) v a T F a b v b = 0 (01) \tag{01} \color{green} \mathbf F_{ab}=(\mathbf K_b^{-T} \mathbf E_{ab}\mathbf K_a^{-1}) ~~~~~~~~~~~~~~~~~ \color{green} v_a^T\mathbf F_{ab}v_b=0 Fab=(Kb−TEabKa−1) vaTFabvb=0(01) 换成源码中的公式为(相机相同，故 K 1 = K 2 \mathbf K_1=\mathbf K_2 K1=K2): F 12 = ( K − T E 12 K ) v 1 T F 12 v 2 = 0 (02) \tag{02} \color{green} \mathbf F_{12}=(\mathbf K^{-T} \mathbf E_{12}\mathbf K) ~~~~~~~~~~~~~~~~~~~~ \color{green} v_1^T\mathbf F_{12}v_2=0 F12=(K−TE12K) v1TF12v2=0(02)如果把 v 1 T F 12 v_1^T\mathbf F_{12} v1TF12 看作极线，则其表示为在帧 pKF2 的极线，如果把 F 12 v 2 \mathbf F_{12}v_2 F12v2 看作极线，则表示在帧pKF1中的极线，则表示在帧 pKF1 的极线。源码中是把 v 1 T F 12 v_1^T\mathbf F_{12} v1TF12 看作极线，然后再计算 pKF2 中的特征点 kp2 距离极线的距离是否在允许偏差范围内，如果直线(极线)方程为 a x + b y + c = 0 ax+by+c=0 ax+by+c=0, 那么点到直线的距离为: d = ∣ a x + b x + c ∣ a 2 + b 2 (02) \tag{02} \color{green} d = \frac{|ax+bx+c|}{\sqrt{a^2+b^2}} d=a2+b2 ∣ax+bx+c∣(02)
四、源码注释
该该代码位于 src/ORBmatcher.cc 中，函数名为 ORBmatcher::SearchForTriangulation()。
/**
* @brief 用基础矩阵检查极线距离是否符合要求
*
* @param[in] kp1 KF1中特征点
* @param[in] kp2 KF2中特征点
* @param[in] F12 从KF2到KF1的基础矩阵
* @param[in] pKF2 关键帧KF2
* @return true
* @return false
*/
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
// Epipolar line in second image l2 = x1'F12 = [a b c]
// Step 1 求出kp1在pKF2上对应的极线
const float a = kp1.pt.x*F12.at(0,0)+kp1.pt.y*F12.at(1,0)+F12.at(2,0);
const float b = kp1.pt.x*F12.at(0,1)+kp1.pt.y*F12.at(1,1)+F12.at(2,1);
const float c = kp1.pt.x*F12.at(0,2)+kp1.pt.y*F12.at(1,2)+F12.at(2,2);
// Step 2 计算kp2特征点到极线l2的距离
// 极线l2：ax + by + c = 0
// (u,v)到l2的距离为： |au+bv+c| / sqrt(a^2+b^2)
const float num = a*kp2.pt.x+b*kp2.pt.y+c;
const float den = a*a+b*b;
// 距离无穷大
if(den==0)
return false;
// 距离的平方
const float dsqr = num*num/den;
// Step 3 判断误差是否满足条件。尺度越大，误差范围应该越大。
// 金字塔最底层一个像素就占一个像素，在倒数第二层，一个像素等于最底层1.2个像素（假设金字塔尺度为1.2）
// 3.84 是自由度为1时，服从高斯分布的一个平方项（也就是这里的误差）小于一个像素，这件事发生概率超过95%时的概率 （卡方分布）
return dsqr mvLevelSigma2[kp2.octave];
}
/*
* @brief 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对
* 具体来说，pKF1图像的每个特征点与pKF2图像同一node节点的所有特征点依次匹配，判断是否满足对极几何约束，满足约束就是匹配的特征点
* @param pKF1 关键帧1
* @param pKF2 关键帧2
* @param F12 从2到1的基础矩阵
* @param vMatchedPairs 存储匹配特征点对，特征点用其在关键帧中的索引表示
* @param bOnlyStereo 在双目和rgbd情况下，是否要求特征点在右图存在匹配
* @return 成功匹配的数量
*/
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
vector &vMatchedPairs, const bool bOnlyStereo)
{
const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
// Compute epipole in second image
// Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标
// KF1相机光心在世界坐标系坐标Cw
cv::Mat Cw = pKF1->GetCameraCenter();
// KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
// KF1的相机光心转化到KF2坐标系中的坐标
cv::Mat C2 = R2w*Cw+t2w;
const float invz = 1.0f/C2.at(2);
// 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标
const float ex =pKF2->fx*C2.at(0)*invz+pKF2->cx;
const float ey =pKF2->fy*C2.at(1)*invz+pKF2->cy;
// Find matches between not tracked keypoints
// Matching speed-up by ORB Vocabulary
// Compare only ORB that share the same node
int nmatches=0;
// 记录匹配是否成功，避免重复匹配
vector vbMatched2(pKF2->N,false);
vector vMatches12(pKF1->N,-1);
// 用于统计匹配点对旋转差的直方图
vector rotHist[HISTO_LENGTH];
for(int i=0;ifirst对应node编号，f1it->second对应属于该node的所有特特征点编号
DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
// Step 2.1：遍历pKF1和pKF2中的node节点
while(f1it!=f1end && f2it!=f2end)
{
// 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理
if(f1it->first == f2it->first)
{
// Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点
for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];
// Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPoint
MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
// If there is already a MapPoint skip
// 由于寻找的是未匹配的特征点，所以pMP1应该为NULL
if(pMP1)
continue;
// 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值
const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
if(bOnlyStereo)
if(!bStereo1)
continue;
// Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点
const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
// 通过特征点索引idx1在pKF1中取出对应的特征点的描述子
const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
int bestDist = TH_LOW;
int bestIdx2 = -1;
// Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点
for(size_t i2=0, iend2=f2it->second.size(); i2second[i2];
// 通过特征点索引idx2在pKF2中取出对应的MapPoint
MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
// If we have already matched or there is a MapPoint skip
// 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2
if(vbMatched2[idx2] || pMP2)
continue;
const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
if(bOnlyStereo)
if(!bStereo2)
continue;
// 通过特征点索引idx2在pKF2中取出对应的特征点的描述子
const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
// Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离
const int dist = DescriptorDistance(d1,d2);
if(dist>TH_LOW || dist>bestDist)
continue;
// 通过特征点索引idx2在pKF2中取出对应的特征点
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
//? 为什么双目就不需要判断像素点到极点的距离的判断？
// 因为双目模式下可以左右互匹配恢复三维点
if(!bStereo1 && !bStereo2)
{
const float distex = ex-kp2.pt.x;
const float distey = ey-kp2.pt.y;
// Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
// 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th
// 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素
//? 对这个阈值的有效性持怀疑态度
if(distex*distex+distey*disteymvScaleFactors[kp2.octave])
continue;
}
// Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值
if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
{
// bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离
bestIdx2 = idx2;
bestDist = dist;
}
}
if(bestIdx2>=0)
{
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
// 记录匹配结果
vMatches12[idx1]=bestIdx2;
vbMatched2[bestIdx2]=true; // !记录已经匹配，避免重复匹配。原作者漏掉！
nmatches++;
// 记录旋转差直方图信息
if(mbCheckOrientation)
{
// angle：角度，表示匹配点对的方向差。
float rot = kp1.angle-kp2.angle;
if(rot=0 && binfirst first)
{
f1it = vFeatVec1.lower_bound(f2it->first);
}
else
{
f2it = vFeatVec2.lower_bound(f1it->first);
}
}
// Step 3 用旋转差直方图来筛掉错误匹配对
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i
关注
打赏
![]()
1592542134
查看更多评论
最近更新
- 深拷贝和浅拷贝的区别（重点）
- 【Vue】走进Vue框架世界
- 【云服务器】项目部署—搭建网站—vue电商后台管理系统
- 【React介绍】 一文带你深入React
- 【React】React组件实例的三大属性之state，props，refs（你学废了吗）
- 【脚手架VueCLI】从零开始，创建一个VUE项目
- 【React】深入理解React组件生命周期----图文详解（含代码）
- 【React】DOM的Diffing算法是什么？以及DOM中key的作用----经典面试题
- 【React】1_使用React脚手架创建项目步骤--------详解(含项目结构说明)
- 【React】2_如何使用react脚手架写一个简单的页面？
