文章目录

• ORB_SLAM2之LocalMapping局部建图
• • Run线程主函数
  • MapPointCulling删除质量不好的地图点
  • CreateNewMapPoints创建新的地图点
  • ComputeF12根据位姿计算基础矩阵
  • SearchInNeighbors检查并融合当前关键帧与相邻帧重复的地图点
  • KeyFrameCulling删除冗余关键帧

ORB_SLAM2之LocalMapping局部建图

局部建图的主要任务就是寻找更多匹配点，有利于跟踪的稳定性。

Run线程主函数

ProcessNewKeyFrames处理第一步处理新关键帧

1. 从缓冲队列中取出一帧关键帧

{uunique_lock<mutex> lock(mMutexNewKFs);
  mapCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();

2. 计算关键帧特征点的词袋向量ComputeBoW
3. 处理地图点。遍历当前关键帧的所有地图点GetMapPointMaches()，判断是这个地图点是否能来自当前关键帧IsInKeyFrame
   1. 如果这个地图点不来当前帧的观测(可以来自局部地图点)，则添加观测AddObservation，更新这个地图点的平均观测方向和观测距离UpdateNormalAndDepth()，并且更新这个地图点的最佳描述子ComputeDistinctiveDescriptors()。
   2. 如果地图点已经包含了这个关键帧，那么可能来自于CreateNewMapPoint或者双目匹配得到的。那么我们将这个点放入mlpRecentAddedMapPoints等待后续判断这个点的好坏MapPointCulling。
4. 更新关键帧之间的连接关系UpdateConnections()
5. 将该关键帧插入地图中

mpMap->AddKeyFrame(mCurrentkeyFrame);

MapPointCulling删除质量不好的地图点

1. 根据相机的类型设置不同的观测阈值。
   对于Monocular：nThObs = 2
   对于RGBD和Stere：nThObs = 3
2. 遍历所有新加入的地图点mlpRecentAddedMapPoints。
   1. 如果这个点本身就是坏点isBad()，那么就删除。
   2. 如果这个点被观测/应该被观测小于0.25的话，说明地图点的质量不好，被观测的数量为mnFoun d，应该被观测为mnVisible，如果没有超过比率，删除该店。
   3. 从该点建立开始mnFirstKFid到当前帧nCurrentKFid不小于两帧，没达到观测阈值nThObs那么就删除该帧。
3. 如果从创建到当前帧经历了三帧，那么它的质量高。

CreateNewMapPoints创建新的地图点

设定最佳共视关键帧的数目，单目为20，其他为10。

1. 取出当前帧共视程度最高的nn帧，创建一个匹配器，取出当前帧从世界坐标系到相机坐标系的变换矩阵Rcw1、tcw1。然后取出相机的内参。
2. 遍历相邻关键帧vpNeighKFs，第$i$个关键帧为KeyFrame* pKF2 = vpNeighKFs[i]，然后检查两个关键帧相机之间的基线cv::Mat vBaseline = Ow2 - Ow1，判断是否足够长
   1. 对于非单目：baseline=norm(vBaseline)>pKF2->mb?。如果小于这个阈值则合格，否则反之。
   2. 对于单目：计算关键帧的深度中值，计算$baseline/medianDepth2$如果这个值小于0.1，那么这个关键帧不满足条件
3. 根据两帧的位姿计算它们之间的基础矩阵cv::Mat F12 = ComputeF12(mpCurrentKeyFrme, pKF2)
4. 根据词袋对两个关键帧的未匹配的特征点进行快速匹配，用基础矩阵约束离群点，产生新的匹配点SearchForTriangulation。
5. 上面已经得到了这两帧之间匹配关系vMatchIndices保存着两帧之间的特征点的索引，然后三角化得到3D点。
   对于每对点，利用匹配点得到视角差:

cv::Mat xn1 = (cv::Mat_<float>(3, 1)<<(kp1.pt,x-cn1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0;
cv::Mat xn2 = (cv::Mat_<float>(3, 1)<<(kp2.pt,x-cn2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0;

得到匹配点射线夹角余弦值:

cv::Mat ray1 = Rwc1*xn1;
cv::Mat ray2 = Rwc2*xn2;
const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(cv::norm(ray1)*cv::norm(ray2));

然后对于双目，利用双目得到视角差，单目相机不处理:

if(bStereo1)
// 传感器是双目相机,并且当前的关键帧的这个点有对应的深度
// 假设是平行的双目相机，计算出双目相机观察这个点的时候的视差角余弦
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
else if(bStereo2)
// 传感器是双目相机,并且邻接的关键帧的这个点有对应的深度，和上面一样操作
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));         
// 得到双目观测的视差角中最小的那个
cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);

6. 然后三角化恢复3D点

if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
{
    // Linear Triangulation Method
    // 见Initializer.cc的 Triangulate 函数,实现是一样的,顶多就是把投影矩阵换成了变换矩阵
    cv::Mat A(4,4,CV_32F);
    A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
    A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
    A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
    A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);

    cv::Mat w,u,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);

    x3D = vt.row(3).t();
    // 归一化之前的检查
    if(x3D.at<float>(3)==0)
        continue;
    // 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}
// 匹配点对夹角小，用双目恢复3D点
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)  
{
    // 如果是双目，用视差角更大的那个双目信息来恢复，直接用已知3D点反投影了
    x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);                
}
else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)  
{
    x3D = pKF2->UnprojectStereo(idx2);
}
else
    continue; //No stereo and very low parallax, 放弃

