(01)ORB-SLAM2源码无死角解析-(24) 单目SFM地图初始化→CreateInitialMapMonocular()-细节分析:尺度不确定性

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言对

Tracking::MonocularInitialization():单目初始化需要连续的两帧，这两帧提取的特征点都要>100，且匹配的点数要>100，否则就接着读取下一帧。然后调用Initialize::Initialize()，判断能否初始化以及计算R,t（创建两个线程并行计算H和F矩阵，根据得分判断用哪个模型，然后用该模型进行三角化，验证模型是否合适）。上篇博客对 src/Tracking.cc 文件中 Tracking::CreateInitialMapMonocular() 函数进行了总体介绍，该片博客主要对该函数进行细节分析，也就是分析其中被该函数调用的函数。这篇文章中，涉及到了一个比较重要的问题，那就是单目追踪的尺度不确定性，在前面的博客：
(01)ORB-SLAM2源码无死角解析-(14) 地图初始化→单目初始化MonocularInitialization():尺度不确定性
提到了尺度不确定性的概念，知道了单目追踪的尺度具有不确定性，是相对的。那么我们如何却设定这个相对值呢？
Tracking::CreateInitialMapMonocular()创建初始关键帧pKFini和当前关键帧pKFcur，创建地图点并进行局部BA。然后就可以认为初始化结果是正确的，这个时候，尺度怎样确定呢？根据高博的SLAM14讲第152页的内容，单目尺度不确定性可以通过对初始两帧的平移向量t归一化作为后续的单位；也可以通过令初始化时所有特征点的平均深度为1固定一个尺度，这种方式可以控制场景规模大小，使计算在数值上更稳定。那么ORB-SLAM2单目初始化使用的是第二种固定尺度的方法(与书上略有不同)，$核心要点参考下面的第八项$。

 

二、MapPoint* pMP = new MapPoint(worldPos,pKFcur, mpMap):

该为地图点的初始化函数，从代码中可以看到，该函数位于初始化帧(第一帧)特征点(匹配成功)循环中，也就是初始帧中每个匹配的特征点，都是为其生成一个地图点。每个地图点，主要包含如下成员：

		mWorldPos(Pos) //3D点，或者说世界坐标
		mpRefKF(pRefKF) // 生成该地图点的关键帧
		mpMap(pMap) //地图点所属地图 
		mnFirstKFid(pRefKF->mnId), //第一次观测/生成它的关键帧 id
		mnFirstFrame(pRefKF->mnFrameId), //创建该地图点的帧ID(因为关键帧也是帧啊)
		nObs(0)// 被观测次数
		mnVisible(1), //在帧中的可视次数
		mfMinDistance(0), //当前地图点在某帧下,可信赖的被找到时其到关键帧光心距离的下界
		mfMaxDistance(0), //上界

从代码中可以看到，地图点初始化时，主要的参数为 worldPos,pKFcur, mpMap。地图点创建完成之后，首先把地图点添加到初始化帧关键帧 pKFini 以及当前帧关键帧 pKFcur 之中。另外该地图点，可以被那些关键帧观测到，也需要记录下来：

        // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

AddObservation 的第一个参数为关键帧，第二个参数为该地图点在该帧中对应的第几个特征点。比如 pMP->AddObservation(pKFini,i); 就表示，该地图点，对应与 pKFini 关键帧中第 i 个特征点。在记录的同时，成员变量 nObs 会进行累计。主要是统计一个地图点，能够被多少关键帧观测到，

 

三、pMP->ComputeDistinctiveDescriptors()

该函数主要的作用是从众多观测到该地图点的特征代点中挑选具有最有代表性的描述子。因为一个地图点会被许多相机观测到，因此在插入关键帧后，需要判断是否更新代表当前点的描述子，先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。通过前面的介绍，描述子是256个bit位。在该函数中，可以看到这样一句代码：

    observations=mObservations; //观测到该MapPoint的KF和该MapPoint在KF中的索引

通过该变量，能够获得观测到该地图点，所有关键帧与之对应的特征点。接下来会进行一个循环，循环的主体代码如下：

	vDescriptors.push_back(pKF->mDescriptors.row(mit->second)); 

其目的是获得所有特征点的描述子，添加到 vDescriptors 变量之中。输出一个距离矩阵 Distances，计算距离矩阵的主体代码如下：

    int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
    Distances[i][j]=distij;
    Distances[j][i]=distij;

距离矩阵 Distances 保存描述子描述子之间两两距离。接着依据该变量，循环排序每个描述子到其他描述子的距离，即可轻易获得中间距离。也就是获得了每个描述子到其他的描述子的中间距离，然后还需要从众多的中间距离中，找到最小的中间距离，并且把该描述子在 vDescriptors 变量中的索引记录下来，也就是变量 BestIdx。最终根据索引把对应的描述子返回即可。
 

四、pMP->UpdateNormalAndDepth()

该函数中的主要功能是更新地图点的平均观测方向、观测距离范围。首先是获得观测到该地图点的所有关键帧、参考帧，坐标信息，代码如下：

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

世界坐标系中的一个3D点，会被多帧图像观测到，该点被每帧图像观测到方向是不一样的。这里的观测方向是是一个单位向量。计算的方式是: 3D点减去拍摄该帧相机中心的世界坐标，获得该向量之后再进行单位化，其核心代码如下：

	for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
     	cv::Mat normali = mWorldPos - Owi;
     	normal = normal + normali/cv::norm(normali);   
     ......
     ......
     mNormalVector = normal/n; // 获得地图点平均的观测方向

