[数据结构与算法]【ORB-SLAM2源码梳理6】Track()函数的第一步：单目初始化MonocularInitialization()

前言

其实前几篇文章都是在继续为Tracking.cc的框架填坑，当然今天的也不例外，今天继续为GrabImageMonocular()中的跟踪（Track()函数）填坑，GrabImageMonocular()首次出现于System.cc中的cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)。

关于计算单应矩阵H和基础矩阵F、BA什么的后续再开一篇文章写吧，不然这篇文章自己会看不下去。

一入SLAM坑，终生SLAM人。

来，开始GrabImageMonocular()中的第三步Track()函数中的第一步单目初始化MonocularInitialization()。

一、Track()函数

位于Tracking.cc文件中，首次出现于GrabImageMonocular()函数。

void Tracking::Track()

该函数主要包括三部分：
step1：初始化
step2：跟踪
step3：记录位姿信息，用于轨迹复现

继续着重关注单目系统。

二、单目初始化MonocularInitialization()

作用：并行计算基础矩阵F和单应性矩阵H，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云得到初始两帧的匹配、相对运动、初始地图点。

流程：

1. （未创建单目初始化器）得到用于初始化的第一帧，但初始化需要两帧；
2. （已创建单目初始化器）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧；
3. 在初始化帧和当前帧中找到匹配的特征点对；
4. 如果初始化的两帧之间的匹配点太少，重新初始化；
5. 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始地图点；
6. 删除那些无法进行三角化的匹配点；
7. 将三角化得到的3D点包装成地图点。

1. 判断单目初始化器是否创建，若没有就创建。

如果后续重新初始化，这个初始化器会被删掉。

if(!mpInitializer)
    {
        // Set Reference Frame
        if(mCurrentFrame.mvKeys.size()>100)			// 判断当前帧的特征点是否大于100
        {
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            
		    // 记录当前帧（于第二帧而言，会被称为“上一帧”）所有特征点
            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;

            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);		// 构造初始化器
			
			// mvIniMatches：初始化时前两帧的匹配关系（匹配点的id），-1表示目前没有任何匹配
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            
            return;			// 如果是第一帧进来，这里已经结束了，出去后第二帧进来再执行else
        }
    }

2. 已创建初始化器，判断特征点数目

如果当前帧特征点数太少（小于100），则重新创建新的初始化器；
只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程。

3. 在初始化帧与当前帧中找匹配的特征点对（关键）

此步骤中，创建了一个特征点匹配器，匹配初始化帧和当前帧的特征点对。

// mvbPrevMatched为参考帧的特征点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标
// mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

初始化参考帧和当前帧的特征点匹配SearchForInitialization()

旋转直方图（方向梯度直方图HOG）：
核心思想：用梯度或边缘的方向密度分布，表述出图像中局部目标的表象和形状。
本质：统计梯度信息，而梯度变化通常发生于图像边缘。
实现方法：首先将图像分成若干个相连通的细胞单元（cell），随后采集细胞单元中各像素的梯度或者边缘方向分布，最后组合成
为构成特征的描述器。
相关文献：Histogram of Oriented Gradients

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

步骤：

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

预备工作：

// F1：初始化参考帧；F2：当前帧；
vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);  		// F1中特征点和F2中匹配关系，按照F1特征点数目分配空间

step1：构建旋转直方图，HISTO_LENGTH = 30（30是指30个bin，就是30个向量容器，360度划分成了30份）

    vector<int> rotHist[HISTO_LENGTH];		// 存储的是直方图的bin及其内容
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);			// 预分配空间
    
    /** 此处应该是有问题的，应为：const float factor = HISTO_LENGTH/360.0f;
    /* 原因：把长度为30bin的直方图（单个bin角度范围12°）变成了长度为12bin（单个bin角度范围30°），
    /* 这里会导致后面划分特征点的时候，会出问题。
    /* 影响：弱化了对角度不一致性错误的剔除，导致只有角度差超过30°（本来是12°）才会被去除。 
    /* 但前面rotHist 预分配了30个bin，实际第13-30个都没有用到
    /* 来源：@计算机视觉life 
    **/
    const float factor = 1.0f/HISTO_LENGTH;	

    vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);		// 匹配点对距离，按照F2特征点数目分配空间
    vector<int> vnMatches21(F2.mvKeysUn.size(),-1);					// 从帧2到帧1的反向匹配，按照F2特征点数目分配空间

	// 遍历帧1中的所有特征点，只考虑原始图像（第0层）上提取的特征点，其他的不要
    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;

