ORB-SLAM2特点匹配代码分析
本文代码注释为计算机视觉life小六老师的工作，在此主要供大家一起学习。
Track()
首先我们进入Track.cpp文件中，需要进入Track函数来进行单目初始化
void Tracking::Track()
{
// track包含两部分：估计运动、跟踪局部地图
// mState为tracking的状态，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState 存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
// 上锁。保证地图不会发生变化
// 疑问:这样子会不会影响地图的更新?
// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Step 1：初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
//双目RGBD相机的初始化共用一个函数
StereoInitialization();
else
//单目初始化
MonocularInitialization();
//更新帧绘制器中存储的最新状态
mpFrameDrawer->Update(this);
//这个状态量在上面的初始化函数中被更新
if(mState!=OK)
return;
}
判断mState为未初始化状态后，进入**MonocularInitialization()**中进行单目初始化
* @brief 单目的地图初始化
*
* 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
* 得到初始两帧的匹配、相对运动、初始MapPoints
*
* Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
* Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
* Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
* Step 4：如果初始化的两帧之间的匹配点太少，重新初始化
* Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
* Step 6：删除那些无法进行三角化的匹配点
* Step 7：将三角化得到的3D点包装成MapPoints
*/
void Tracking::MonocularInitialization()
{
// Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
if(!mpInitializer)
{
// Set Reference Frame
// 单目初始帧的特征点数必须大于100
if(mCurrentFrame.mvKeys.size()>100)
{
// 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
mInitialFrame = Frame(mCurrentFrame);
// 用当前帧更新上一帧
mLastFrame = Frame(mCurrentFrame);
// mvbPrevMatched 记录"上一帧"所有特征点
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
// 删除前判断一下，来避免出现段错误。不过在这里是多余的判断
// 不过在这里是多余的判断，因为前面已经判断过了
if(mpInitializer)
delete mpInitializer;
// 由当前帧构造初始器 sigma:1.0 iterations:200
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
// 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
}
else //如果单目初始化器已经被创建
{
// Try to initialize
// Step 2 如果当前帧特征点数太少（不超过100），则重新构造初始器
// NOTICE 只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
// Find correspondences
// Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
ORBmatcher matcher(
0.9, //最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7
true); //检查特征点的方向
// 对 mInitialFrame,mCurrentFrame 进行特征点匹配
// mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标
// mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
int nmatches = matcher.SearchForInitialization(
mInitialFrame,mCurrentFrame, //初始化时的参考帧和当前帧
mvbPrevMatched, //在初始化参考帧中提取得到的特征点
mvIniMatches, //保存匹配关系
100); //搜索窗口大小
// Check if there are enough correspondences
// Step 4 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
// Step 5 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
if(mpInitializer->Initialize(
mCurrentFrame, //当前帧
mvIniMatches, //当前帧和参考帧的特征点的匹配关系
Rcw, tcw, //初始化得到的相机的位姿
mvIniP3D, //进行三角化得到的空间点集合
vbTriangulated)) //以及对应于mvIniMatches来讲,其中哪些点被三角化了
{
// Step 6 初始化成功后，删除那些无法进行三角化的匹配点
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
// Set Frame Poses
// Step 7 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到相机坐标系的变换矩阵
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);
// Step 8 创建初始化地图点MapPoints
// Initialize函数会得到mvIniP3D，
// mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
// CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
CreateInitialMapMonocular();
}//当初始化成功的时候进行
}//如果单目初始化器已经被创建
}
- Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
- Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
- Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
- Step 4：如果初始化的两帧之间的匹配点太少，重新初始化
- Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
- Step 6：删除那些无法进行三角化的匹配点
- Step 7：将三角化得到的3D点包装成MapPoints
在本篇博客中主要讲解的是 Step3，也就是在单目初始化过程中两相邻帧之间的特征点匹配，关于特征点提取的程序下次有机会在写一下。
上面的程序中在特征点匹配之前，我们已经得到连续的两帧Frame，并且两帧提取的特征点数量都大于一百，这时我们创建一个特征点匹配器matcher
，进行单目初始化两帧特点的匹配matcher.SearchForInitialization
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
{
int nmatches=0;
// F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间
vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);
// Step 1 构建旋转直方图，HISTO_LENGTH = 30
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
// 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量
rotHist[i].reserve(500);
//! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码
const float factor = HISTO_LENGTH/360.0f;
// 匹配点对距离，注意是按照F2特征点数目分配空间
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
// 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
// 遍历帧1中的所有特征点
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
cv::KeyPoint kp1 = F1.mvKeysUn[i1];
int level1 = kp1.octave;
// 只使用原始图像上提取的特征点
if(level1>0)
continue;
// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
// vbPrevMatched 输入的是参考帧 F1的特征点
// windowSize = 100，输入最大最小金字塔层级 均为0
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
// 没有候选特征点，跳过
if(vIndices2.empty())
continue;
// 取出参考帧F1中当前遍历特征点对应的描述子
cv::Mat d1 = F1.mDescriptors.row(i1);
int bestDist = INT_MAX; //最佳描述子匹配距离，越小越好
int bestDist2 = INT_MAX; //次佳描述子匹配距离
int bestIdx2 = -1; //最佳候选特征点在F2中的index
// Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
size_t i2 = *vit;
// 取出候选特征点对应的描述子
cv::Mat d2 = F2.mDescriptors.row(i2);
// 计算两个特征点描述子距离
int dist = DescriptorDistance(d1,d2);
if(vMatchedDistance[i2]<=dist)
continue;
// 如果当前匹配距离更小，更新最佳次佳距离
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestIdx2=i2;
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
// 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值
if(bestDist<=TH_LOW)
{
// 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高
if(bestDist<(float)bestDist2*mfNNratio)
{
// 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
if(vnMatches21[bestIdx2]>=0)
{
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--;
}
// 次优的匹配关系，双向建立
// vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;
vMatchedDistance[bestIdx2]=bestDist;
nmatches++;
// Step 5 计算匹配点旋转角度差所在的直方图
if(mbCheckOrientation)
{
// 计算匹配特征点的角度差，这里单位是角度°，不是弧度
float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
// 前面factor = HISTO_LENGTH/360.0f
// bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin
int bin = round(rot*factor);
// 如果bin 满了又是一个轮回
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(i1);
}
}
}
}
// Step 6 筛除旋转直方图中“非主流”部分
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
int idx1 = rotHist[i][j];
if(vnMatches12[idx1]>=0)
{
vnMatches12[idx1]=-1;
nmatches--;
}
}
}
}
//Update prev matched
// Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
}
STEP1首先构建了一个旋转直方图rotHist，这里讲一下直方图的作用，简单来说，是通过两个匹配的特征点之间的旋转关系来筛选掉一些旋转值异常的匹配特征点，这里我们后面会比较详细的介绍。
STEP2遍历Frame1中金字塔第一层中的所有特征点，在Frame2中对应Frame1中特征点附近范围中寻找匹配的特征点，进入GetFeaturesInArea函数
* @brief 找到在 以x,y为中心,半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点
*
* @param[in] x 特征点坐标x
* @param[in] y 特征点坐标y
* @param[in] r 搜索半径
* @param[in] minLevel 最小金字塔层级
* @param[in] maxLevel 最大金字塔层级
* @return vector<size_t> 返回搜索到的候选匹配点id
*/
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
{
// 存储搜索结果的vector
vector<size_t> vIndices;
vIndices.reserve(N);
// Step 1 计算半径为r圆左右上下边界所在的网格列和行的id
// 查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕，慢慢理解下：
// (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1）
// mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1）
// (x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数
// 两者相乘，就是求出那个半径为r的圆的左侧边界在哪个网格列中
// 保证nMinCellX 结果大于等于0
const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv));
// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vector
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
// 计算圆所在的右边界网格列索引
const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vector
if(nMaxCellX<0)
return vIndices;
//后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的id
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
// 检查需要搜索的图像金字塔层数范围是否符合要求
//? 疑似bug。(minLevel>0) 后面条件 (maxLevel>=0)肯定成立
//? 改为 const bool bCheckLevels = (minLevel>=0) || (maxLevel>=0);
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);
// Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
const vector<size_t> vCell = mGrid[ix][iy];
// 如果这个网格中没有特征点，那么跳过这个网格继续下一个
if(vCell.empty())
continue;
// 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
// 根据索引先读取这个特征点
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
// 保证给定的搜索金字塔层级范围合法
if(bCheckLevels)
{
// cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据
// 保证特征点是在金字塔层级minLevel和maxLevel之间，不是的话跳过
if(kpUn.octave<minLevel)
continue;
if(maxLevel>=0) //? 为何特意又强调？感觉多此一举
if(kpUn.octave>maxLevel)
continue;
}
// 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 如果x方向和y方向的距离都在指定的半径之内，存储其index为候选特征点
if(fabs(distx)<r && fabs(disty)<r)
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
这个函数的主要作用可以描述为：从Frame1的金字塔第一层选取特征点，然后在Frame2中的金字塔第一层相同的坐标为原点，r为半径选取所有在范围内的Grid，然后遍历所有范围内的Grid，将所有Grid内的特征点选为待匹配的点。
STEP3遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
size_t i2 = *vit;
// 取出候选特征点对应的描述子
cv::Mat d2 = F2.mDescriptors.row(i2);
// 计算两个特征点描述子距离
int dist = DescriptorDistance(d1,d2);
if(vMatchedDistance[i2]<=dist)
continue;
// 如果当前匹配距离更小，更新最佳次佳距离
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestIdx2=i2;
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
Step4对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
if(bestDist<=TH_LOW)
{
// 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高
if(bestDist<(float)bestDist2*mfNNratio)
{
// 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
if(vnMatches21[bestIdx2]>=0)
{
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--;
}
// 次优的匹配关系，双向建立
// vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;
vMatchedDistance[bestIdx2]=bestDist;
nmatches++;
// Step 5 计算匹配点旋转角度差所在的直方图
if(mbCheckOrientation)
{
// 计算匹配特征点的角度差，这里单位是角度°，不是弧度
float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
// 前面factor = HISTO_LENGTH/360.0f
// bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin
int bin = round(rot*factor);
// 如果bin 满了又是一个轮回
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(i1);
}
}
}
Step 6 筛除旋转直方图中“非主流”部分
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配对，因为他们不符合“主流旋转方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
int idx1 = rotHist[i][j];
if(vnMatches12[idx1]>=0)
{
vnMatches12[idx1]=-1;
nmatches--;
}
}
}
}
进入ComputeThreeMaxima函数
* @brief 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引
*
* @param[in] histo 匹配特征点对旋转方向差直方图
* @param[in] L 直方图尺寸
* @param[in & out] ind1 bin值第一大对应的索引
* @param[in & out] ind2 bin值第二大对应的索引
* @param[in & out] ind3 bin值第三大对应的索引
*/
void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)
{
int max1=0;
int max2=0;
int max3=0;
for(int i=0; i<L; i++)
{
const int s = histo[i].size();
if(s>max1)
{
max3=max2;
max2=max1;
max1=s;
ind3=ind2;
ind2=ind1;
ind1=i;
}
else if(s>max2)
{
max3=max2;
max2=s;
ind3=ind2;
ind2=i;
}
else if(s>max3)
{
max3=s;
ind3=i;
}
}
// 如果差距太大了,说明次优的非常不好,这里就索性放弃了
if(max2<0.1f*(float)max1)
{
ind2=-1;
ind3=-1;
}
else if(max3<0.1f*(float)max1)
{
ind3=-1;
}
}
step7将最后通过筛选的匹配好的特征点保存到vbPrevMatched
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
