当前位置：首页 > 编程日记 > 正文

ORB_SLAM2源码：ORBmatcher.cc

编程日记 2024-06-10 16:00:00

ORBmatcher.cc中的函数，主要实现（1）路标点和特征点的匹配（2D-3D点对）。（2）特征点和特征点的匹配（2D-2D点对）。SearchByProjection的函数重载看得我一脸懵逼。在这做一下笔记，以后可以参考参考。
每个路标点会有一个自己的最优描述子。个人理解，这个最优描述子的含义应该是：一个路标点会对应多个特征点，先获得所有特征点的描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。

匹配策略

在匹配2D-2D点对，2D-3D点对时，常用的策略包括：
（1）比较描述子的汉明距离，选出最优的特征点（2D-2D、3D-3D）。

在这里插入图片描述

（2）使用词袋加速匹配（2D-2D）。
词袋中包含单词。每个单词即使用聚类算法获得的同一类的特征点的描述子。每幅图像的每一个特征点都对应一个单词。在两幅图像的单词分布已知的情况下，如：
图像1：单词1（包含50个特征点）、单词2（包含100个特征点）；
图像2：单词1（包含60个特征点）、单词2（包含90个特征点）。
可以在每个单词中去比较特征点，这样只需比较50×60+100×90=12000次，远小于150×150=22500。从而实现特征点的加速匹配。但这种方法会造成很多未匹配的特征点（比如选择的两个特征点间的汉明距离大于设置的阈值）。

约束条件

（1）相机坐标系下，z值应该大于0
（2）找到的特征点对间的汉明距离要小于阈值。
（3）找到的最优、次优特征点间的汉明距离应满足一定的比例关系。
（4）相机坐标转到像素坐标后，应该在图像的范围之内。
（5）路标点—光心间的长度应在一定范围内。
（6）在旋转直方图中，只选出前3个直方图中的特征点对。
旋转直方图：
直方图中存放的是找出的候选特征点的编号。
在这里插入图片描述

参考自知乎：https://zhuanlan.zhihu.com/p/267971447?utm_source=wechat_timeline

1）构建直方图
30个直方图，每个直方图的高（高，即存储500个KeyPoint的索引）

2）计算KeyFrame1的KeyPoints和其对应KeyFrame2的KeyPoints的角度差。
角度差为1~30之间的数，对应着30个直方图，并将KeyFrame1中的KeyPoint索引值放入直方图中。
示例：角度差为100，那么100/30=3.3，四舍五入为3，应该将KeyFrame1中的KeyPoint索引值存入第3个直方图中

3）筛选落在直方图中索引值最多的3个直方图：ComputeThreeMaxima
最后取选择上图中最高的3个直方图：（1）（2）（3）

SearchByProjection函数的重载

SearchByProjection函数一共有4种重载。分别为局部地图跟踪，后一帧跟踪前一帧，重定位中的跟踪，回环检测中的跟踪。均是为了建立路标点与特征点间的联系。

局部地图跟踪

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)

流程为：
（1）遍历有效的地图点。
（2）设定搜索搜索窗口的大小。取决于视角, 若当前视角和路标点的平均视角夹角较小时, r取一个较小的值。
（3）通过投影点以及搜索窗口和预测的尺度进行搜索, 找出搜索半径内的候选匹配点索引。
（4）寻找候选匹配点中的最佳和次佳匹配点。
（5）筛选最佳匹配点。
建立局部地图的路标点与当前帧的特征点间的联系。流程如下：
在这里插入图片描述

当前帧追踪上一帧

将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)

流程为：
（1）建立旋转直方图，用于检测旋转一致性。
（2）计算当前帧和前一帧的平移向量。
（3）对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标。
（4）根据相机的前后前进方向来判断搜索金字塔层的范围。
（5）遍历候选匹配点，寻找距离最小的最佳匹配点。
（6）计算匹配点旋转角度差所在的直方图。
（7）进行旋转一致检测，剔除不一致的匹配。

在这里插入图片描述

重定位中的投影

在重定位过程中，会首先使用词袋算法加速特征点匹配，找到与当前帧F最相似的关键帧KF。建立起KF中的路标点和F中的特征点间的联系。但这会使得很多匹配没有被检测到，因此还需将KF中的路标点投影到F中，进一步查找，以增加KF中的路标点和F中的特征点的匹配数量。

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)

（1）建立旋转直方图，用于检测旋转一致性。
（2）遍历关键帧中的每个地图点，通过相机投影模型，得到投影到当前帧的像素坐标。
（3）确定搜索半径，获得候选特征点。
（4）遍历候选匹配点，寻找距离最小的最佳匹配点。
（5）计算匹配点旋转角度差所在的直方图。
（6）进行旋转一致检测，剔除不一致的匹配。

