文章目录

前言
一、Track()函数
二、单目初始化MonocularInitialization()
- 1. 判断单目初始化器是否创建，若没有就创建。
- 2. 已创建初始化器，判断特征点数目
- 3. 在初始化帧与当前帧中找匹配的特征点对（关键）
- - 初始化参考帧和当前帧的特征点匹配SearchForInitialization()
  - 搜索当前帧F2中的候选匹配特征点GetFeaturesInArea()
  - 筛选旋转角度差落在直方图区间内数量最多的前三个bin的索引ComputeThreeMaxima()
- 4. 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
- 5. 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始地图点（最关键）
- 6. 初始化后，删除无法进行三角化的匹配点
- 7. 将初始化的第一帧作为世界坐标系
- 8. 创建初始化地图点CreateInitialMapMonocular()
- - 计算观测到地图点的特征点中最具有代表性的描述子ComputeDistinctiveDescriptors()
  - 更新该MapPoint平均观测方向以及观测距离的范围UpdateNormalAndDepth()
  - 更新图上的连接关系UpdateConnections()，关键
  - 为关键帧之间添加或更新连接AddConnection()
  - 更新连接关系后进行重新排序UpdateBestCovisibles()
  - 取场景中值深度ComputeSceneMedianDepth()
总结

前言

其实前几篇文章都是在继续为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，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云得到初始两帧的匹配、相对运动、初始地图点。

流程：

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

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

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

if(!mpInitializer){// Set Reference Frameif(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)

步骤：

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

预备工作：

// 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的圆的左侧边界在哪个网格列中// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vectorif(nMinCellX>=FRAME_GRID_COLS)return vIndices;// 计算圆所在的右边界网格列索引const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vectorif(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,      // 当前帧F2const vector<int> &vMatches12,    // 当前帧F2和参考帧F1的匹配关系// vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值，无匹配关系则vMatches12[i]的值为-1cv::Mat &R21, cv::Mat &t21,      // 从1到2的旋转和平移vector<cv::Point3f> &vP3D,       // 三角化测量之后的三维地图点vector<bool> &vbTriangulated) // 标记三角化点是否有效，有效为true

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

步骤：

重新记录特征点对的匹配关系（其实只是放在另一个容器里而已）
在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵
计算fundamental 矩阵和homography 矩阵，为了加速分别开了线程计算
计算得分比例来判断选取哪个模型来求位姿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 selectionvector<size_t> vAllIndices;              // 新建一个容器vAllIndices存储特征点索引，为后面的基础矩阵F的八点法所创建的矩阵vAllIndices.reserve(N);vector<size_t> vAvailableIndices;        // //在RANSAC的某次迭代中，还可以被抽取来作为数据样本的特征点对的索引，所以这里起的名字叫做可用的索引// 初始化所有特征点对的索引，索引值0到N-1for(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 homographyvector<bool> vbMatchesInliersH, vbMatchesInliersF;       // 用于标记在H和F的计算中哪些特征点对被认为是Inlierfloat 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 KeyFramesKeyFrame* 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,  //当前的描述子vectormBowVec,          //输出，词袋向量，记录的是单词的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：为该地图点添加属性：

观测到该MapPoint的关键帧
该MapPoint的描述子
该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()，关键

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

流程：

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

源码步骤：

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());// 遍历KFcounterfor(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;elsereturn;}// 如果添加了更新的连接关系就要更新，主要是重新排序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 1float medianDepth = pKFini->ComputeSceneMedianDepth(2);float invMedianDepth = 1.0f/medianDepth;// 条件：平均深度要大于0，或者在当前帧中被观测到的地图点的数目应该大于100if(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整理好长啊。

【ORB-SLAM2源码梳理6】Track()函数的第一步：单目初始化MonocularInitialization()相关推荐

ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
Android 源码梳理
Android 源码梳理前言作为霜枫司机一年学习的总结,附上帅照一张. 目录 1. Android系统启动过程分析 2. Linux内核文件系统 3. Android进程间通信源码梳理 4. An ...
ed2k 网络中搜索资源并选择资源下载的分析及eMule源码梳理
上一篇博客中,客户端已连接到ed2k网络及客户端与服务器交互的eMule源码梳理,这里将开始搜索资源并下载及客户端与客户端交互的eMule源码梳理 emule 源码下载地址 http://downl ...
【Android 逆向】ART 脱壳 ( InMemoryDexClassLoader 脱壳 | DexFile 构造函数及相关调用函数 | Android 源码中查找 native 函数 )
文章目录一.DexFile 构造函数二.DexFile.openInMemoryDexFile 函数三.Android 源码中查找 native 函数一.DexFile 构造函数上一篇博客 ...
c语言逃跑的蠕虫函数,源码指标：穿越5线第一时间吃鱼身上行末端逃跑没有未来函数通达信幅图精品公式...
原标题:源码指标:穿越5线第一时间吃鱼身上行末端逃跑没有未来函数通达信幅图精品公式大盘技术周日发 7月新的:石盘铁蛋! 5线穿越副图:通达信源码没有未来函数使用说明:如果个股走强的特点 ...
jQuery源码分析之$.grep()函数四问
问题1:jQuery.grep源码是什么? //grep函数,第三个参数表示是否根据fn的结果取反! grep: function( elems, callback, invert ) { var c ...
c语言实现股票软件cost函数,高仿真(主图)｛源码有成本未来函数COST｝
好股票软件下载网(www.goodgupiao.com)提示:您正在下载的是:高仿真(主图){源码有成本未来函数COST} MA1:=EMA(C,3); MA5:EMA(C,5),COLORFFFF ...
RTKlib相对定位源码解析：zdres函数
最近阅读RTKlib开源代码,非常感谢"塔奇克敲代码"博主的博客(RTKLIB源码解析--单点定位),他将单点定位部分整理成函数小卡片,为我理解RTKlib提供了很大的帮助.我参照 ...
[源码]Delphi源码免杀之函数动态调用实现免杀的下载者
[免杀]Delphi源码免杀之函数动态调用实现免杀的下载者 2013-12-30 23:44:21 来源:K8拉登哥哥's Blog 自己编译这份代码看看过N多杀软没什么技 ...

【ORB-SLAM2源码梳理6】Track()函数的第一步：单目初始化MonocularInitialization()