单目初始化中的特征点搜索匹配、搜索候选匹配特征点
Frame 跳转 Track
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;
开启线程锁，判断如果状态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();
Track 跳转 MonocularInitialization
下面是针对单目的初始化函数。
void Tracking::MonocularInitialization()
{
如果初始化器没有创建则创建初始化器。
创建初始化器时，当前帧特征点数目要＞100，否则不创建。
若满足100特征点条件，则用当前帧拷贝给初始帧mInitialFrame 和最新帧mLastFrame 。
第一帧的特征点要求100个，比后面的帧要求高，以第一帧做模板分配初始化器的容量，后面的帧就都不会超出容量？？？（猜测）
// Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
if(!mpInitializer)
{
// Set Reference Frame
// 单目初始帧的特征点数必须大于100
if(mCurrentFrame.mvKeys.size()>100)
{
// 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
mInitialFrame = Frame(mCurrentFrame);
// 用当前帧更新上一帧
mLastFrame = Frame(mCurrentFrame);
mvbPrevMatched存储当前帧特征点信息。
// 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;
}
}
如果初始化器已被创建，判断如果当前帧特征点数小于100，则删除这个初始化器。
1.C++中的
static_cast执行非多态的转换，用于代替C中通常的转换操作。
如：i = static_cast<int>(f);将f强制转换成int类型。
2.C++fill函数的作用是赋初始值
如：int a[5]; fill(a,a+5,2);
意思是：a[0]~a[5]赋初值2。
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;
}
创建匹配器matcher，调用初始化时的匹配函数SearchForInitialization对初始帧和当前帧特征匹配。
如果匹配结果还是＜100，删除初始化器。
// 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;
}
MonocularInitialization 跳转 SearchForInitialization
参数：
F1：初始帧F2：当前帧vbPrevMatched：存储初始帧的匹配到的特征点windowSize：窗口大小
int ORBmatcher::SearchForInitialization(Frame &F1,
Frame &F2,
vector<cv::Point2f> &vbPrevMatched,
vector<int> &vnMatches12,
int windowSize)
{
创建容器vnMatches12 用来存储F1（初始帧）与F2（当前帧）的特征点匹配关系，容量为F1的大小，值为-1表示F2中没有对应的匹配。
创建旋转直方图rotHist。有30个长度，每个长度分配500个空间。
创建容器vMatchedDistance存储F1 F2特征点匹配距离。
创建容器vnMatches21存储F2与F1的特征点匹配关系（与vnMatches12 成反向匹配）。
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);
遍历F1中的特征点，判断如果图像金字塔层数>0则跳过（只关心原始层）。
// 遍历帧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;
GetFeaturesInArea函数里输入F1的特征点信息，F2调用该函数，在F2中以windowSize=100为半径的窗口寻找F2的对应的特征点。
// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
// vbPrevMatched 输入的是参考帧 F1的特征点
// windowSize = 100，输入最大最小金字塔层级 均为0
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
SearchForInitialization 跳转 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);
nMinCellX 表示圆左侧边界所在栅格数（列）
nMaxCellX表示圆右侧边界所在栅格数（列）
并分别判断是否超出边界，若超出则返回空vector。
nMinCellY 、nMaxCellY 类似。
// 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);
以栅格为单位遍历圆形区域（实际上是矩形），mGrid中存储的是特征点的序号，在笔记7中提到过：
// 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;
如果栅格中有特征点，那么根据该特征点序号，把特征点从去畸变特征点容器中取出，
如果特征点的xy坐标都与圆心xy坐标差r以下，那么将该特征点存储到vIndices容器中，最后返回该容器。
// 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
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;
}
扫一扫
655
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