step2：在窗口内搜索当前帧F2中所有的候选匹配特征点（GetFeaturesInArea()函数）

		// vbPrevMatched 输入的是初始化参考帧F1的特征点
		// windowSize = 100，输入最大最小金字塔层级均为0
		// 输入参考帧的特征点坐标，在F2中查找，GetFeaturesInArea返回的是F2中候选的匹配特征点到vIndices2容器
        vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);

        if(vIndices2.empty())
            continue;

        cv::Mat d1 = F1.mDescriptors.row(i1);		// 取出参考帧F1中当前遍历特征点对应的描述子

        int bestDist = INT_MAX;						// 最佳描述子匹配距离
        int bestDist2 = INT_MAX;					// 次优描述子匹配距离
        int bestIdx2 = -1;							// 最佳候选特征点在F2中的index

step3：遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的

step4：对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
即使得到了最佳描述子的匹配距离，也不能保证配对成功，要小于设定的阈值。

step5：计算匹配点旋转角度差所在的直方图

step6：筛除旋转直方图中“非主流”部分，为了剔除误匹配的点对（ComputeThreeMaxima()函数）

step7：将通过筛选的匹配好的特征点保存到vbPrevMatched

返回成功匹配的特征点数目。

搜索当前帧F2中的候选匹配特征点GetFeaturesInArea()

找到在以（x，y）为中心，半径为r的圆形内，金字塔层级在[minLevel, maxLevel]的特征点.

基本流程：计算圆的上下左右边界（网格的索引），然后开始查找遍历网格里是否有符合要求的匹配点，有的话再检查一次是否在圆内（有可能在外接正方形内）。

优点：相比于遍历，速度较快。

vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const

图源：@计算机视觉life

step1：计算半径为r的圆左右上下边界所在的网格列和行的id

    const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));	// 半径为r的圆的左侧边界在哪个网格列中

	// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空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;
    
    ...
    

Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里

	for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {
            const vector<size_t> vCell = mGrid[ix][iy];     // 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
            if(vCell.empty())								// 该网格中没有特征点，则跳到下一个
                continue;

            for(size_t j=0, jend=vCell.size(); j<jend; j++)	// 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
            {
                const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];		// 获取这个特征点
                if(bCheckLevels)							// 确定给定搜索的金字塔层数范围是否合法
                {
                    if(kpUn.octave<minLevel)				// 确定特征点在金字塔层级[minLevel, maxLevel]之间
                        continue;
                    if(maxLevel>=0)
                        if(kpUn.octave>maxLevel)
                            continue;
                }
				
				// 计算候选特征点到圆心的距离，确定候选特征点在圆内。
                const float distx = kpUn.pt.x-x;
                const float disty = kpUn.pt.y-y;
                if(fabs(distx)<r && fabs(disty)<r)
                    vIndices.push_back(vCell[j]);
            }
        }
    }
    return vIndices;				// 返回搜索到的候选匹配点id
}

筛选旋转角度差落在直方图区间内数量最多的前三个bin的索引ComputeThreeMaxima()

void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)

函数源码没什么特别难理解的东西，可自行查看。

图(b)是要舍弃的那部分“非主流”方向，即误匹配点。

4. 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化

5. 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始地图点（最关键）

        if(mpInitializer->Initialize(
            mCurrentFrame,      // 当前帧
            mvIniMatches,       // 当前帧F2和参考帧F1的特征点的匹配关系
            Rcw, tcw,           // 初始化得到的相机的位姿
            mvIniP3D,           // 成功三角化得到的空间点集合
            vbTriangulated))    // 对应于mvIniMatches，哪些点被三角化了