然后检查这个点是否在相机的前方($z>0$)，不是的话就放弃这个点。

7. 检验这个点是否满足重投影误差要求。
   我们将这个3D点分别投影两个帧计算重投影误差。

const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
const float invz1 = 1.0/z1;

然后我们计算误差与卡方分布做对比，判断是否为外点。

8. 检查尺度连续性，就是计算这个这两个特征点到3D点的距离的比值与图像金字塔比例，如果差太多就退出。
9. 如果满足要求，创建3D点MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap)，然后为MapPoint添加属性。然后添加mlpRecentAddedMapPoints.push_back(pMP).然后之间检测这个点是否需要剔除MapPointCulling.

ComputeF12根据位姿计算基础矩阵

计算公式
$$F_{12}=[t_{12}{]}_{\times }{R}_{12}$$

// 先构造两帧之间的R12,t12
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();

cv::Mat R12 = R1w*R2w.t();

cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;

// 得到 t12 的反对称矩阵
cv::Mat t12x = SkewSymmetricMatrix(t12);

const cv::Mat &K1 = pKF1->mK;
const cv::Mat &K2 = pKF2->mK;

// Essential Matrix: t12叉乘R12
// Fundamental Matrix: inv(K1)*E*inv(K2)
return K1.t().inv()*t12x*R12*K2.inv();

SearchInNeighbors检查并融合当前关键帧与相邻帧重复的地图点

1. 获得当前关键帧在共视图中权重排名前$nn$得邻接关键帧vpNeighKFs。
   单目取20，其余取10

2. 遍历一级关键帧vpNeighKFs，取出第$i$个一级关键帧的前5个共视关键帧，然后将他们存在vpTargetKFs中。

3. 创建一个匹配器matcher，并取出当前关键帧中地图点的匹配关系vpMapPointMatches。遍历一级及二级共视关键帧vpTargetKFs，将第$i$个关键帧pKFi与当前关键帧进行融合matcher.Fuse(pKFi, vpMapPointMatches)

4. 遍历所有目标帧vTargetKFs，收集地图点，存进vpFuseCandidates中。

5. 然后进行地图点投影融合matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates)

6. 由于可能改变了地图点，所以我们要对该帧地图点和共视信息其进行更新：

...
pMP->ComputeDistinctiveDescritors();
pMP->UpdateNormalAndDepth();
...
mpCurrentKeyFrame->UpdateConnections();

KeyFrameCulling删除冗余关键帧

1. 获取当前帧的所有共视帧

vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames()

2. 遍历所有共视帧。然后获取每个共视帧的所有地图点:

const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();

然后遍历这个关键帧所有的地图点，如果地图点被观测超过三次，然后我们遍历能观测这个地图点的所有关键帧。如果当前帧特征点对应的金字塔层级scaleLevel等于或者小于这个地图点能观测到的关键帧对应特征点金字塔层级scaleLeveli说明这个点是不准确的。(ORB_SLAM中认为同样或者更低的金字塔层级地图点更准确)

KeyFrame* pKFi = mit->first;
if(pKFi==pKF)
    continue;
const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;

// 尺度约束：为什么pKF 尺度+1 要大于等于 pKFi 尺度？
// 回答：因为同样或更低金字塔层级的地图点更准确
if(scaleLeveli<=scaleLevel+1)
{
    nObs++;
    // 已经找到3个满足条件的关键帧，就停止不找了
    if(nObs>=thObs)
        break;
}

3. 最后计算超过三个观测地图点个数nRedundantObservations有与效地图点个数nMps的比值是否大于0.9，如果大于0.9的话，说明当前帧mCurrentKeyFrame观测观测到的太多3D点都能被其他关键帧观测到，那么它就是冗余的。删除它pKF->SetBadFlag()。