代码可以看到单位化之后的观测向量做累加的操作，然后再获取平均值，其上的 mNormalVector 就是该地图点在当前帧的平均观测方向。

另外呢，代码中还做了一些其他的操作，比如根据图像金字塔计算出观测到该点的距离上限与下限。首先获得拍摄当前帧时相机到3D点的距离：

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();                           // 参考关键帧相机指向地图点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);                                        // 该点到参考关键帧相机的距离

在获取图像ORB特征代点时，对图像进行了金字塔处理，也就是对图像进行了缩放。那么相当于拉近或者拉远相机到3D点的距离。另外一般来说，远距离能观测到的特征点，近距离也应该能观测到。这样也就有了观测到该点的距离上限，以及观测到该点的距离下限。不过暂时大家不必太过于关注。目前来看，似乎没有使用到这个变量。
 

五、pKFini 或 pKFcur->UpdateConnections()

根据3D点为所有关键帧与当前帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数(也称为两帧之间的共视成都)。首先我们获得当前帧的地图点(该帧的地图点,都能被当前帧所见)。代码如下：

	vpMP = mvpMapPoints;

获得当前帧的所有地图点之后，则进入一个循环，统计该地图点能够能够被多少关键帧观测到，与当前帧形成共视关系，统计的数量存放于 KFcounter 变量中。主要流程如下：

 for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
	MapPoint* pMP = *vit; //目前进行统计的地图点
    // 对于每一个地图点，observations记录了可以观测到该地图点的所有关键帧
    map<KeyFrame*,size_t> observations = pMP->GetObservations();
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    	KFcounter[mit->first]++;

其代码的思路也是比较简单的，主要注意 KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度。

获得当前帧与所有帧的共视程度之后，接下来会记录最高共视程度，同时如果共视程度超过了一定阈值(如默认th=15)，则建立连接关系。主要的核心变量是:

vector<pair<int,KeyFrame*> > vPairs; //将关键帧的权重写在前面，关键帧写在后面方便后面排序

如果当前帧与所有关键帧的共视程度都没有达到指定阈值，则与共视程度最高(权重最大)的关键帧建立连接。也就是只需要更新他与最高共视程度关键帧的权重即可。获得当前帧与所有关键帧的连接关系以及权重之后，然后组织成两种数据类型(已经按权重进行排序):

   // push_front 后变成了从大到小顺序
   lKFs.push_front(vPairs[i].second);  //关键帧，
   lWs.push_front(vPairs[i].first); //当前帧与关键帧对应的权重

最后，还要更新生成树的连接。其操作比较简单，就是把共视程度最高的关键帧作为当前帧的父节点。并且建立双向连接，也就是还要为共视程度最高的关键帧添加一个子节点，当前帧则为该子节点。
 

六、Optimizer::GlobalBundleAdjustemnt(mpMap,20);

该函数后面的博客进行详细的讲解。

七、mpReferenceKF->TrackedMapPoints(nMinObs);

如果刚刚完成初始化，这参考帧为当前帧，否则在 UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 。这里认为为与当前帧共视程度最高的关键帧为参考帧。

在当前帧中存在N个特征点，但是并不是每个特征点都能够三角化，生成一个地图点。所以这里对当前帧的地图点做了一个统计，如果当前帧的一个地图点，被 minObs 以上关键帧(相机) 观测到，认为该关键点被追踪到。该函数返回的局部变量 nRefMatches 则表示当前帧被追踪到的地图点总数。

 

八、pKFini->ComputeSceneMedianDepth(2)

该函数过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值作为当前场景的平均深度。其实现比较简单，这里就不做详细的介绍了。计算出深度之后，该函数会返回平均深度，返回之后呢，利用结果做如下操作：

	//将两帧之间的变换归一化到平均深度1的尺度下
    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1 
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

其主要将两帧之间的变换归一化到平均深度1的尺度下。另外还需要把3D点的尺度也归一化到1，主要代码如下：

     MapPoint* pMP = vpAllMapPoints[iMP];
     pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);

从这里我们可以出来，尺度固定是对于平移向量来说的，即一个形象表达式为 $s\ast \mathbf{t}$($s$是尺度变换因子，即代码段中的invMedianDepth)。和旋转矩阵R没有关系(常识也是这样，沿着方向，走的距离不同(1m还是1cm)，造成的轨迹尺度也不一样，但是形状是一样的）。

 

九、赋值操作

前面的大部分流程，都属于局部变量的计算，我们需要对其进行赋值， 赋值到 Tracking 类对象成员变量中。核心代码如下：

    //  Step 8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);

    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId=mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;

    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    // 单目初始化之后，得到的初始地图中的所有点都是局部地图点
    mvpLocalMapPoints=mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    //也只能这样子设置了,毕竟是最近的关键帧
    mCurrentFrame.mpReferenceKF = pKFcur;

    mLastFrame = Frame(mCurrentFrame);

    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());

    mpMap->mvpKeyFrameOrigins.push_back(pKFini);

 

十、结语

通过两篇博客的讲解，我们已经理清楚了 CreateInitialMapMonocular() 函数，现在我们回到之前的函数 src/Tracking.cc 文件中的 void Tracking::MonocularInitialization() 函数。可以很明显的看到，到这里为止，该函数也讲解完成了。也就是单目初始化过程已经完结了。接下来，我们需要讲解其他的内容了。

 
 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件