ORBSLAM源码理论分析2—单目初始化

1.构造初始化帧1

1．通过特征点提取得到关键点和描述子(mDescriptors)，此部分参考文章《ORBSLAM源码理论分析1—特征点提取》，然后对关键点进行矫正，得到矫正的关键点（mvKeysUn）。帧1的关键点记为mvKeys1。

2．构造地图点（mvpMapPoints）。地图点mvpMapPoints.size与关键点mvKeys1.size相同，此时地图点指针为NULL，即地图点为空。

３．计算图片畸变矫正之后的边界mnMinX、mnMaxX、mnMinY、mnMaxY。计算每个小网格的宽高的倒数mfGridElementWidthInv、mfGridElementWidthInv。这里的小网格是人为将图片划分的，一共有48 $\times$ 64个，将用于特征匹配。得到相机的内参。
　　第３步只在初始化构造帧1时执行，构造后续帧时不再有此步骤。

4．得到帧1的唯一身份识别mnId。

５．得到图像金字塔的相关参数，mnScaleLevels、mfScaleFactor、mvScaleFactors、mvLevelSigma2、mvInvLevelSigma2。

６．给48 $\times$ 64个小网格mGrid[i][j]分配特征点。特征点的像素坐标在哪个网格中，则特征点属于哪个网格。

７．构造外点(mvbOutlier)，外点mvbOutlier.size与关键点mvKeys1.size相同，此时外点都为false。即此时认为所有关键点都为内点。

2.第一次初始化

  如果帧1的关键点mvKeys1.size < 100则输入新图片，重新构造初始化帧1。如果帧1的关键点mvKeys1.size > 100则继续往下执行。

1．将帧1构造为mInitialFrame和mLastFrame，即帧1 = mInitialFrame = mLastFrame

２．帧1预备参与特征匹配的点为所有特征点，所以mvbPrevMatched.size与帧1的关键点mvKeys1.size完全相同，并且存储的内容为帧1关键点mvKeys1的像素坐标(pt)。
３．通过当前帧构造初始化器mpInitializer。设置一些参数，将服务于计算基础矩阵和单应矩阵。

3.构造初始化帧2

  与构造初始化帧1相似，除了步骤3外，其它步骤都执行。帧2的关键点记为mvKeys2，帧2就是mCurrentFrame。另外，为了陈述方便，在下文中帧1可记为F1, 帧2可记为F2。

4.F1与F2特征匹配

  如果F2的关键点mvKeys2.size $⩽$ 100则，输入新图片，重新构造初始化F1。如果F2的关键点mvKeys2.size > 100则，继续往下执行。
  F1与F2开始特征匹配，遍历F1的关键点mvKeys1，在F2中寻找图像金字塔第0层的匹配点。以F1金字塔第0层中的一个特征点kp1为例讲解具体匹配过程。
  我们拿到了特征点kp1，根据kp1的像素坐标，比如kp1像素坐标为(20,30)，找到F2中对应位置的像素点(20,30)，如下图所示，红色的像素点即为此点。
　　　　　　
　　　　　　　　　帧2搜索区域示意
　　　　　　　　　
  程序中的windowSize为100，表示以红色的像素点为中心，边长为2 $\times$ 100的虚线框区域。为什么有个比虚线框大的红色实线框呢？下面解释一下。
  因为我们搜寻kp1在F2中的候选匹配点时，最小搜寻单位是一个小网格，所以虽然根据windowSize实际计算出来的搜寻区域是虚线框，我们也要扩大搜寻区域，使搜寻区域包含整数个小网格。这个扩大的搜寻区域就是红色实线框。请记住虽然搜寻区域为红色实线框，但我们选取的候选匹配点都在虚线框内，超过虚线框的特征点将被直接丢弃。另外，候选匹配点有很多，匹配点有可能会在这里面诞生。
  在kp1的候选匹配点中，选取描述子与kp1描述子距离最小和次小的两个点，距离分别记为bestDist和bestDist2，距离最小的那个点可能是kp1在F2中的匹配点。
  对F1图像金字塔第0层中所有特征点进行如是操作，找出F2图像金字塔第0层中的可能匹配点。那么这些可能的匹配点中，哪些是匹配点，哪些是误匹配呢？下面给出判断的条件。
