ORBSLAM2系列-单目初始化

本文主要是介绍ORBSLAM2系列-单目初始化，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

单目初始化是在Tracking线程中Track函数的第一步，单目初始化最重要的一点就是尺度问题，因为单目相机无法得到空间的绝对位置，例如：程序中输出的 $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){// 单目初始帧的特征点数必须大于100if(mCurrentFrame.mvKeys.size()>100){// 初始化需要两帧，分别是mInitialFrame，mCurrentFramemInitialFrame = 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:200mpInitializer =  new Initializer(mCurrentFrame,1.0,200);// 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的idfill(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.7true);      //检查特征点的方向// 对 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中，将这些点对的角度差落在直方图中，只提取直方图中最大的三个分布，其余不合群的点就删除（实际上，并没有多少点是在这一步剔除的）。如下图，就是匹配角度差不一致的点，会被删除：

在这里插入图片描述

//输入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 = 30vector<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匹配的特征点索引i2if(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--;}}}}//将最后通过筛选的匹配好的特征点保存到vbPrevMatchedfor(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
{//存储搜索结果的vectorvector<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 结果大于等于0const 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]值为 -1if(vMatches12[i]>=0){//把匹配关系存放在mvMatches12对组中mvMatches12.push_back(make_pair(i,vMatches12[i]));mvbMatched1[i]=true;}elsemvbMatched1[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-1for(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-1int 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对点进行矩阵计算，计算后会进行评比，使用评比分数最大的矩阵作为最终的矩阵模型：

ORBSLAM2系列-矩阵模型的计算

矩阵模型的分解

这时已经计算了最好的矩阵模型，需要对这个最好的矩阵模型进行分解求解R,t，但是由于分解原理会得到多个R,t的解，就需要分别使用位姿R,t计算地图点，判断地图点是否符合要求，就会得到最好的位姿R,t解：

ORBSLAM2系列-矩阵模型的分解

初始化地图

其实到这里初始化的功能基本结束了：通过两帧图像计算得到了位姿，也得到了一些地图点，后面一切的过程都是在这个基础上完成的。

而这个初始化地图部分，就是将已经计算的位姿和地图点封装，然后显示在地图中，这部分的内容主要就是一些逻辑的处理和优化。

构造关键帧

将第一帧和第二帧封装成关键帧，主要就是一些数据的初始化，比如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$ 为单位1，计算相机运动和特征点的3D位置
令初始化后所有的特征点平均深度为1，也可以作为固定尺度的方法，而且这种方法可以控制场景的规模，使得计算在数值上更加稳定

在ORBSLAM2中使用了第二种方法（我怎么觉得两种都使用了，因为在矩阵模型分解那块，对 $t$ 进行了归一化？？），设置一个平均深度作为相对尺度：

使用了ComputeSceneMedianDepth函数计算了当前帧（由于初始化时地图点是由两帧共同计算，因此这里第一帧还是第二帧都一样）所有地图点的深度（就是地图点相机坐标系的 $Z$ ），并取这些深度的中值作为平均深度
对位姿变换归一化到平均深度1的尺度下（对 $t$ 除以平均深度），注意：只是对 $t$ 而言， $R$ 只是旋转
对地图点坐标归一化到平均深度1的尺度下（ $X, Y, Z$ 都除以平均深度）