在这里插入图片描述

回环检测时的投影

首先使用词袋加速算法，找到当前关键帧的闭环关键帧。但会有遗漏。
此时已经找到了当前关键帧KFC的闭环关键帧KF（与KFC相似程度最高的关键帧）。将KF及其共视关键帧全部投影至KFC中，建立3D-2D联系。

int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)

流程为：
（1）分解Sim变换矩阵，获得KF，KFC间的位姿变换关系。
（2）遍历闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF。
（3）根据预测的路标点所在的金字塔层级获得搜索半径。搜索候选匹配特征点。
（4）遍历候选匹配点，找到最佳匹配点。
（5）建立路标点与特征点的联系。

词袋模型加速匹配函数

词袋算法的原理上面已经介绍了。建立的是当前帧的特征点和参考帧的路标点间的联系。
在后一帧追踪前一帧、追踪局部地图时，不需要去寻找与当前帧最相似的一帧，因此无需使用。
在回环检测中，需要寻找当前关键帧的闭环关键帧。在重定位时，需要寻找与当前帧最相似的关键帧。此时使用词袋加速算法，虽然会有很多遗漏的匹配，但可以快速的找到目标帧。

重定位中的词袋加速

int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)

流程为：
（1）分别取出两图中属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)。
（2）遍历KF中属于该node的特征点。
（3）遍历F中属于该node的特征点，寻找最佳匹配点。
（4）根据汉明距离阈值和旋转直方图剔除误匹配(关键帧和当前帧进行特征点匹配之后，已知两个特征点的角度，从而可以获得角度变化)。

回环检测中的词袋加速

int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)

流程与重定位中的流程相同。

检测极线距离是否符合要求

bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)

在已知基础矩阵F的情况下，可获得图像1中的特征点，在图像2中的极线方程。在已知基础矩阵的情况下，用于检测所匹配的特征点是否正确。
在这里插入图片描述

    // Epipolar line in second image l2 = x1'F12 = [a b c]// Step 1 求出kp1在pKF2上对应的极线const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(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;

单目初始化中的特征点匹配

选第1帧作为参考，第2帧去匹配第一帧。如果匹配失败的话，第3帧作为参考，第4帧去匹配。

int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)

流程为：
（1）构建旋转直方图。
（2）在半径窗口内搜索当前帧F2中所有的候选匹配特征点。
（3）遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的。
（4）对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配。
（5）计算匹配点旋转角度差所在的直方图，筛除旋转直方图中“非主流”部分。
（6）将最后通过筛选的匹配好的特征点保存。

局部建图中的特征点匹配

因为在local mapping线程中，关键帧的位姿已经相当准确了，即F12也是比较准确的。所以可以使用三角化生成新的MapPoint，匹配使用的是BoW，筛选部分使用了对极点邻域剔除、极线约束、角度投票（旋转一致性）进行剔除误匹配。

int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)

（1）使用词袋加速匹配，获得匹配点对。
（2）使用特征点到极线的距离、旋转直方图剔除误匹配。

回环检测中寻找更多的匹配点

在回环检测时使用，考虑了尺度漂移，因此使用Sim3（Sim3有7自由度）。使用词袋加速算法获得的匹配点存在遗漏，因此对当前帧和回环帧中的ORB特征相互投影，从而找到更多的匹配点。
（1）遍历KF1中的地图点，如果该地图点已经匹配（使用词袋加速算法时已经匹配了），就跳过。
（2）转换为KF2中的相机坐标、像素坐标。
（3）在搜索区域找到候选特征点。
（4）根据汉明距离，找到最优特征点。得到vnMatch1[i1]=bestIdx。其中i1为KF1中特征点的索引，bestIdx为在KF2中找到的特征点的索引。
（5）遍历KF2中的地图点，重复以上步骤，获得vnMatch1[i2]=bestIdx。
（6）比较KF1中的特征点和KF2中的特征点是否相互索引。

int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12,const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)

路标点融合函数

就是将未匹配的特征点投影至图像中，进行路标点的融合
将路标点1投影至当前帧中，寻找与之匹配的特征点。
（1）若该特征点存在与之相连的路标点2，则在路标点1，路标点2中选择被观测次数最多的点。
（2）若该特征点没有与之相连的路标点，则建立特征点与路标点1间的联系。

int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)

在这里插入图片描述

https://www.dkcj.cn/info/10119.html

匹配策略

约束条件

SearchByProjection函数的重载

局部地图跟踪

当前帧追踪上一帧

重定位中的投影

回环检测时的投影

词袋模型加速匹配函数

重定位中的词袋加速

回环检测中的词袋加速

检测极线距离是否符合要求

单目初始化中的特征点匹配

局部建图中的特征点匹配

回环检测中寻找更多的匹配点

路标点融合函数

相关文章：