　　(1)最小距离bestDist < th，th为一个阈值，即最小距离小于阈值。
　　(2)bestDist < 0.9 $\times$ bestDist2，即最小距离明显小于次小距离。
　　(3)大多数可能的匹配点对的方向角差值在同一个范围内。
  满足上述三个条件的可能的匹配点对就是F1与F2的匹配点对，匹配成功的数量记为nmatches。这些匹配点对的索引存储在容器mvIniMatches中，mvIniMatches.size与F1的mvKeys1.size相同，F1与F2匹配上的点对表示为，
　　　　　　　　mvIniMatches[i1]=i2;
i1是F1中特征点在mvKeys1中的索引，i2是F2中特征点在mvKeys2中的索引。mvIniMatches[i1]的值还可以表示为，
　　　　　　　　mvIniMatches[i1]=-1;
  可以明显的看出-1表示F1中的i1特征点在F2中没有匹配点。如果匹配的特征点对数量nmatches < 100，则输入新图片，重新构造初始化帧1。

5.初始化解算位姿

  在开始之前先介绍一个重要的变量mvMatches12，mvMatches12记录了上述匹配成功的特征点对在各自帧中的索引组成的点对 (i1,i2)，比如，
　　　　　　　　　　　　mvMatches12.push_back(make_pair(i1,i2);
   i1是F1中特征点在mvKeys1中的索引，i2是F2中特征点在mvKeys2中的索引。mvMatches12的size与nmatches相同。

5.1.计算单应矩阵

   假设F1与F2的特征点所对应的空间点都在一个平面内，所以计算单应矩阵。下面开始讲解具体计算思路。
  首先，将F1的所有特征点都归一化，注意这里的归一化不是简单的让特征点的像素坐标模长归一，而是让这些特征点的中心矩归一，对F2的特征点进行同样操作。在得到归一化特征点vPn1i与vPn2i的同时，还应该得到归一化矩阵T1、T2。
  利用随机抽样一致性算法(RANSAC)选取8对归一化匹配点对，并且参考高翔博士的经典SLAM书籍《视觉SLAM十四讲》P146-P147的理论和公式，求出归一化尺度下的单应矩阵Hn。然后将Hn恢复到原尺度，得到候选单应矩阵H21i。程序中利用RANSAC选取了200次8对归一化随机匹配点对，最终得到了200个候选单应矩阵，哪个是最优的单应矩阵呢?下面讲解最优判断法则。
  将F2匹配成功的特征点通过单应矩阵投影到F1，计算F1中匹配的特征点像素坐标 $(u1,v1)$ 与投影得到的像素坐标 $(u2,v2)$ 之间的误差squareDist1++，同理将F1匹配成功的特征点通过单应矩阵投影到F2，计算出误差squareDist2++，根据这两个误差，得到评判单应矩阵优劣的score。在200个候选单应矩阵中选择score值最大的候选单应矩阵H21i做为单应矩阵H。并记录此矩阵的内点标志位vbMatchesInliersH和分数SH=score。
  内点标志位vbMatchesInliersH.size与nmatches相同，投影检验误差小的点对，即合格的点对为内点，记
　　　　　　　　　　　vbMatchesInliersH [i]=true
投影检验误差大的点对，即不合格的点对不是内点，记
　　　　　　　　　　　vbMatchesInliersH [i]=false
把得到的内点数量记为Inliers。

5.2.计算基础矩阵

   假设F1与F2的特征点所对应的空间点不在一个平面内，所以计算基础矩阵。下面开始讲解具体计算思路。
   首先，将F1的所有特征点都归一化，注意这里的归一化不是简单的让特征点的像素坐标模长归一，而是让这些特征点的中心矩归一，对F2的特征点进行同样操作。在得到归一化特征点vPn1i与vPn2i的同时，还应该得到归一化矩阵T1、T2。
   利用随机抽样一致性算法(RANSAC)选取8对归一化匹配点对，并且参考高翔博士的经典SLAM书籍《视觉SLAM十四讲》P142-P146的理论和公式，求出归一化尺度下的基础矩阵Fn。然后将Fn恢复到原尺度，得到候选基础矩阵F21i。程序中利用RANSAC选取了200次8对归一化随机匹配点对，最终得到了200个候选基础矩阵，哪个是最优的基础矩阵呢?下面讲解最优判断法则。
   将F1匹配成功的特征点通过基础矩阵投影到F2，计算F2中匹配的特征点像素坐标 $(u1,v1)$ 与投影得到的像素坐标 $(u2,v2)$ 之间的误差squareDist1++，同理将F2匹配成功的特征点通过基础矩阵投影到F1，计算出误差squareDist2++，根据这两个误差，得到评判基础矩阵优劣的score。在200个候选基础矩阵中选择score值最大的候选基础矩阵F21i做为基础矩阵F。并记录此矩阵的内点标志位vbMatchesInliersF和分数SF=score。
   内点标志位vbMatchesInliersF.size与nmatches相同，投影检验误差小的点对，即合格的点对为内点，记
　　　　　　　　　　　　vbMatchesInliersF[i]=true
投影检验误差大的点对，即不合格的点对不是内点，记
　　　　　　　　　　　　vbMatchesInliersF[i]=false
把得到的内点数量记为Inliers。

5.3.评估特征点场景模型

   根据单应矩阵H的分数SH和基础矩阵F的分数SF,计算如下公式，
　　　　　　　　　　　　RH = SH/(SH+SF);
如果RH > 0.40，则选取单应矩阵模型，否则选取基础矩阵模型。

5.3.1.单应矩阵模型(RH > 0.40)

  由单应矩阵解算旋转量Rcw和平移量tcw，请参考吴博的ppt《ORB-SLAM2源码详解》，单应矩阵会解算出8组旋转量vR[i]和平移量vt[i](0$⩽$i<8)，如何从8组结果中选取最优的解算结果呢？以其中一组解为例，进行分析。
  因为F1的相机坐标系与世界坐标系重合，所以可以直接求得投影矩阵P1,根据旋转量vR[1]和平移量vt[1]，求得F2的投影矩阵P2。利用投影矩阵P1和P2，并且通过奇异值分解恢复出所有内点Inliers的空间点p3dC1，这部分的理论知识可以参考吴博的ppt《ORB-SLAM2源码详解》。接下来要从这些3D点中挑选出合格的空间点，合格的空间点要满足以下条件，
　　(1).3D点在F1的前面。
　　(2).3D点在F2的前面。
　　(3).3D点在F1上的投影误差小于阈值。
　　(4).3D点在F2上的投影误差小于阈值。
　　满足这些条件的3D点数量记为nGood。8组解算结果就对应8个nGood，将最大的和次大的分别记为bestGood，secondBestGood。bestGood所对应的旋转量vR[i]和平移量vt[i]有可能是最优解，此解对应的视差角为bestParallax。那么，此解什么情况下是最优解？什么情况下不是呢？
　　(1).secondBestGood < 0.75$\times$bestGood，即最大3D点数量明显多于次大3D点数量。
　　(2).bestParallax > 1.0°，即视差角要大于1°。
　　(3).bestGood > 50，即最大3D点数量应多于50。
　　(4).bestGood > 0.9$\times$Inliers，即最大3D点数量应多于0.9倍内点数量
　　如果满足上述条件，则初始化成功，得到最优旋转量Rcw和平移量tcw的同时，还得到了初始化3D点集合mvIniP3D和vbTriangulated。
　　如果不满足上述条件，则初始化失败，输入新图片，重新构造初始化帧2。
　　mvIniP3D记录了3D点在世界坐标系下的坐标(X,Y,Z)，mvIniP3D.size与F1关键点mvKeys1.size相同，3D点的索引与其所对应的关键点在mvKeys1中的索引相同。vbTriangulated表示了三角化成功与否的状态。三角化成功
　　　　　　　　　　　　vbTriangulated=true
三角化失败，
　　　　　　　　　　　　vbTriangulated=false
此时，3D点数量记为bestGood。

5.3.2.基础矩阵模型(RH $⩽$ 0.40)

  由基础矩阵解算旋转量Rcw和平移量tcw，请参考《视觉SLAM十四讲》145P 公式(7.14)。基础矩阵会解算出4组旋转量vR[i]和平移量vt[i](0$⩽$i<4)，如何从4个结果中选取最优的解算结果呢？以其中一组解为例，进行分析。
  因为F1的相机坐标系与世界坐标系重合，所以可以直接求得投影矩阵P1,根据旋转量vR[1]和平移量vt[1]，求得F2的投影矩阵P2。利用投影矩阵P1和P2，并且通过奇异值分解恢复出所有内点Inliers的空间点p3dC1，这部分的理论知识可以参考吴博的ppt《ORB-SLAM2源码详解》。接下来要从这些3D点中挑选出合格的空间点，合格的空间点要满足以下条件，
　　(1).3D点在F1的前面。
　　(2).3D点在F2的前面。
　　(3).3D点在F1上的投影误差小于阈值。
　　(4).3D点在F2上的投影误差小于阈值。
　　满足这些条件的3D点数量记为nGood1，视差角为parallax1。另外3组解算结果分别对应nGood2、nGood3、nGood4，视差角为parallax2、parallax3、parallax4。将3D点数量最大的记为bestGood。bestGood所对应的旋转量vR[i]和平移量vt[i]有可能是最优解。那么，此解什么情况下是最优解？什么情况下不是呢？
　　(1).设nGood1最大，则其它3个nGood2、nGood3、nGood4要满足
　　　　nGood2 < 0.7$\times$bestGood且nGood3 < 0.7$\times$bestGood且nGood4 < 0.7$\times$bestGood
　　　　即最优解的3D点数量要明显多于另外3组解的3D点数量。
　　(2)假设nGood1最大，则parallax1 > 1.0°，即视差角要大于1°。
　　(3)bestGood > 50，即最大3D点数量应多于50。
　　(4)bestGood > 0.9$\times$Inliers，即最大3D点数量应多于0.9倍内点数量。
　　如果满足上述条件，则初始化成功，得到最优旋转量Rcw和平移量tcw的同时，还得到了初始化3D点集合mvIniP3D和vbTriangulated。
　　如果不满足上述条件，则初始化失败，输入新图片，重新构造初始化帧2。
　　mvIniP3D记录了3D点在世界坐标系下的坐标（X,Y,Z），mvIniP3D.size与F1关键点mvKeys1.size相同，3D点的索引与其所对应的关键点在mvKeys1中的索引相同。
　　vbTriangulated表示了三角化成功与否的状态。三角化成功
　　　　　　　　　　vbTriangulated=true
三角化失败，
　　　　　　　　　　vbTriangulated=false
此时，3D点数量记为bestGood。
　　mvIniMatches存储着匹配点对的索引，匹配成功的数量为nmatches，在得到3D空间点后，将那些没有三角化成功的匹配点对注销匹配。这时，匹配成功的点对数量变为bestGood。

6.创建初始化地图

6.1.位姿传递

  上面得到的最优旋转量Rcw和平移量tcw即为F2的位姿信息。F1的旋转量为单位阵，平移量为0。

6.2.关键帧的创建与词袋模型说明

  将F1与F2分别构造为关键帧pKF1和pKF2。并且把pKF1与pKF2的描述子转换为词袋向量mBowVec和节点与索引特征向量mFeatVec。以pKF1的一个描述子desc为例，分析具体转换过程。
　　先简单介绍一下词袋模型，词袋模型是根据训练好的txt文本文件生成的树形存储结构，可将其看作一本词典。设这棵树有6层（不包括根节点），每层的每个节点最多有10个子节点，则这棵树的节点m_nodes最多有1111111个，叶子节点（单词）m_words最多有1000000个。节点m_nodes有可能是叶子节点m_words，那么这时，节点m_nodes除了本身的节点索引外，还应有作为叶子节点的索引。
　　遍历词典节点m_nodes，得到词典第二层所有节点的描述子，在这些描述子中选出与desc描述子最为相似的那个描述子，此描述子所对应的节点的索引为nid，mFeatVec记录了该节点的索引nid和desc的索引。
　　继续遍历nid的孩子节点，得到nid节点后代的所有叶子节点的描述子，在这些描述子中选出与desc描述子最为相似的那个描述子，此描述子所对应的节点的索引记为mid，节点权重记为weight，此节点作为叶子节点的索引记为word_id。mBowVec记录了叶子节点的索引word_id和节点权重weight。
　　对pKF1与pKF2的描述子分别进行如是操作，得到各自的词袋向量mBowVec和节点与索引特征向量mFeatVec。

6.3.关键帧和地图点插入到mpMap

  将pKF1和pKF2插入到地图mpMap，存储在set容器mspKeyFrames中。
　　将bestGood个3D空间点构造为地图点MapPoint，地图点的参考关键帧都设为关键帧pKF2。以其中一个地图点pMP为例，讲解此过程。
　　(1)将pMP添加到pKF1，pMP将被存储在pKF1的mvpMapPoints中，例如，
　　　　　　　　　　　　mvpMapPoints[idx]=pMP;
　　idx为地图点pMP对应的关键点在mvKeys1中的索引。表示pKF1的idx关键点可以观测到pMP。mvpMapPoints.size与关键点mvKeys1.size相同。将pMP添加到pKF2，操作相同。
　　(2)将pKF1添加到pMP，pKF1将被存储在pMP的mObservations中，例如，
　　　　　　　　　　　　mObservations[pKF1]=idx;
　　pKF1表示关键帧1，idx为地图点pMP对应的关键点在mvKeys1中的索引。表示地图点pMP可以被pKF1的idx关键点观测到。将pKF2添加到pMP，操作相同。
　　(3)地图点pMP也有描述子，如何确定pMP的描述子呢？由于pMP会被许多关键帧观测到，因此在插入关键帧后，需要判断是否更新当前点的描述子。先获得pMP对应所有特征点的描述子，然后计算描述子之间的两两距离，代表pMP的描述子与其他描述子的距离中值应该最小。
　　(4)获得地图点pMP的平均观测方向mNormalVector，最小观测距离mfMinDistance和最大观测距离mfMaxDistance。pMP指向观测到该点的所有帧的相机中心，所得的归一化向量的平均值为mNormalVector。根据pMP对应参考关键帧特征点的层数level，尺度因数scaleFactor，本层尺度因数levelScaleFactor和pMP距当前参考帧的距离dist，求出mfMinDistance和mfMaxDistance。
　　(5)将地图点pMP添加到F2的mvpMapPoints。例如，
　　　　　　　　　　　　　mvpMapPoints[i2] = pMP
　　i2为地图点pMP对应关键点在F2的mvKeys2中的索引。
　　(6)将地图点pMP添加到地图mpMap，存储在set容器mspMapPoints中。
对每一个3D空间点进行上述操作，包装成地图点。

6.4.更新关键帧与关键帧的连接关系

  为了能更好的解释清楚此过程，我们假设除了关键帧pKF1与pKF2外，还有2个关键帧pKF3、pKF4。以更新关键帧pKF1为例，讲解此过程。
　　(1)更新关键帧pKF1的mConnectedKeyFrameWeights，
　　　　　　　mConnectedKeyFrameWeights[pKF2]= weight12
　　　　　　　mConnectedKeyFrameWeights[pKF3]= weight13
　　　　　　　mConnectedKeyFrameWeights[pKF4]= weight14
表示关键帧pKF1与关键帧pKF2，pKF3，pKF4共同观测到的3D点数量，也就是权重，分别为weight12, weight13, weight14。
　　(2)更新关键帧pKF1的排序好的连接关键帧mvpOrderedConnectedKeyFrames和排序好的对应共视3D点数量mvOrderedWeights。这里的顺序是按着共视3D点数量从大到小排列的。取出容器mvpOrderedConnectedKeyFrames的第一个量，作为关键帧pKF1的mpParent。而pKF1为mpParent的孩子。
　　(3)更新关键帧pKF2、pKF3、pKF4的mConnectedKeyFrameWeights，
　　　　　　　　pKF2->mConnectedKeyFrameWeights[pKF1]= weight21
　　　　　　　　pKF3->mConnectedKeyFrameWeights[pKF1]= weight31
　　　　　　　　pKF4->mConnectedKeyFrameWeights[pKF1]= weight41
表示关键帧pKF2，pKF3，pKF4与关键帧pKF1共同观测到的3D点数量（权重）分别为weight21（=weight12）, weight31（=weight13）, weight41（=weight14）。
　　(4)分别更新关键帧pKF2，pKF3，pKF4的mvpOrderedConnectedKeyFrames和mvOrderedWeights。这里的顺序是按着共视3D点数量从大到小排列的。
　　(1)、(2)、(3)和(4)中的连接关系，并不是无条件进行添加的，要根据以下原则更新，
　　①．对于(1)而言，只要pKF1与pKF2，pKF3，pKF4有共视3D点即可。
　　②．对于(2)、(3)和(4)而言，上述三个权重小于15的关键帧不会与pKF1添加连接。
　　③．对于(2)、(3)和(4)而言，如果这三个权重都小于15，则只添加权重最大的关键帧与pKF1的连接。比如，15 > weight21 > weight31 > weight41，则只添加pKF2与pKF1的连接。
　　对pKF2同样进行上述操作，更新连接。

6.5.更新位姿和地图点位置

  3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw)，迭代20次。得到优化结果，优化所有关键帧pKF1与pKF2的位姿，优化地图点的空间位置。