bool Initializer::Initialize(
			const Frame &CurrentFrame, 		// 当前帧F2
			const vector<int> &vMatches12, 	// 当前帧F2和参考帧F1的匹配关系
			// vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值，无匹配关系则vMatches12[i]的值为-1
			cv::Mat &R21, cv::Mat &t21,		// 从1到2的旋转和平移
            vector<cv::Point3f> &vP3D, 		// 三角化测量之后的三维地图点
            vector<bool> &vbTriangulated)	// 标记三角化点是否有效，有效为true

计算基础矩阵和单应性矩阵，选取最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点

步骤：

1. 重新记录特征点对的匹配关系（其实只是放在另一个容器里而已）
2. 在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
3. 计算fundamental 矩阵 和homography 矩阵，为了加速分别开了线程计算
4. 计算得分比例来判断选取哪个模型来求位姿R,t
   返回是否成功初始化，是则为true，否为false。

step1：重新记录特征点对的匹配关系存储在mvMatches12，是否有匹配存储在mvbMatched1

    for(size_t i=0, iend=vMatches12.size();i<iend; i++)
    {
        if(vMatches12[i]>=0)
        {
        	// mvMatches12 中只记录有匹配关系的特征点对的索引值
        	// i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
            mvMatches12.push_back(make_pair(i,vMatches12[i]));	
            mvbMatched1[i]=true;
        }
        else		// 标记参考帧1中的这个特征点没有匹配关系
            mvbMatched1[i]=false;
    }
    
    const int N = mvMatches12.size();		// 有匹配的特征点的对数
    
    // Indices for minimum set selection
    vector<size_t> vAllIndices;				// 新建一个容器vAllIndices存储特征点索引，为后面的基础矩阵F的八点法所创建的矩阵
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;		// //在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引
	// 初始化所有特征点对的索引，索引值0到N-1
    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

step2：在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵，此处默认最大迭代次数是200。

step3：计算fundamental矩阵和homography 矩阵，为了加速分别开了线程计算

    // Launch threads to compute in parallel a fundamental matrix and a homography
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;		// 用于标记在H和F的计算中哪些特征点对被认为是Inlier
    float SH, SF;					// 计算出来的单应矩阵和基础矩阵的RANSAC评分，这里其实是采用重投影误差来计算的
    cv::Mat H, F;

    // 构造线程来计算H矩阵及其得分
    // thread方法比较特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝
    thread threadH(&Initializer::FindHomography,	//该线程的主函数
				   this,							//由于主函数为类的成员函数，所以第一个参数就应该是当前对象的this指针
				   ref(vbMatchesInliersH), 			//输出，特征点对的Inlier标记
				   ref(SH), 						//输出，计算的单应矩阵的RANSAC评分
				   ref(H));							//输出，计算的单应矩阵结果
	// 构造计算F矩阵线程
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    threadH.join();
    threadF.join();

step4：计算得分比例来判断选取哪个模型来求位姿R,t

// 通过这个规则来判断谁的评分占比更多一些，不是简单的比较绝对评分，而是看评分的占比
float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    // 这里更倾向于用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;			// 一般程序不应该走到这一行

6. 初始化后，删除无法进行三角化的匹配点

7. 将初始化的第一帧作为世界坐标系

因为第一帧是世界坐标系，所以第一帧的变换矩阵T为单位矩阵。

8. 创建初始化地图点CreateInitialMapMonocular()

单目相机成功初始化后用三角化得到的点生成地图点。

步骤：

step1：将单目初始化的第一、二帧创建为关键帧

    // Create KeyFrames
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
	// ...(参数初始化列表)
{
    mnId=nNextId++;			// 获取下一个关键帧的id，并进行自增

    mGrid.resize(mnGridCols);	// 根据指定的普通帧, 初始化用于加速匹配的网格对象信息; 
    							// 其实就把每个网格中有的特征点的索引复制过来
    for(int i=0; i<mnGridCols;i++)
    {
        mGrid[i].resize(mnGridRows);
        for(int j=0; j<mnGridRows; j++)
            mGrid[i][j] = F.mGrid[i][j];
    }

    SetPose(F.mTcw);    		// 设置当前关键帧的位姿，根据各种教科书的计算方式即可理解
}

