单目初始化是在Tracking线程中Track函数的第一步，单目初始化最重要的一点就是尺度问题，因为单目相机无法得到空间的绝对位置，例如：程序中输出的 t t t 第一维是2.8，那么这个2.8指的是2.8米还是2.8厘米，这对于单目相机来说是无法确定的，换句话说，单目相机建立的地图缩放任意倍数，依然满足对级约束。因此，单目相机需要这样一个初始化过程，确定这个地图的相对尺度（就是在第一步中计算的尺度作为后续计算的基单位）。
单目初始化是在MonocularInitialization函数中，其中包括以下几个部分，也是接下来需要讲解的各个部分：
- 创建单目初始器（只运行一次）
- 特征点匹配（重点）
- 计算帧间运动
- 初始化地图
创建单目初始器
单目相机因为每次只有一帧图像传入，无法直接通过单帧图像计算出空间信息，因此至少要有两帧（但是这两帧也必须是质量高的），才能够完成计算。ORBSLAM2创建了单目初始器，就是用来寻找高质量的两帧图像，供后续处理，简单的流程如下👇
- 1.设置
mpInitializer作为是否是第一帧的判断 - 2.如果是第一帧，那么此帧特征点必须大于100，然后记录此帧的信息（存储特征点、建立初始化器防止下次再次进入这里）
- 3.如果第一帧创建，那么就判断第二帧特征点是否大于100，如果不够100，就重新来过，否则就对两帧进行初始化过程
需要注意的👇
- ⏩只有连续的两帧都是高质量（特征点数都大于100）才会进行接下来的初始化过程，否则会不断的进行寻找高质量初始帧的过程
- ⏩第一帧被初始化为
mInitialFrame，第二帧被初始化为mCurrentFrame（每一帧刚进来都是这个）
void Tracking::MonocularInitialization()
{
if(!mpInitializer)
{
// 单目初始帧的特征点数必须大于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 //第一帧已经建立，等待第二帧的进入
{
//如果当前帧特征点数太少（不超过100），则重新构造初始器
//只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
/************后面的单目初始化过程都在这个else中************/
/*********************特征点匹配***********************/
/*********************计算帧间运动*********************/
/*********************设置世界坐标*********************/
/*********************初始化地图***********************/
}
}
特征点匹配
特征点的匹配是将第一帧mInitialFrame和第二帧mCurrentFrame提取的特征点进行匹配，寻找相同空间点在不同帧下的投影（就是特征点），其中最重要的函数就是SearchForInitialization，也是下面要讲解的，现在先梳理一下这个函数的输入输出：
mInitialFrame、mCurrentFrame：既然是做两帧之间的特征点匹配，就一定需要两帧数据的输入mvbPrevMatched：作为输入存储的是第一帧中的特征点（在上面创建单目初始器时传入的特征点坐标），经过特征匹配后，存放的都是匹配好的第一帧中的特征点坐标（其他未匹配的都删掉了）mvIniMatches：输出，保存第一帧的特征点跟第二帧之间的匹配，index保存是第一帧对应特征点索引，值保存的是匹配好的第二帧特征点索引- 返回值
nmatches：匹配的特征点数量
匹配的示意图：
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); //搜索窗口大小
//如果初始化的两帧之间的匹配点太少，重新初始化
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
SearchForInitialization
这个函数就是特征点的匹配过程，在两帧图像中寻找相同空间位置的两个点是很困难的，所以这个匹配过程也是需要经过重重筛选，简单说明一下流程：
- 1.构建旋转直方图（用于剔除方向差不一致的特征点）
- 2.搜索第一帧每个特征点在第二帧的固定半径内，可能的候选匹配特征点，主要是
GetFeaturesInArea函数中完成 - 3.通过计算描述子距离，在这些候选匹配特征点中找到最优点和次优点，主要是
DescriptorDistance函数计算描述子距离 - 4.对最优次优点的结果进行检查：描述子距离阈值、最优次优描述子距离比例、重复匹配
- 5.计算检查后的一对匹配特征点的角度差（就是灰度质心法求得的），存入直方图中
- 6.筛选直方图中角度差不一致的已匹配特征点对，并整理
需要注意的👇
- ⏩所谓的特征匹配是将第一帧中的特征点跟第二帧去匹配（不能反了），因此遍历特征点都是遍历的第一帧的特征点
- ⏩遍历第一帧特征点的时候，这个特征点仅仅使用了原始图像上的特征点，也就是金字塔的最底层中的特征点（为什么不使用其他层的，我想一个是这两帧是连续的两帧，相同的特征点本应该就很多，再一个我认为可能是想快速的完成初始化过程，毕竟金字塔最底层的特征点足够了）
- ⏬特征点匹配过程经过了多次检验，下面每个都介绍一下：
-
- ⏩在固定半径内搜索：在视觉slam十四讲中提到过特征点匹配的过程，但是书中主要介绍的是暴力匹配（就是将第一帧的每个特征与第二帧的每个特征进行比较），这种方法耗时耗力；因此ORBSLAM2中就使用了方框搜索的方法：在提取特征点之后，将整张图像分割成一个个方格，特征点根据坐标位置存放在不同的方格中；在特征匹配时，将第一帧每个特征点a坐标放在第二帧图像中后，寻找半径r范围内的所有涉及到的方格，求解这些方格中的特征点坐标与特征点a坐标的距离，判断是否在半径r内。那么为什么说是在固定半径内搜索而不是方格搜索？因为最终只是想要在半径r内的特征点，而使用方格去搜索特征点是为了不去计算每个特征点与特征点a的距离，形成了暴力匹配这种耗时的方法。在后面会主要讲解半径搜索过程
GetFeaturesInArea函数如何实现 - ⏩计算描述子距离：特征点的描述子是根据特征点周围固定点对的对比得到的，是一个二进制信息，因此使用的是汉明距离计算描述子距离，就是计算两个描述子之间不同二进制位数的数量，ORBSLAM2中不仅仅使用了描述子距离筛选最优和次优匹配特征点，还自定义了最小阈值。
- ⏩最优次优描述子距离比例：在上面有这么一行代码
ORBmatcher matcher(0.9,true);，其中0.9就是设置这个比例的，指的是对于当前第一帧某一个特征点a来说，最优匹配特征点的描述子距离（对a）/次优匹配特征点的描述子距离（对a）=0.9（注意：是描述子距离，就是两个特征点的相似程度，越小越好），在初始化的时候这个是0.9，在跟踪的时候会较严格设置为0.7。为什么要设置这个最优次优，或者说这个比例的意义？设置最优次优主要就是为了给匹配的特征点设置一个条件，就是这个比例（因此主要就使用最优的，而次优就是因为比例而存在），这个比例是最优/次优，那么就意味着这个值越小，最优和次优之间的“距离”越远，越发的能够表明这个最优的匹配特征点是唯一最好的匹配，而这个0.9就是一个比较宽松的阈值，如果设置成0.7，那么就能够得到更好的匹配点 - ⏩重复匹配：假设第一帧的特征点a，经过特征匹配到第二帧的特征点c，然而第一帧的特征点b，经过特征匹配也一样匹配到第二帧的特征点c，这就叫重复匹配（就是两个特征点匹配到了同一个点）。具体做法就是删除这个特征点c的所有匹配关系，因为并不清楚哪个才是真正对的匹配。因此在程序能够看到设定了
vnMatches12和vnMatches21两个变量，供筛选重复匹配 - ⏩剔除角度差不一致的点：在
SearchForInitialization函数中开头建立了旋转直方图，这个旋转直方图就是存放的匹配点之间的角度差（灰度质心法的角度），将整个360°的角度分成每12°为一格的30个格（作为横坐标），两帧之间匹配点对的角度差（作为纵坐标），这些都是为了剔除角度差不一致的点，其思想就是：两帧之间，如果是真正匹配的n个点对，那么n个点对之间的角度差应该也是差不多的。由此可以剔除掉那些**角度差“不合群”**的点。在ORBSLAM2中，将这些点对的角度差落在直方图中，只提取直方图中最大的三个分布，其余不合群的点就删除（实际上，并没有多少点是在这一步剔除的）。如下图，就是匹配角度差不一致的点，会被删除：
- ⏩在固定半径内搜索：在视觉slam十四讲中提到过特征点匹配的过程，但是书中主要介绍的是暴力匹配（就是将第一帧的每个特征与第二帧的每个特征进行比较），这种方法耗时耗力；因此ORBSLAM2中就使用了方框搜索的方法：在提取特征点之后，将整张图像分割成一个个方格，特征点根据坐标位置存放在不同的方格中；在特征匹配时，将第一帧每个特征点a坐标放在第二帧图像中后，寻找半径r范围内的所有涉及到的方格，求解这些方格中的特征点坐标与特征点a坐标的距离，判断是否在半径r内。那么为什么说是在固定半径内搜索而不是方格搜索？因为最终只是想要在半径r内的特征点，而使用方格去搜索特征点是为了不去计算每个特征点与特征点a的距离，形成了暴力匹配这种耗时的方法。在后面会主要讲解半径搜索过程
//输入F1是第一帧，F2是第二帧，windowSize是100（就是半径搜索）
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);
//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; 是错误的，更改为下面代码
//就是准备分解成30个格，每个格12°
const float factor = HISTO_LENGTH/360.0f;
//匹配的点对描述子距离，初始化为INT_MAX，注意是按照F2特征点数目分配空间
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
//建立一个双向匹配，用于后面的剔除重复匹配的点
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
//遍历F1中的所有特征点，开始进行特征匹配
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
cv::KeyPoint kp1 = F1.mvKeysUn[i1];
int level1 = kp1.octave;
//只提取第0层特征点，也就是原图像大小
if(level1>0)
continue;
//2.在半径窗口内搜索F2中所有的候选匹配特征点
/***************下面详细讲解此函数***************/
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
//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;
//下面就是更新最佳次佳距离，和最佳的F2匹配的特征点索引i2
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestIdx2=i2;
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
//4.对最优次优结果进行检查，满足最小阈值、最优/次优比例，删除重复匹配
if(bestDist<=TH_LOW)
{
//最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高
if(bestDist<(float)bestDist2*mfNNratio)
{
//删除重复匹配，将原来的匹配也删掉
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++;
//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);
}
}
}
}
//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--;
}
}
}
}
//将最后通过筛选的匹配好的特征点保存到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;
}
GetFeaturesInArea
此函数就是用于计算第一帧特征点a在第二帧中半径100以内的特征点，称为候选匹配特征点。为了防止出现暴力匹配的耗时行为，使用方格搜索，如下图：
上图中，黑色虚线代表着边界四点经过畸变矫正后设置的图像边界，红色框是图像分割的方格（用于特征匹配），红色点代表前一帧在当前帧图像上的坐标，黑色点是当前帧图像上的特征点，绿色圆是半径搜索的范围，绿色虚线是圆的最大边界，黑色框是最大边界所在的最大范围的需搜索的方格
对这个函数的搜索流程进行简要说明👇
- 1.根据需要匹配的特征点坐标（红色点），按照半径100搜索范围内找到这个圆范围内的最大边界（绿色虚线），
x-mnMinX-r就是绿色左边虚线所在的横坐标，然后乘以mfGridElementWidthInv（每个像素可以均分几个方格列），就得到搜索框（黑色框）最左边方格的横坐标（也可以说是横向索引值）。分别计算四个方向，就得到了搜索框（黑色框）内方格（红色方格）的索引值 - 2.遍历搜索框（黑色框）中所有的方格（红色方格），提取每个方格中的特征点坐标（黑色点），计算其与需要匹配的特征点（红色点）之间的距离是否满足100，在100以内就保存下来给返回值，否则删除。
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);
//计算半径为r圆左右上下边界所在的方格列和行的索引
//(mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1）
//mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1）
// (x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数
// 保证nMinCellX 结果大于等于0
const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv));
//如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
//分别计算四个方向
const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;
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;
// 检查需要搜索的图像金字塔层数范围是否符合要求
const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);
//遍历搜索框内的所有网格，寻找满足条件的候选特征点，并将其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;
}
计算帧间运动
帧间运动指的就是两个相机之间的位姿，而在最开始只有两张图像的基础上，计算位姿：R（旋转矩阵）和t（位移向量），就只能依靠对极约束和一些相应的条件，需要使用基础矩阵F和单应矩阵H计算。那么就涉及到基础矩阵F和单应矩阵H的计算和分解成R,t的过程。
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
//计算帧间运动，首先计算基础矩阵F和单应矩阵H，最后根据计算的矩阵恢复R，t
if(mpInitializer->Initialize(
mCurrentFrame, //当前帧（第二帧）
mvIniMatches, //当前帧和参考帧（第一帧）的特征点的匹配关系
Rcw, tcw, //初始化得到的相机的位姿
mvIniP3D, //进行三角化得到的空间点集合
vbTriangulated)) //以及对应于mvIniMatches来讲,其中哪些点被三角化了
{
//初始化成功后，删除那些无法进行三角化的匹配点
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
//将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
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);
/************************初始化地图************************/
CreateInitialMapMonocular();
}
Initialize
在计算基础矩阵和单应矩阵的时候，都是使用的8点法，因此需要使用8对匹配好的特征点进行计算，那么在计算前就应该将特征点对准备好，然后再计算，最后分解求解位姿。简单的流程如下👇
- 1.整理好匹配点对的关系
- 2.在所有匹配特征点对中随机选择8对点为一组
- 3.计算基础矩阵F和单应矩阵H，分开两个线程计算
- 4.根据评比分数选取哪个模型求解R,t
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{
//获取当前帧的去畸变之后的特征点
mvKeys2 = CurrentFrame.mvKeysUn;
//使用对组存放在mvMatches12中
mvMatches12.clear();
// 预分配空间，大小和关键点数目一致mvKeys2.size()
mvMatches12.reserve(mvKeys2.size());
// 记录参考帧1中的每个特征点是否有匹配的特征点
// 这个成员变量后面没有用到，后面只关心匹配上的特征点
mvbMatched1.resize(mvKeys1.size());
//1.整理匹配好的点对关系
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
//vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
//没有匹配关系的话，vMatches12[i]值为 -1
if(vMatches12[i]>=0)
{
//把匹配关系存放在mvMatches12对组中
mvMatches12.push_back(make_pair(i,vMatches12[i]));
mvbMatched1[i]=true;
}
else
mvbMatched1[i]=false;
}
// 有匹配的特征点的对数
const int N = mvMatches12.size();
// Indices for minimum set selection
// 新建一个容器vAllIndices存储特征点索引，并预分配空间
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
//在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引
vector<size_t> vAvailableIndices;
//初始化所有特征点对的索引，索引值0到N-1
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
//在所有匹配特征点对中随机选择8对点为一组
//共选择 mMaxIterations (默认200) 组
//mvSets用于保存每次迭代时所使用的向量
mvSets = vector< vector<size_t> >(mMaxIterations, //最大的RANSAC迭代次数
vector<size_t>(8,0));
//用于进行随机数据样本采样，设置随机数种子
DUtils::Random::SeedRandOnce(0);
//就是说最后计算了mMaxIterations个矩阵，从中选择最好的，用于分解位姿
//将所有的特征点对分成每8对为一组，将索引值存放在mvSets，而索引值代表的就是mvMatches12中的索引
for(int it=0; it<mMaxIterations; it++)
{
//迭代开始的时候，所有的点都是可用的
vAvailableIndices = vAllIndices;
for(size_t j=0; j<8; j++)
{
// 随机产生一对点的id,范围从0到N-1
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一个索引对应的特征点对被选中
int idx = vAvailableIndices[randi];
//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中
mvSets[it][j] = idx;
// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,
// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素
// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
//3.计算基础矩阵F和单应矩阵H，分开两个线程计算
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF;
cv::Mat H, F;
// 构造线程来计算H矩阵及其得分
// thread方法比较特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝
thread threadH(&Initializer::FindHomography, //该线程的主函数
this, //由于主函数为类成员函数，所以第一个参数就应该是当前对象的this指针
ref(vbMatchesInliersH), //输出，特征点对的Inlier标记
ref(SH), //输出，计算的单应矩阵的RANSAC评分
ref(H)); //输出，计算的单应矩阵结果
// 计算fundamental matrix并打分，参数定义和H是一样的，这里不再赘述
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
//等待两个计算线程结束
threadH.join();
threadF.join();
//4.根据评比分数选取哪个模型求解R,t
//通过这个规则来判断谁的评分占比更多一些，注意不是简单的比较绝对评分大小，而是看评分的占比
float RH = SH/(SH+SF); //RH=Ratio of Homography
// 注意这里更倾向于用H矩阵恢复位姿。如果单应矩阵的评分占比达到了0.4以上,则从单应矩阵恢复运动,否则从基础矩阵恢复运动
if(RH>0.40)
//更偏向于平面，此时从单应矩阵恢复，函数ReconstructH返回bool型结果
return ReconstructH(vbMatchesInliersH, //输入，匹配成功的特征点对Inliers标记
H, //输入，前面RANSAC计算后的单应矩阵
mK, //输入，相机的内参数矩阵
R21,t21, //输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换
vP3D, //特征点对经过三角测量之后的空间坐标，也就是地图点
vbTriangulated, //特征点对是否成功三角化的标记
1.0, //这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时
//需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
50); //为了进行运动恢复，所需要的最少的三角化测量成功的点个数
else //if(pF_HF>0.6)
// 更偏向于非平面，从基础矩阵恢复
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
return false;
}
矩阵模型的计算
基础矩阵F和单应矩阵H的计算原理虽然不同，但是整体流程差不多，循环这8对点组成的数组，使用这8对点进行矩阵计算，计算后会进行评比，使用评比分数最大的矩阵作为最终的矩阵模型：
矩阵模型的分解
这时已经计算了最好的矩阵模型，需要对这个最好的矩阵模型进行分解求解R,t，但是由于分解原理会得到多个R,t的解，就需要分别使用位姿R,t计算地图点，判断地图点是否符合要求，就会得到最好的位姿R,t解：
初始化地图
其实到这里初始化的功能基本结束了：通过两帧图像计算得到了位姿，也得到了一些地图点，后面一切的过程都是在这个基础上完成的。
而这个初始化地图部分，就是将已经计算的位姿和地图点封装，然后显示在地图中，这部分的内容主要就是一些逻辑的处理和优化。
构造关键帧
将第一帧和第二帧封装成关键帧，主要就是一些数据的初始化，比如id、位姿等等
描述子转BoW
pKFini->ComputeBoW();
词袋BoW主要是在后面闭环检测中使用，就是我们在回环检测的时候，需要用到词袋向量mBowVec和特征点向量mFeatVec，所以这里要计算
添加关键帧
mpMap->AddKeyFrame(pKFini);
将该关键帧插入到地图中，用于地图的绘制
构造地图点
将三角化后的地图点封装成地图点，主要就是初始化，比如id、坐标等等
添加地图点
//i是该地图点的索引，用于寻找该地图点
pKFini->AddMapPoint(pMP,i);
将地图点存放在该关键帧中，主要的变量是mvpMapPoints[idx]=pMP
添加观测信息
//i是该地图点的索引，用于寻找该地图点
pMP->AddObservation(pKFini,i);
向该地图点添加能够观测它的关键帧，主要变量是mObservations[pKF]=idx;
挑选描述子
pMP->ComputeDistinctiveDescriptors();
在特征点提取的时候，对每个特征点进行描述子的计算，但是对于地图点来说，需要一个最能代表这个地图点的描述子，因此，需要在该地图点的多个特征点描述子中，挑选一个合适的：就是选取的描述子与其他描述子应该具有最小的距离中值
- 查找该地图点所有能观测到的关键帧，找到该地图点对应特征点的描述子
- 计算两两之间的描述子距离（汉明距离）
- 将描述子距离使用一个对称的矩阵存储（注意矩阵中数值指的是描述子距离）
- 取矩阵每一行的中值，设置这些中值最小值对应行所在的描述子，作为最合适的描述子
观测方向和距离
pMP->UpdateNormalAndDepth();
更新平均观测方向
观测方向就是该地图点到相机光心之间的向量，但是该地图点会有很多观测的向量，因此需要计算平均的观测方向：能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向，这就是观测方向，再除以观测到该地图点的关键帧数量，就是平均观测方向。
normal = normal + normali/cv::norm(normali); //将每个观测关键帧的观测单位向量累加
//最后将累加的值除以总数
mNormalVector = normal/n; //存放在mNormalVector中
更新观测距离范围
将该点与参考关键帧（第一次建立这个点的关键帧）光心之间的距离与该点所在金字塔层数相关联，得到该点如果在金字塔层的最低层和最高层时应该的距离（就是观测距离范围）
mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离上限
mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离下限
上面的计算如下图：
其实就是在计算该点如果在第0层的距离（线性关系）和该点如果在第7层的距离（线性关系）
更新帧间连接关系
pKFini->UpdateConnections();
帧间的链接关系就是按照关键帧之间的共视程度连接每个关键帧，这个共视程度就是共视点的数量，也就是权重大小
这个函数中不仅仅有更新该关键帧的连接关系，还有更新其他涉及关键帧的连接关系，总体思想就是找到点的共视帧，然后统计所有关键帧中与该关键帧的共视点个数，最后按照权重排序。更新其他设计关键帧也是差不多的流程
全局优化
Optimizer::GlobalBundleAdjustemnt(mpMap,20);
初始化之后，这时候已经得到了初步的位姿和地图点坐标，需要对这些数据进行优化，使用g2o优化库
尺度归一化
在视觉slam十四讲中，对单目视觉的尺度不确定性提出了两种方法：
- 对两张图像的 t t t 归一化，相当于固定了尺度，虽然并不知道实际长度是多长，但是我们以这时的 t t t 为单位1，计算相机运动和特征点的3D位置
- 令初始化后所有的特征点平均深度为1，也可以作为固定尺度的方法，而且这种方法可以控制场景的规模，使得计算在数值上更加稳定
在ORBSLAM2中使用了第二种方法（我怎么觉得两种都使用了，因为在矩阵模型分解那块，对 t t t 进行了归一化？？），设置一个平均深度作为相对尺度：
- 使用了
ComputeSceneMedianDepth函数计算了当前帧（由于初始化时地图点是由两帧共同计算，因此这里第一帧还是第二帧都一样）所有地图点的深度（就是地图点相机坐标系的 Z Z Z），并取这些深度的中值作为平均深度 - 对位姿变换归一化到平均深度1的尺度下（对 t t t 除以平均深度），注意：只是对 t t t 而言， R R R 只是旋转
- 对地图点坐标归一化到平均深度1的尺度下（ X , Y , Z X,Y,Z X,Y,Z 都除以平均深度）
总结
至此，单目初始化就彻底结束了，后续再进来的图像帧就是用来跟踪Tracking
扫一扫
440
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