6.6.统一尺度

  得到pKF1的场景深度medianDepth，pKF1的所有地图点mvpMapPoints在相机坐标系下的Z坐标的中值即为medianDepth。将medianDepth作为尺度1，统一地图点和平移量的尺度。关于尺度统一，可以参考《视觉SLAM十四讲》152P的内容。

6.7.重要变量进行交接

(1)在局部地图mpLocalMapper中插入关键帧pKF1和关键帧pKF2，详情请参考建图线程《Local Mapping线程》。
(2)关键帧pKF2位姿传递给F2(mCurrentFrame)。
(3)将F2(mCurrentFrame)设为上一帧mLastFrame。
(4)把F2(mCurrentFrame)的mnId传递给mnLastKeyFrameId
(5)将关键帧pKF2设为上一关键帧mpLastKeyFrame。
(6)把关键帧pKF1与关键帧pKF2插入局部关键帧mvpLocalKeyFrames中。
(7)将地图mpMap的所有地图点插入到局部地图点mvpLocalMapPoints。
(8)将关键帧pKF2设为参考关键帧mpReferenceKF。
(9)将mvpLocalMapPoints设为地图mpMap的参考地图点mvpReferenceMapPoints。
(10)将地图发布者mpMapPublisher的相机位姿mCameraPose设为关键帧pKF2的位姿。

附
　　　　关于一些变量size和专有点的数量说明
　　　　⑴匹配成功的数量nmatches < mvKeys1.size && nmatches < mvKeys2.size。
　　　　⑵内点数量Inliers < nmatches
　　　　⑶3D点数量bestGood < 内点数量Inliers
　　　　⑷mvKeys1.size = mvIniMatches.size = mvIniP3D.size
　　　　⑸vbMatchesInliersH.size = vbMatchesInliersF.size = nmatches