step2：将初始关键帧、当前关键帧的描述子转为BoW（bag of words），就将初始关键帧和当前关键帧转化为词袋中要用的变量

    pKFini->ComputeBoW();           
    pKFcur->ComputeBoW();

void KeyFrame::ComputeBoW()
{
    if(mBowVec.empty() || mFeatVec.empty())
    {
    	// 将描述子mDescriptors转换为DBOW要求的输入格式
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        
        // Feature vector associate features with nodes in the 4th level (from leaves up)
        // 特征向量将特征与第4层的节点相关联(从叶子开始)  
        // We assume the vocabulary tree has 6 levels, change the 4 otherwise
        //我们假设词汇表树有6级，否则更改4级
        // 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
        // transform 是第三方库
        mpORBvocabulary->transform(vCurrentDesc,	//当前的描述子vector
								   mBowVec,			//输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
								   mFeatVec,		//输出，记录node id及其对应的图像 feature对应的索引
								   4);				//4表示从叶节点向前的层数
    }
}

step3：将关键帧插入到地图

    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

void Map::AddKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexMap);		// 加互斥锁
    
    mspKeyFrames.insert(pKF);				
    
    // 更新最大id的关键帧
    if(pKF->mnId>mnMaxKFid)
        mnMaxKFid=pKF->mnId;
}

step4：用初始化得到的3D点来生成地图点
跟前面的一样，mvIniMatches[i] 表示初始化两帧特征点匹配关系，i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值；没有匹配关系的话，vMatches12[i]值为 -1。

step4.1：预备工作

    for(size_t i=0; i<mvIniMatches.size();i++)
    {
    // ...
    	cv::Mat worldPos(mvIniP3D[i]);		// 用三角化点初始化空间点的世界坐标
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);		// 用3D点构造地图点

step4.2： 为该地图点添加属性：

1. 观测到该MapPoint的关键帧
2. 该MapPoint的描述子
3. 该MapPoint的平均观测方向和深度范围

        pKFini->AddMapPoint(pMP,i);         		// first：3D地图点；second：2D特征点索引值，i为第1帧关键点的索引值
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);   // mvIniMatches[i]为第二帧关键点的索引值

        // 该MapPoint可以被哪个KeyFrame的哪个特征点观测，与上面相反
        pMP->AddObservation(pKFini,i);         
        pMP->AddObservation(pKFcur,mvIniMatches[i]);   
			
		// 从众多观测到该MapPoint的特征点中挑选最有代表性的描述子；一个地图点可以被多个关键帧的特征点观测
		pMP->ComputeDistinctiveDescriptors(); 
		
        // 更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();

        //mvIniMatches下标i表示在初始化参考帧F1中的特征点的序号
        //mvIniMatches[i]是初始化当前帧F2中的特征点的序号
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        mpMap->AddMapPoint(pMP);			// 将地图点加入地图中

计算观测到地图点的特征点中最具有代表性的描述子ComputeDistinctiveDescriptors()

由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断：是否更新当前地图点最适合的描述子。
先获得当前地图点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。

void MapPoint::ComputeDistinctiveDescriptors()

step1：跳过坏点，获取所有观测（observations）
观测：记录能观测到该地图点的关键帧特征点和该特征点关键帧中的索引

step2：遍历观测到3d点的所有关键帧，获得ORB特征点的描述子，并插入到vDescriptors中

    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        // mit->first取观测到该地图点的关键帧
        // mit->second取该地图点在关键帧中的索引
        KeyFrame* pKF = mit->first;
        if(!pKF->isBad())                                                       
            vDescriptors.push_back(pKF->mDescriptors.row(mit->second));     // 取对应的描述子向量
    }

step3：获得这些描述子两两之间的距离

   const size_t N = vDescriptors.size();		// 描述子数量
	
    // 将Distances表述成一个对称的矩阵
    // float Distances[N][N];
	std::vector<std::vector<float> > Distances;
	Distances.resize(N, vector<float>(N, 0));
	for (size_t i = 0; i<N; i++)
    {
        Distances[i][i]=0;
        for(size_t j=i+1;j<N;j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);	// 计算描述子之间的汉明距离
            Distances[i][j]=distij;
            Distances[j][i]=distij;
        }
    }

