-
void LocalMapping::Run()
局部建图主程序：
当等待列表不为空时，进入程序：- 计算关键帧特征点的BoW映射，将关键帧插入地图
- ProcessNewKeyFrame函数中引入的不合格MapPoints
- 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints
- 处理完队列中最后一个关键帧时，检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
- 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping，当局部地图中的关键帧大于2个的时候进行局部地图的BA
-
void LocalMapping::ProcessNewKeyFrame()
计算关键帧特征点的BoW映射，将关键帧插入地图，每次只对一帧进行操作：- 计算该关键帧特征点的Bow映射关系
- 跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches(); // 对当前处理的这个关键帧中的所有的地图点展开遍历 for(size_t i=0; i<vpMapPointMatches.size(); i++) { MapPoint* pMP = vpMapPointMatches[i]; if(pMP) { if(!pMP->isBad()) { // 非当前帧生成的MapPoints // 为当前帧在tracking过程跟踪到的MapPoints更新属性 if(!pMP->IsInKeyFrame(mpCurrentKeyFrame)) { // 添加观测 pMP->AddObservation(mpCurrentKeyFrame, i); // 获得该点的平均观测方向和观测距离范围 pMP->UpdateNormalAndDepth(); // 加入关键帧后，更新3d点的最佳描述子 pMP->ComputeDistinctiveDescriptors(); } else // this can only happen for new stereo points inserted by the Tracking { mlpRecentAddedMapPoints，等待检查 // CreateNewMapPoints函数中通过三角化也会生成MapPoints // 这些MapPoints都会经过MapPointCulling函数的检验 mlpRecentAddedMapPoints.push_back(pMP); // 认为这些由当前关键帧生成的地图点不靠谱,将其加入到待检查的地图点列表中 } } } }- 更新关键帧之间的连接关系
- 讲关键帧插入到 地图中
-
void LocalMapping::MapPointCulling()
剔除 ProcessNewKeyFrame 和 CreateNewMapPoints 函数中引入的质量不好的 MapPoints- 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%
- 从该点建立开始，到现在已经过了不小于2个关键帧
- 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
-
void LocalMapping::CreateNewMapPoints()
相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints，- 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
- 遍历相邻关键帧，根据两个关键帧的位姿计算它们之间的基本矩阵
- 通过极线约束限制匹配时的搜索范围，进行特征点匹配
- 对每对匹配通过三角化生成3D点
三角化恢复3D点
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; // Euclidean coordinates 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标 x3D = x3D.rowRange(0,3)/x3D.at<float>(3);- 之后进行检验（检验地图点在两帧中的深度，在两帧中的重投影误差，尺度连续性）），如果检验通过则建立当前帧的地图点及其属性（a.观测到该MapPoint的关键帧 b.该MapPoint的描述子 c.该MapPoint的平均观测方向和深度范围）
// step 6.8：三角化生成3D点成功，构造成MapPoint MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap); // step 6.9：为该MapPoint添加属性： // a.观测到该MapPoint的关键帧 // b.该MapPoint的描述子 // c.该MapPoint的平均观测方向和深度范围 pMP->AddObservation(mpCurrentKeyFrame,idx1); pMP->AddObservation(pKF2,idx2); mpCurrentKeyFrame->AddMapPoint(pMP,idx1); pKF2->AddMapPoint(pMP,idx2); pMP->ComputeDistinctiveDescriptors(); pMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pMP); // step 6.10：将新产生的点放入检测队列 // 这些MapPoints都会经过MapPointCulling函数的检验 mlpRecentAddedMapPoints.push_back(pMP); nnew++; -
void LocalMapping::SearchInNeighbors()
检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints- 获取相邻关键帧和相邻关键帧的相邻关键帧
// 候选的一级相邻关键帧 const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn); // 筛选之后的一级相邻关键帧以及二级相邻关键帧 vector<KeyFrame*> vpTargetKFs; // 开始对所有候选的一级关键帧展开遍历： for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++) { KeyFrame* pKFi = *vit; // 没有和当前帧进行过融合的操作 if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId) continue; vpTargetKFs.push_back(pKFi);// 加入一级相邻帧 pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// 并标记已经加入 // Extend to some second neighbors const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5); // 遍历得到的二级相邻关键帧 for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++) { KeyFrame* pKFi2 = *vit2; // 当然这个二级关键帧要求没有和当前关键帧发生融合,并且这个二级关键帧也不是当前关键帧 if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId) continue; vpTargetKFs.push_back(pKFi2);// 存入二级相邻帧 } }- 将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合，这里用的ORBMatcher中的Fuse()，ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint > &vpMapPoints, const float th)将地图点投影到关键帧的方式进行特征匹配选取最优的匹配点，如果匹配点与当前点的描述子距离小于阈值的情况下进行融合。融合的方式是用被观察次数大的地图点代替被观察次数少的地图点，用函数MapPoint::Replace(MapPoint pMP)融合，融合地图点作如下操作：（1）将被替换地图点的被观察次数,被查找次数,以及观察到该地图点的关键帧都清空,坏地图点标志置位（2）将被替换地图点的被观察次数,被查找次数都加到替换地图点pMP中,并将当前地图点在关键帧中的位置用代替地图点代替（3）最后将本地图点从全局地图map中删除
这其中的融合就是删除冗余关键帧和地图点，
- 更新当前帧MapPoints的描述子，深度，观测主方向等属性
- 更新当前帧的MapPoints后更新与其它帧的连接关系,更新covisibility图 -
cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
根据两关键帧的姿态计算两个关键帧之间的基本矩阵，原理可以看高翔的SLAM十四讲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();
void LocalMapping::KeyFrameCulling()
关键帧剔除,在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。这个函数用于数据融合
// STEP3：遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad())
{
if(!mbMonocular)
{
// 对于双目，仅考虑近处的MapPoints，不超过mbf * 35 / fx
// 不过配置文件中给的是近点的定义是 40 * fx
if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
continue;
}
nMPs++;
// MapPoints至少被三个关键帧观测到
if(pMP->Observations()>thObs)
{
const int &scaleLevel = pKF->mvKeysUn[i].octave;
const map<KeyFrame*, size_t> observations = pMP->GetObservations();
// 判断该MapPoint是否同时被三个关键帧观测到
int nObs=0;
// 遍历当前这个邻接关键帧上的地图点的所有观测信息
for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
if(pKFi==pKF)
continue;
const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
// Scale Condition
// 尺度约束，要求MapPoint在该局部关键帧的特征尺度大于（或近似于）其它关键帧的特征尺度
if(scaleLeveli<=scaleLevel+1)
{
nObs++;
// 已经找到三个同尺度的关键帧可以观测到该MapPoint，不用继续找了
if(nObs>=thObs)
break;
}
}
// 该MapPoint至少被三个关键帧观测到
if(nObs>=thObs)
{
nRedundantObservations++;
}
}
}
}
}
// STEP4：该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧
if(nRedundantObservations>0.9*nMPs)
// 剔除的时候就设置一个 bad flag 就可以了
pKF->SetBadFlag();
三角测量
三角测量的用途是用来确定深度信息的，从而确定地图点的三维点坐标。已知匹配特征点对和各自相机矩阵
,估计三维点X，存在
,都属于
模型：
image
image
采用DLT方法，x叉乘PX=0,得到：
image
对于x'有同样关系：
image
可以写作AX=0
通过SVD分解可以得到X的解
