4.1ORB-SLAM3之处理缓存队列中的关键帧

0.简介

该函数主要包括以下几个部分：

• 计算该关键帧特征点的Bow信息
• 更新当前关键帧新增地图点的属性
• 更新共视图中关键帧间的连接关系
• 将该关键帧插入到地图中

1.计算该关键帧特征点的Bow信息ComputeBoW()

vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
mpORBvocabulary->transform(vCurrentDesc, mBowVec, mFeatVec, 4);

将当前关键帧的所有描述子vCurrentDesc转换为词袋向量，主要用于加速特征匹配(跟踪参考关键帧)和计算两帧之间的相似性(重定位和回环检测)。前面已经讲过，在ORB特征检测后得到的描述子是一个N行的cv::Mat，N表示特征点的数量。为了方便将描述子进行词袋向量转换会对描述子进行一些转换，即将每个特征点对应的描述子拿出来放到vector容器中，得到一个cv::Mat类型的N维vector,其每个元素都表示一个特征点的描述子。

计算描述子函数的输出有两个：
mBowVec记录了由描述子转换得到的词袋向量信息以及每个单词的权重，这个主要用于计算两帧之间的图像相似性。

mFeatVec记录了每个特征点的索引以及该特征点对应单词在词袋树上的节点id，根据这一信息可以在寻找特征匹配时只对统一节点上的特征点寻找匹配关系，以实现加速匹配。

2.更新当前关键帧新增地图点的属性

参考链接：《ORB-SLAM2代码详解03: 地图点MapPoint》

1. 为当前地图点添加观测
2. 更新平均观测方向和观测距离范围
3. 更新地图点的最佳描述子

if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)){
   pMP->AddObservation(mpCurrentKeyFrame, i);
   pMP->UpdateNormalAndDepth();
   pMP->ComputeDistinctiveDescriptors();
}

2.1为当前地图点添加观测AddObservation

表示该地图点被当前关键帧观测到了，函数AddObservation()和EraseObservation()同时维护两个成员变量mObservations和nObs以更新地图点的观测情况。

std::map<KeyFrame*,size_t> mObservations是一个map类型的容器，键KeyFrame*表示观测到该Mappoint的关键帧，值size_t表示该MapPoint在KF中的索引。通过函数GetIndexInKeyFrame()和IsInKeyFrame()可以对mObservations进行简单查询。

nObs为int类型变量，记录了当前地图点被多少个关键帧相机观测到了(单目关键帧每次观测算1个相机,双目/RGBD帧每次观测算2个相机).

2.2更新平均观测方向和观测距离范围UpdateNormalAndDepth

函数UpdateNormalAndDepth()更新当前地图点的平均观测方向和距离.

1.观测方向：
其中平均观测方向是根据mObservations中所有观测到本地图点的关键帧取平均得到的，通过地图点与关键帧对应相机光心的连线可以得到该地图点的观测方向mWorldPos - pKF->GetCameraCenter()，然后计算该地图点在所有关键帧中的观测方向，并取平均值。

// UpdateNormalAndDepth()
for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
   KeyFrame* pKF = mit->first;

   tuple<int,int> indexes = mit -> second;
   int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

   if(leftIndex != -1){
      Eigen::Vector3f Owi = pKF->GetCameraCenter();// 获得光心相机坐标
      Eigen::Vector3f normali = Pos - Owi;// 计算到光心的向量也即观测方向
      normal = normal + normali / normali.norm();// 对每个方向向量进行归一化然后相加，最终得到综合方向向量
      n++;
   }
   if(rightIndex != -1){// 针对双单目组合模式
      Eigen::Vector3f Owi = pKF->GetRightCameraCenter();
      Eigen::Vector3f normali = Pos - Owi;
      normal = normal + normali / normali.norm();
      n++;
   }
}

(2)观测距离：
平均观测距离是根据参考关键帧得到的，观测距离指的是该3D点到参考关键帧光心的距离。

Eigen::Vector3f PC = Pos - pRefKF->GetCameraCenter();
const float dist = PC.norm();

...

{
   unique_lock<mutex> lock3(mMutexPos);
   mfMaxDistance = dist*levelScaleFactor;// 计算距离上限
   mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];// 计算距离下限
   mNormalVector = normal/n;// 计算平均观测距离
}

除了更新观测距离外还会更新该3D点观测距离的上下限:mfMinDistance和mfMaxDistance,用于判断某个特征点在反投影后是否真正在视野范围内，其计算方式金字塔图像的尺度有关。

如下图，mfMaxDistance表示若地图点匹配在某特征提取器图像金字塔第7层上的某特征点,观测距离值;mfMinDistance表示若地图点匹配在某特征提取器图像金字塔第0层上的某特征点,观测距离值;

反过来，也可以根据平均观测距离和该特征点的观测距离上限推测特征点所在的金字塔图像的level.

计算公式如下：

2.3更新地图点的最佳描述子ComputeDistinctiveDescriptors

对于某个3D地图点会在多个时刻下被相机观测到，因此同一3D地图点会被多个关键帧观测到，也即该3D点是多个特征点的反投影点，该3D点的描述子也由与其对应特征点的描述子决定。假设该3D点存在N个对应的特征点，计算方式如下：

• 统计该3D点所有对应特征点的描述子
• 计算这些描述子两两之间的距离，则可以得到一个关于描述子距离NxN的矩阵
• 每个描述子对应N个与其他描述子的距离，对于每个描述子取出其与其他描述子距离的中位数
• 对N个中位数进行排序，选择中位数最小时对应的描述子作为3D点的描述子

3.3 更新共视图中关键帧间的连接关系

void KeyFrame::UpdateConnections(bool upParent)

在没有执行这个函数前，关键帧只和MapPoints之间有连接关系(将观测信息进行关联)，这个函数可以更新关键帧之间的连接关系。

1. 首先获得该关键帧的所有MapPoint点，统计有多少关键帧与当前关键帧存在共视关系(是否观测到了同一个3D点)，统计结果放在KFcounter(可以通过地图点的observations属性获得)。
2. 只要共视点数量大于阈值th=15,就为关键帧与当前帧互相添加连接关系，边的权重为共视点的数量。
3. 如果遍历完所有的共视关键帧，没有连接到关键帧（权重超过阈值），则对权重最大的关键帧建立连接关系

前面三步都是为了寻找满足要求的关键帧，找到有资格和当前KF建立连接关系的共视关键帧，添加关键帧的核心函数如下：

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(!mConnectedKeyFrameWeights.count(pKF))// 如果没建立共视关系则直接建立连接关系，本质上是一个map类型的变量
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight)// 如果已经建立共视关系了更新权重(共视点的数量)
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;
    }

   // 按照权重对当前关键帧的共视关键帧进行排序
   UpdateBestCovisibles();
}

4. 更新生成树的连接，初始化该关键帧的父关键帧为共视程度最高的那个关键帧，将当前关键帧作为其子关键帧

4.将该关键帧插入到地图中

本质上就是将该关键帧KF插入到当前活跃子地图对应成员变量的容器中

void Map::AddKeyFrame(KeyFrame *pKF){
   ...
   mspKeyFrames.insert(pKF);
   ...
}