step4：选择最有代表性的描述子，它与其他描述子应该具有最小的距离中值

    for(size_t i=0;i<N;i++)
    {
        // 第i个描述子到其它所有所有描述子之间的距离
		vector<int> vDists(Distances[i].begin(), Distances[i].end());
		sort(vDists.begin(), vDists.end());     // sort默认从小到大排列

        int median = vDists[0.5*(N-1)];			// 获得中值
        
        // 寻找最小的中值
        if(median<BestMedian)
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

更新该MapPoint平均观测方向以及观测距离的范围UpdateNormalAndDepth()

由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要更新相应变量，创建新的关键帧的时候会调用。

void MapPoint::UpdateNormalAndDepth()

step1：获得该地图点的相关信息

        observations=mObservations; // 获得观测到该地图点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧（第一次创建时的关键帧）
        Pos = mWorldPos.clone();    // 地图点（三维点）在世界坐标系中的位置

step2：计算该地图点的法线方向，也就是朝向等信息

能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向。
初始值为0向量，累加为归一化向量，最后除以总数n。

        KeyFrame* pKF = mit->first;     // 关键帧
        cv::Mat Owi = pKF->GetCameraCenter();   // 关键帧（相机）光心的世界坐标系
        cv::Mat normali = mWorldPos - Owi;      // 向量指向被减数（向量的减法）
        normal = normal + normali/cv::norm(normali);     // 归一化、然后矢量相加                    
        n++;
    } 

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);                                        // 地图点到参考关键帧相机的距离
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;        // 观测到该地图点的当前帧的特征点在金字塔的第几层
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];          // 当前金字塔层对应的缩放倍数
    const int nLevels = pRefKF->mnScaleLevels;                              // 金字塔层数

    {
        unique_lock<mutex> lock3(mMutexPos);
        
		// 不同图层图像层数越高，离现实空间越近
        mfMaxDistance = dist*levelScaleFactor;                              // 观测到该点的距离上限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];    // 观测到该点的距离下限
        mNormalVector = normal/n;                                           // 获得地图点平均的观测方向
    }

step4.3：更新关键帧间的连接关系

    // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共三维点的个数
    pKFini->UpdateConnections();        
    pKFcur->UpdateConnections();

更新图上的连接关系UpdateConnections()，关键

在没有执行这个函数前，关键帧只有与地图点之间有连接关系，该函数可以更新关键帧之间的连接关系。

流程：

1. 首先获得该关键帧的所有地图点，统计观测到这些三维地图点的每个关键帧与其它所有关键帧之间的共视程度。
   即对每一个找到的共视关键帧建立一条边，边的权重是该关键帧与当前关键帧公共三维地图点的个数。
2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
   更新完共视图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树。

源码步骤：

void KeyFrame::UpdateConnections()
{
    map<KeyFrame*,int> KFcounter;		// 关键帧-权重，权重：当前关键帧与其他关键帧共视三维点的个数
    vector<MapPoint*> vpMP;	

    {
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;			// 获得当前关键帧的所有地图点
    }

step1：统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter

代码有点绕，梳理一下：

最终目标：当前关键帧与共视关键帧的共视程度，即观测到公共三维地图点的个数。

① 获得当前关键帧有可以观测到许多地图点

② 对这些地图点分别进行以下操作：获取所有能观测到这个地图点的关键帧

③ 判断这些关键帧是否为当前关键帧本身

④ 如果不是，那就KFcounter.second值加一，即这个共视关键帧与当前帧的共视程度+1

    //For all map points in keyframe check in which other keyframes are they seen
    // 对关键帧中的所有地图点，检查是否被其他关键帧所看到  
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
        if(!pMP)
            continue;
        if(pMP->isBad())
            continue;
	// 对每一个地图点，observations中记录了可以观测到该地图点的所有关键帧
        map<KeyFrame*,size_t> observations = pMP->GetObservations();		// 获取所有能观测到当前地图点的关键帧

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            if(mit->first->mnId==mnId)			// 自己跟自己当然不算共视
                continue;
            
			/**
			* map[key] = value
			* 当要插入键时，如果原来有键(key)存在，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
			* mit->first 是看到地图点的关键帧KeyFrame，同一个关键帧看到的地图点会累加到该关键帧计数value
			* 所以KFcounter第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少个当前帧的地图点，也就是共视程度
			* 这里是对KFcounter.second进行+1操作，最后的second值为和当前关键帧共视的地图点个数
			*/
			KFcounter[mit->first]++;
        }
    }

step2：找到对应权重最大的关键帧（共视程度最高的共视关键帧）

    //If the counter is greater than threshold add connection 如果计数器大于阈值，添加连接  
    //In case no keyframe counter is over threshold add the one with maximum counter 
    // 在没有关键帧计数器超过阈值的情况下，添加一个最大计数器  
    int nmax=0;
    KeyFrame* pKFmax=NULL;
    int th = 15;

    vector<pair<int,KeyFrame*> > vPairs;	// vPairs记录共视程度大于th的共视关键帧
    										// pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vPairs.reserve(KFcounter.size());

	// 遍历KFcounter
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second>nmax)
        {
            nmax=mit->second;		// 共视程度
            pKFmax=mit->first;		// 某个关键帧
        }
        if(mit->second>=th)			// 权重值大于阈值，对这些关键帧建立联系（边）
        {
            vPairs.push_back(make_pair(mit->second,mit->first));		// 记录权重值大于阈值的关键帧及其权重值
            
            // 更新KFcounter中该关键帧的权重mConnectedKeyFrameWeights
            // 更新其它关键帧的权重mConnectedKeyFrameWeights
            // 即更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);	
        }
    }

为关键帧之间添加或更新连接AddConnection()

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        // 如果被占用就一直等着,这个添加连接的操作不能够被放弃
        unique_lock<mutex> lock(mMutexConnections);

        // 判断当前关键帧是否已经和其他关键帧创建了联系
        // std::map::count函数只可能返回0或1两种情况
        if(!mConnectedKeyFrameWeights.count(pKF))       // count函数返回0，mConnectedKeyFrameWeights中没有pKF，之前没有连接
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight) // 若与之前连接的权重不一样，更新
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;
    }
    // 如果添加了更新的连接关系就要更新，主要是重新排序
    UpdateBestCovisibles();
}

更新连接关系后进行重新排序UpdateBestCovisibles()

其实这个函数就是更新共视图。

void KeyFrame::UpdateBestCovisibles()
{
    unique_lock<mutex> lock(mMutexConnections);
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());		// 关注一下权重参数

	// 取出所有连接的关键帧，mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>
    // vPairs变量将共视程度放在前面，利于排序
    for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
       vPairs.push_back(make_pair(mit->second,mit->first));

    sort(vPairs.begin(),vPairs.end());		// 按照权重进行排序（默认是从小到大）
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0, iend=vPairs.size(); i<iend;i++)		// 将顺序变成从大到小排列
    {
        lKFs.push_front(vPairs[i].second);				// 关键帧
        lWs.push_front(vPairs[i].first);				// 权重
    }

	// 按权重从大到小
    mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());    
}

ok，回到CreateInitialMapMonocular()

step5：全局BA（先不展开了）

   Optimizer::GlobalBundleAdjustemnt(mpMap,20);    

step6：取场景的中值深度，用于尺度归一化

    // Set median depth to 1
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;

	// 条件：平均深度要大于0，或者在当前帧中被观测到的地图点的数目应该大于100
    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

取场景中值深度ComputeSceneMedianDepth()

评估当前关键帧场景深度，q=2表示中值，只有在单目情况下才会使用。
过程：对当前关键帧下所有地图点的深度进行从小到大排序，
返回距离头部1/q处的深度值作为当前场景的平均深度。

源码没什么，可自行阅读

step7：将两帧之间的变换T归一化到平均深度1的尺度下

step8：把地图点的尺度也归一化到1

step9：将关键帧插入局部地图，更新归一化后的位姿、局部地图点

创建初始地图点CreateInitialMapMonocular()结束

返回MonocularInitialization()函数，单目初始化结束。

总结

啊、梳理and整理好长啊。