欢迎访问我的博客首页。
ORB_SLAM 源码
1. 关键帧
阅读 ORB_SLAM 的源码最好从 KeyFrame.cc 开始。因为 KeyFrame.cc 中定义了一些很重要的数据结构，比如共视图、生成树。同时，KeyFrame.cc 中的函数几乎不会调用其它文件中的函数，而里程计、回环检测、回环闭合、优化等经常调用 KeyFrame.cc 中的函数，所以 KeyFrame.cc 即简单又重要。
1.1 相机坐标系原点在世界坐标系中的坐标
已知相机坐标到世界坐标的变换是 R w c R_{wc} Rwc 和 t w c t_{wc} twc，求相机坐标系原点 O c O_c Oc 在世界坐标系中的坐标 O c w O_c^w Ocw：假设相机坐标系原点 O c O_c Oc 在相机坐标系中的坐标是 O c c O_c^c Occ，由 R w c O c c + t w c = O c w R_{wc} O_c^c + t_{wc} = O_c^w RwcOcc+twc=Ocw 知
O c w = t w c . (1.1) O_c^w = t_{wc}. \tag{1.1} Ocw=twc.(1.1)
已知世界坐标到相机坐标的变换是 R c w R_{cw} Rcw 和 t c w t_{cw} tcw，求相机坐标系原点 O c O_c Oc 在世界坐标系中的坐标 O c w O_c^w Ocw：假设相机坐标系原点 O c O_c Oc 在世界坐标系中的坐标是 O c w O_c^w Ocw，由 R c w O c w + t c w = O c c R_{cw} O_c^w + t_{cw} = O_c^c RcwOcw+tcw=Occ 知
O c w = − R w c t c w . (1.2) O_c^w = - R_{wc} t_{cw}. \tag{1.2} Ocw=−Rwctcw.(1.2)
其中， O c c = 0 O_c^c = \bf 0 Occ=0， R w c = R c w − 1 R_{wc} = R_{cw}^{-1} Rwc=Rcw−1。
1.2 共视图
ORB_SLAM 中有共视图、生成树、本质图的概念，下图是它们的示意图。
有共同地图点的两个结点（关键帧）被一条边相连，组成共视图，共同地图点的数量是这条边的权重。共视图包含所有 N 个结点，每个结点可能与多个节点相连（共视地图点数量大于等于 15），也可能只与一个节点相连（共视地图点数量小于 15 但和该节点共视程度最好）。每个结点记录与它自己相连的结点，所有节点的记录组成共视图。
class KeyFrame {
protected:
// 要么与当前关键帧的共视地图点数量大于等于 15，要么是当前关键帧共视程度最好的一个关键帧（共视地图点数量小于 15）。
std::map<KeyFrame *, int> mConnectedKeyFrameWeights;
// mConnectedKeyFrameWeights 权重由高到低排序后的关键帧。
std::vector<KeyFrame *> mvpOrderedConnectedKeyFrames;
// mConnectedKeyFrameWeights 权重由高到低排序后的权重。
std::vector<int> mvOrderedWeights;
};
图 1.1 中的 4 个图都基于上面三个数据结构的前两个（尤其是第 1 个）。而下面的代码是 ORB_SLAM 管理这三个数据结构的主要函数。
void KeyFrame::UpdateConnections();
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight);
void KeyFrame::UpdateBestCovisibles();
两个节点有共视关系也称为有连接关系，连接关系不强调权重大小顺序，共视关系强调权重大小顺序：
set<KeyFrame *> KeyFrame::GetConnectedKeyFrames() {
// 无序的共视关键帧。
unique_lock<mutex> lock(mMutexConnections);
set<KeyFrame *> s;
for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(); mit != mConnectedKeyFrameWeights.end(); mit++)
s.insert(mit->first);
return s;
}
vector<KeyFrame *> KeyFrame::GetVectorCovisibleKeyFrames() {
// 有序的共视关键帧。
unique_lock<mutex> lock(mMutexConnections);
return mvpOrderedConnectedKeyFrames;
}
1.3 生成树
一个连通无向图 G 的生成树 T 是 G 的一个极小连通子图：T 含有 G 中的全部 n 个顶点，但只有足以构成一棵树的 n - 1 条边。
在 ORB_SLAM 中，与该关键帧的共同地图点数量最多的关键帧，被选为该关键帧的父结点。生成树的权重也是共同地图点的数量。生成树是共视图的一个极小连通子图：它和共视图一样包含所有 N 个结点（关键帧），但只有 N - 1 条边。每个结点记录它自己的父结点和子结点，所有节点的记录组成生成树：
class KeyFrame {
protected:
bool mbFirstConnection;
KeyFrame *mpParent;
std::set<KeyFrame *> mspChildrens;
};
下面的代码是 ORB_SLAM 创建生成树的主要过程：
void KeyFrame::UpdateConnections() {
// ...
// 如果当前关键帧没有爹（mbFirstConnection == true），为它找个爹。
if (mbFirstConnection && mnId != 0) {
mpParent = mvpOrderedConnectedKeyFrames.front();
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
创建生成树很容易，删除生成树的一个结点相对有点麻烦。因为删除生成树的一个结点前，需要为待删除结点的所有子结点寻找新的父结点。下图是 ORB_SLAM 删除生成树的一个结点的算法示意图。
我们把待删除的结点称为父结点，如上图红色的结点；把待删除结点的子结点称为孙结点，如上图蓝色的结点；待删除结点的父结点称为爷结点。绿色是候选父结点。
首先需要明确的是，ORB_SLAM 中的生成树更像一条代表轨迹的线，如图 (a) 而不是图 (b)。因此，孙结点的新父结点应该只从爷结点或该孙结点的亲兄弟节点中找，这是 ORB_SLAM 更新生成树的关键。
以图 (c) 为例，ORB_SLAM 更新生成树算法：
- 爷结点加入候选父结点集合。
- 遍历所有孙结点，找出与候选父结点集合中的某个结点 pP 共视程度最大的孙结点 pC。令 pC 的新父结点为 pP，同时把 pC 从孙结点（蓝色）移到候选父结点集合（绿色）。以图 (c) 为例，pP = B，pC = F，由图 (c) 得到图 (e)。如果孙结点都与候选父结点没有共视关系，则爷结点 B 作为它们的父结点，由图 (c) 得到图 (d)。
- 重复 2 直到孙结点为空，得到图 (h)。
下面的代码是 ORB_SLAM 删除生成树一个结点的更新过程。
// 从生成树中删除父结点，并为孙结点们找新父结点。
void KeyFrame::SetBadFlag() {
// ...
// 1. 爷结点被加入孙节点们的候选父结点集合中。
set<KeyFrame *> sParentCandidates;
sParentCandidates.insert(mpParent);
// 2. 如果有孙结点：每轮 while 循环为一个孙结点找个新父结点，同时该孙结点被加入候选父结点集合中。
while (!mspChildrens.empty()) {
bool bContinue = false;
int max = -1;
KeyFrame *pC;
KeyFrame *pP;
// 2.1 在候选父结点集合中为每个孙结点找一个新父结点。
for (set<KeyFrame *>::iterator sit = mspChildrens.begin(), send = mspChildrens.end(); sit != send; sit++) {
KeyFrame *pKF = *sit;
if (pKF->isBad())
continue;
vector<KeyFrame *> vpConnected = pKF->GetVectorCovisibleKeyFrames();
for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) { // 孙结点 pKF 的共视结点。
for (set<KeyFrame *>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end(); spcit != spcend; spcit++) { // 孙结点 pKF 的候选父结点。
// 候选父结点是共视结点。
if (vpConnected[i]->mnId == (*spcit)->mnId) {
int w = pKF->GetWeight(vpConnected[i]);
// 从候选父结点集合中选取共视程度最高的候选父结点。
if (w > max) {
pC = pKF;
pP = vpConnected[i];
max = w;
bContinue = true;
} // if
} // if
} // for
} // for
} // for
// 2.2 共视程度最高的候选父结点作为孙结点 pKF 的新父结点，同时孙结点 pKF 被加入候选父结点集合中。
if (bContinue) {
pC->ChangeParent(pP);
sParentCandidates.insert(pC);
mspChildrens.erase(pC);
}
else
break;
} // while
// 3. 如果有孙结点与所有候选父结点都没有共视关系，把爷结点作为这样的孙节点的新父结点。
if (!mspChildrens.empty())
for (set<KeyFrame *>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++) {
(*sit)->ChangeParent(mpParent);
}
// 4. 删除父结点。
mpParent->EraseChild(this);
mTcp = Tcw * mpParent->GetPoseInverse();
mbBad = true;
}
1.4 回环边
回环边在回环检测中用到，其定义如下：
map<KeyFrame *, set<KeyFrame *>> LoopConnections;
回环边就是共视关系，LoopConnections[KF] = KFs 指关键帧 KF 与关键帧 KFs 有共同地图点。其中 KF 是回环这一端的一个关键帧，KFs 是回环另一端的若干个关键帧。回环边就是图 1.1b 中的红线，它的产生过程见图 3.2 右。
1.5 本质图
本质图也包含所有 N 个结点（关键帧），在回环闭合后优化全局位姿时用到。它是回环边、生成树、回环的两个端点关键帧、共视图这四部分的统称。
2. 局部地图
局部地图保存在 Tracking.h 中，跟踪局部地图的函数是 bool Tracking::TrackLocalMap()。
KeyFrame* mpReferenceKF;
std::vector<KeyFrame*> mvpLocalKeyFrames;
std::vector<MapPoint*> mvpLocalMapPoints;
3. 回环
ORB_SLAM 中的回环检测单独是一个线程，包含回环检测、端点融合、回环闭合三大部分。
3.1 回环检测
当前关键帧是环的这一端，回环关键帧是环的另一端。它们在时间上不相邻，但在空间上相邻。回环检测可以分为两大部分：
- 查找候选回环关键帧：利用词袋，从关键帧数据库中查找与当前关键帧 mpCurrentKF 相似程度高的关键帧，作为候选回环关键帧 vpCandidateKFs。如下面代码的第 1 到 4 部分。
- 查找具有连续性的候选回环关键帧：至少连续四次回环检测时，候选回环区域连续的候选回环关键帧才被选中（见图 3.1a）。如下面代码的第 5 部分。
bool LoopClosing::DetectLoop() {
// ...
// 1. 每 10 个关键帧进行 1 次回环检测。
if (mpCurrentKF->mnId < mLastLoopKFid + 10) {
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
// 2. 计算当前关键帧与其共视关键帧的最小词袋相似性得分 minScore 。这个得分将作为筛选候选回环关键帧的最小分数阈值。
const vector<KeyFrame *> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
float minScore = 1;
for (size_t i = 0; i < vpConnectedKeyFrames.size(); i++) {
KeyFrame *pKF = vpConnectedKeyFrames[i];
if (pKF->isBad()) continue;
const DBoW2::BowVector &BowVec = pKF->mBowVec;
// 当前关键帧的词袋与其共视关键帧的词袋的相似性得分。
float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
if (score < minScore) minScore = score;
}
// 3. 根据最小词袋相似性得分 minScore 查找候选回环关键帧。
vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
// 4. 没有查找到候选回环关键帧。
if (vpCandidateKFs.empty()) {
mpKeyFrameDB->add(mpCurrentKF);
mvConsistentGroups.clear();
mpCurrentKF->SetErase();
return false;
}
mvpEnoughConsistentCandidates.clear();
vector<ConsistentGroup> vCurrentConsistentGroups;
vector<bool> vbConsistentGroup(mvConsistentGroups.size(), false);
// 5. 筛选候选回环区域。
for (size_t i = 0, iend = vpCandidateKFs.size(); i < iend; i++) {
// 5.1 spCandidateGroup: 候选回环关键帧 pCandidateKF 及其共视关键帧（暂且称之为候选回环区域）。
KeyFrame *pCandidateKF = vpCandidateKFs[i];
set<KeyFrame *> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
spCandidateGroup.insert(pCandidateKF);
// 5.2 这两个作为 if 的条件判断。
bool bEnoughConsistent = false; // 由于连续次数不够，当前候选回环关键帧 pCandidateKF 尚未被选为回环关键帧。
bool bConsistentForSomeGroup = false; // 当前的候选回环区域 spCandidateGroup 与已连通的候选回环区域 mvConsistentGroups 是否连通。
// 5.3 检查当前候选回环区域 spCandidateGroup 与上次回环检测时的若干个候选回环区域 mvConsistentGroups 是否重叠（至少共享一个关键帧）。
for (size_t iG = 0, iendG = mvConsistentGroups.size(); iG < iendG; iG++) {
set<KeyFrame *> sPreviousGroup = mvConsistentGroups[iG].first; // 上次回环检测时的一个候选回环区域 sPreviousGroup 。
bool bConsistent = false;
// 检查当前候选回环区域 spCandidateGroup 与上次回环检测时的一个候选回环区域 sPreviousGroup 是否重叠。
for (set<KeyFrame *>::iterator sit = spCandidateGroup.begin(), send = spCandidateGroup.end(); sit != send; sit++) {
if (sPreviousGroup.count(*sit)) {
bConsistent = true;
bConsistentForSomeGroup = true;
break;
}
}
// 如果重叠。
if (bConsistent) {
// 连续次数增加 1。
int nPreviousConsistency = mvConsistentGroups[iG].second; // 一次回环检测中，mvConsistentGroups[iG].second 的值是相同的。
int nCurrentConsistency = nPreviousConsistency + 1;
if (!vbConsistentGroup[iG]) { // 避免多个当前回环候选区域与同一个上次候选回环区域相连。
ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency); // spCandidateGroup 连接 mvConsistentGroups[iG].first 。
vCurrentConsistentGroups.push_back(cg);
vbConsistentGroup[iG] = true; // this avoid to include the same group more than once
}
// 如果连续 4 次及以上回环检测的候选回环区域相连，就把它加入 mvpEnoughConsistentCandidates 。
// bEnoughConsistent 似乎多余： pCandidateKF 被最外层循环遍历，只会被遍历一次。
if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) {
mvpEnoughConsistentCandidates.push_back(pCandidateKF);
bEnoughConsistent = true; // this avoid to insert the same candidate more than once
}
}
}
// 5.4 如果一个重叠也没有，vCurrentConsistentGroups 置 0，重新开始。
if (!bConsistentForSomeGroup) {
ConsistentGroup cg = make_pair(spCandidateGroup, 0);
vCurrentConsistentGroups.push_back(cg);
}
}
// 6. 为下一次调用 DetectLoop 更新 mvConsistentGroups 。
mvConsistentGroups = vCurrentConsistentGroups;
// ...
}
代码的第 5 部分用于验证候选回环关键帧是不是回环关键帧，找到的回环关键帧都会被放入 mvpEnoughConsistentCandidates。我们用下面的图来说明这部分。
图 3.1a 是回环检测示意图。蓝色圆形代表一个候选回环关键帧 pCandidateKF 及其共视关键帧组成的候选回环区域 spCandidateGroup。一列蓝色圆形是一次回环检测得到的候选回环区域。两个候选回环区域相交是指，至少有一个关键帧同时存在于这两个候选回环区域。我们用单向箭头表示两个候选回环区域相交，箭头由前一次回环检测得到的候选回环区域指向后一次回环检测得到的候选回环区域。ORB_SLAM 要求连续值至少为 3，才算检测到回环，结合图 3.1a 来说就是：
- 第 x 次回环检测得到 a、b、c、d 四个候选回环关键帧，进而得到四个候选回环区域。这四个候选回环区域的连续值都是 0，并作为参照组 mvConsistentGroups。
- 第 x+1 次回环检测得到若干个候选回环关键帧，进而得到若干个候选回环区域。但只有 a、b、c 三个候选回环关键帧对应的候选回环区域与参照组中的候选回环区域相交。于是这三个候选回环区域的连续值是 1，并作为新的参照组 mvConsistentGroups。
- 第 x+2 次回环检测得到若干个候选回环关键帧，进而得到若干个候选回环区域。但只有 a、b 两个候选回环关键帧对应的候选回环区域与参照组中的候选回环区域相交。于是这两个候选回环区域的连续值是 2，并作为新的参照组 mvConsistentGroups。
- 第 x+3 次回环检测得到若干个候选回环关键帧，进而得到若干个候选回环区域。但只有 a、b 两个候选回环关键帧对应的候选回环区域与参照组中的候选回环区域相交。于是这两个候选回环区域的连续值是 3，并作为新的参照组 mvConsistentGroups。由于连续值大于等于 3，a、b 被认定为回环关键帧，被放入 mvpEnoughConsistentCandidates，然后进行回环检测的后续步骤。
- 第 x+4 次回环检测得到若干个候选回环关键帧，进而得到若干个候选回环区域。但只有 a 一个候选回环关键帧对应的候选回环区域与参照组中的候选回环区域相交。于是这个候选回环区域的连续值是 4，并作为新的参照组 mvConsistentGroups。由于连续值大于等于 3，a 被认定为回环关键帧，被放入 mvpEnoughConsistentCandidates，然后进行回环检测的后续步骤。
- 第 x+5 次回环检测得到 a、b、c、d 四个候选回环关键帧，进而得到四个候选回环区域。这四个候选回环区域与参照组中的候选回环区域不相交。于是这四个候选回环区域的连续值是 0，并作为新的参照组 mvConsistentGroups。
代码 5.3 中有两个 if 语句。第一个 if 语句避免多个当前候选回环区域与上次的同一个候选回环区域相连，如图 3.1b 的情况；第二个 if 语句使用 bEnoughConsistent 避免一个当前候选回环区域与上次的多个候选回环区域相连，如图 3.1c 的情况。这时回环关键帧 pCandidateKF 将被重复放入 mvpEnoughConsistentCandidates。由于 pCandidateKF 被第一层循环遍历，仅被遍历一次，因此第二个 if 中没有必要使用 bEnoughConsistent。
3.2 端点匹配
回环检测得到的 mvpEnoughConsistentCandidates 中含有若干个回环关键帧 pKF。端点匹配算法先筛选出最多一个回环关键帧 mpMatchedKF，再从 mpMatchedKF 周围找出更多与当前关键帧匹配的地图点。具体来说就是：
- 求回环关键帧到当前关键帧的粗略相对位姿 Scm：先利用词袋搜索匹配的地图点，与当前关键帧匹配地图点少于 20 的回环关键帧被丢弃。用 RANSAC 算法求剩下的回环关键帧到当前关键帧的位姿 Scm。如下面代码的第 1、2 部分。
- 利用粗略相对位姿求精确相对位姿 gScm：用粗略相对位姿投影，在投影点附近根据描述子搜索更多匹配地图点 vpMapPointMatches。利用更多匹配地图点求精确相对位姿 gScm。如下面代码的第 3、4 部分。
- 在回环关键帧附近搜索更多匹配地图点：利用相对位姿更新当前关键帧的位姿 mScw。再利用投影和描述子匹配，从回环关键帧及其共视关键帧的地图点中搜索更多与当前关键帧匹配的地图点。如下面代码的第 5、6 部分。
至此，我们得到了一个最终的回环关键帧 mpMatchedKF，当前关键帧被更新的位姿 mScw，回环的另一端与当前关键帧匹配的地图点 mvpCurrentMatchedPoints。
bool LoopClosing::ComputeSim3() {
// ...
// 1. 对那些与当前帧 mpCurrentKF 有足够的一致性的关键帧，计算它们与当前帧的词袋相似度。词袋相似度低的被筛选掉，相似度高的计算 Sim3。
for (int i = 0; i < nInitialCandidates; i++) {
KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
// avoid that local mapping erase it while it is being processed in this thread
pKF->SetNotErase();
if (pKF->isBad()) {
vbDiscarded[i] = true;
continue;
}
// 从第 i 个候选回环关键帧 pKF 的地图点中找出与 mpCurrentKF 共有的地图点，放在 vvpMapPointMatches[i]。
int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);
if (nmatches < 20) {
vbDiscarded[i] = true;
continue;
} else {
Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale);
pSolver->SetRansacParameters(0.99, 20, 300);
vpSim3Solvers[i] = pSolver;
}
// 有效候选回环关键帧的数量。
nCandidates++;
}
// 2. 迭代每个候选回环关键帧，有一个成功或全部失败则结束。
bool bMatch = false;
while (nCandidates > 0 && !bMatch) {
for (int i = 0; i < nInitialCandidates; i++) {
// 共同地图点不够多。
if (vbDiscarded[i]) continue;
// 候选回环关键帧。
KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
// 2.1 每次迭代，选取 3 对匹配的地图点计算相似变换。最多迭代 5 次，内点数大于 20 才算成功。
vector<bool> vbInliers;
int nInliers;
bool bNoMore;
Sim3Solver *pSolver = vpSim3Solvers[i];
cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers); // 计算 Sim3。
// 匹配地图点太少或迭代失败。
if (bNoMore) {
vbDiscarded[i] = true;
nCandidates--;
}
// 2.2 迭代成功：回环关键帧 pKF 与当前关键帧 mpCurrentKF 的相似变换为 Scm ，且它们至少有 20 个共同地图点。
if (!Scm.empty()) {
// 取出第 i 个候选回环关键帧 pKF 与当前关键帧 mpCurrentKF 匹配的地图点放入 vpMapPointMatches 。
vector<MapPoint *> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint *>(NULL));
for (size_t j = 0, jend = vbInliers.size(); j < jend; j++) {
if (vbInliers[j]) vpMapPointMatches[j] = vvpMapPointMatches[i][j];
}
// 3. 根据迭代的位姿，搜索更多匹配地图点。
cv::Mat R = pSolver->GetEstimatedRotation();
cv::Mat t = pSolver->GetEstimatedTranslation();
const float s = pSolver->GetEstimatedScale();
// 利用相似变换投影，在投影点附近根据特征描述子相似度，从 pKF 中找出更多与 mpCurrentKF 的地图点匹配的地图点。
// vpMapPointMatches[i] = pMP：pKF 的地图点 pMP 与 mpCurrentKF 下标为 i 的地图点匹配。
matcher.SearchBySim3(mpCurrentKF, pKF, vpMapPointMatches, s, R, t, 7.5);
// 4. 根据更多地图点优化当前关键帧 mpCurrentKF 与回环关键帧 pKF 的相对位姿 gScm 。
g2o::Sim3 gScm(Converter::toMatrix3d(R), Converter::toVector3d(t), s);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
// 5. 如果内点数大于等于 20，则确定为最终回环关键帧 mpMatchedKF 。
// 然后利用相对位姿 gScm 更新当前关键帧 mpCurrentKF 的位姿 mScw(用回环另一端的位姿更新这一端的位姿，使这一端的位姿不再包含累积误差)。
if (nInliers >= 20) {
bMatch = true;
mpMatchedKF = pKF; // 中间结果 1。
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0);
mg2oScw = gScm * gSmw; // CurrentKF <- World = CurrentKF <- MatchedKF * MatchedKF <- World 。
mScw = Converter::toCvMat(mg2oScw); // 中间结果 2。
mvpCurrentMatchedPoints = vpMapPointMatches; // 中间结果 3，这里面是回环关键帧的地图点而不是当前关键帧的地图点！
break; // 只需找到一个回环关键帧 mpMatchedKF 。
}
}
}
}
if (!bMatch) {
// 这些帧没有用，允许被删除。
for (int i = 0; i < nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
// 6. mpMatchedKF 是最终筛选出的一个回环关键帧。
vector<KeyFrame *> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
vpLoopConnectedKFs.push_back(mpMatchedKF); // 回环关键帧 mpMatchedKF 及其共视关键帧。
// 6.1 回环关键帧 mpMatchedKF 及其共视关键帧的地图点。
mvpLoopMapPoints.clear();
for (vector<KeyFrame *>::iterator vit = vpLoopConnectedKFs.begin(); vit != vpLoopConnectedKFs.end(); vit++) {
KeyFrame *pKF = *vit;
vector<MapPoint *> vpMapPoints = pKF->GetMapPointMatches();
for (size_t i = 0, iend = vpMapPoints.size(); i < iend; i++) {
MapPoint *pMP = vpMapPoints[i];
if (pMP) {
if (!pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) {
mvpLoopMapPoints.push_back(pMP);
pMP->mnLoopPointForKF = mpCurrentKF->mnId;
}
}
}
}
// 6.2 从 6.1 获取的地图点 mvpLoopMapPoints 中找出更多地图点放入 mvpCurrentMatchedPoints 。利用投影附近的描述子搜索。
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints, 10);
// 6.3 如果匹配地图点足够多，就接受这个回环。
int nTotalMatches = 0;
for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
if (mvpCurrentMatchedPoints[i]) nTotalMatches++;
}
// ...
}
3.3 回环闭合
回环闭合算法使回环两端建立共视关系，再进行优化。使回环两端建立共视关系的步骤：
- 更新回环这一端的位姿。端点匹配时更新了回环这一端的当前关键帧位姿。现在，根据当前关键帧的新位姿，更新它共视关键帧的位姿和它们地图点的位姿。如下面代码的第 1 到 5 部分。
- 旧地图点替换新地图点：根据回环两端的匹配地图点，用回环另一端的地图点更新回环这一端的地图点。如下面代码的第 6、7 部分。
- 回环两端建立共视关系：更新共视关系，使当前关键帧及其共视关键帧与回环另一端的关键帧建立共视关系。如下面代码的第 8 部分。
- 优化：本质图优化和全局 BA 优化。如下面代码的第 9 到 12 部分。
void LoopClosing::CorrectLoop() {
// ...
// 1. 更新当前帧的共视关系（不会和回环的另一端共视）。
mpCurrentKF->UpdateConnections();
// 2. 当前关键帧及其共视关键帧。
mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);
// 3. 当前关键帧及其共视关键帧的位姿：它们的新位姿 CorrectedSim3, 它们的旧位姿 NonCorrectedSim3 。
KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
CorrectedSim3[mpCurrentKF] = mg2oScw;
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
{
// Get Map Mutex
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// 4. 获取更新后的位姿和原始位姿。
for (vector<KeyFrame *>::iterator vit = mvpCurrentConnectedKFs.begin(), vend = mvpCurrentConnectedKFs.end(); vit != vend; vit++) {
KeyFrame *pKFi = *vit;
cv::Mat Tiw = pKFi->GetPose();
// 4.1 据与当前帧的共视关系更新的位姿。
if (pKFi != mpCurrentKF) {
cv::Mat Tic = Tiw * Twc;
cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3);
cv::Mat tic = Tic.rowRange(0, 3).col(3);
g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0);
g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;
// Pose corrected with the Sim3 of the loop closure
CorrectedSim3[pKFi] = g2oCorrectedSiw;
}
// 4.2 原始位姿（有累积误差）。
cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);
cv::Mat tiw = Tiw.rowRange(0, 3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);
// Pose without correction
NonCorrectedSim3[pKFi] = g2oSiw;
}
// 5. 更新共视关键帧的位姿和地图点位姿：用第 4 步计算的新位姿，更新当前关键帧及其共视关键帧的位姿和地图点位姿。
for (KeyFrameAndPose::iterator mit = CorrectedSim3.begin(), mend = CorrectedSim3.end(); mit != mend; mit++) {
KeyFrame *pKFi = mit->first;
g2o::Sim3 g2oCorrectedSiw = mit->second;
g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];
// 5.1 更新地图点。
vector<MapPoint *> vpMPsi = pKFi->GetMapPointMatches();
for (size_t iMP = 0, endMPi = vpMPsi.size(); iMP < endMPi; iMP++) {
MapPoint *pMPi = vpMPsi[iMP];
if (!pMPi) continue;
if (pMPi->isBad()) continue;
if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId) continue;
// Project with non-corrected pose and project back with corrected pose
cv::Mat P3Dw = pMPi->GetWorldPos();
Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw)); // w <--精-- i <--粗-- w 。
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
pMPi->mnCorrectedReference = pKFi->mnId;
pMPi->UpdateNormalAndDepth();
}
// 5.2 更新关键帧的位姿。
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
double s = g2oCorrectedSiw.scale();
eigt *= (1. / s); // [R t/s;0 1]
cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt);
pKFi->SetPose(correctedTiw);
// Make sure connections are updated
pKFi->UpdateConnections();
}
// 6. 用回环区域的地图点取代当前关键帧的地图点，或添加为当前关键帧的地图点。
for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
if (mvpCurrentMatchedPoints[i]) {
MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];
MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);
if (pCurMP) pCurMP->Replace(pLoopMP);
else {
mpCurrentKF->AddMapPoint(pLoopMP, i);
pLoopMP->AddObservation(mpCurrentKF, i);
pLoopMP->ComputeDistinctiveDescriptors();
}
}
}
}
// 7. 用回环关键帧及其共视关键帧的地图点 mvpLoopMapPoints 替换当前关键帧及其共视关键帧的地图点。因为旧的地图点累积误差小。
SearchAndFuse(CorrectedSim3);
// 8. 地图点融合后，更新当前关键帧及其共视关键帧 pKFi 的共视关系，使回环的两端建立共视关系。
map<KeyFrame *, set<KeyFrame *>> LoopConnections;
for (vector<KeyFrame *>::iterator vit = mvpCurrentConnectedKFs.begin(); vit != mvpCurrentConnectedKFs.end(); vit++) {
KeyFrame *pKFi = *vit;
// 8.1 pKFi 的这些共视关键帧与 pKFi 在时间上相近。
vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); // 当前关键帧 mpCurrentKF 的共视关键帧 pKFi 的共视关键帧。
// Update connections. Detect new links.
// 8.2 pKFi 的共视关键帧包括：1. 与 pKFi 在时间上相近的关键帧，2. 与 pKFi 在时间上不相近但空间上相近（回环的另一端）。
pKFi->UpdateConnections(); // 使回环的两端建立共视关系。
LoopConnections[pKFi] = pKFi->GetConnectedKeyFrames();
// 8.3 排除 8.2 中所述第 1 种关键帧。
for (vector<KeyFrame *>::iterator vit_prev = vpPreviousNeighbors.begin(); vit_prev != vpPreviousNeighbors.end(); vit_prev++) {
LoopConnections[pKFi].erase(*vit_prev);
}
// 8.4 排除当前关键帧 mpCurrentKF 的共视关键帧 vit2。vit2 本来不和 pKFi 共视，pKKFi 更新共视关系后，它们有可能共视。
for (vector<KeyFrame *>::iterator vit2 = mvpCurrentConnectedKFs.begin(); vit2 != mvpCurrentConnectedKFs.end(); vit2++) {
LoopConnections[pKFi].erase(*vit2);
}
// 8.5 LoopConnections[pKFi] = KFs: pKFi 是回环这一端的某个关键帧，它与回环另一端的若干个关键帧 KFs 有共视关系。
}
// 9. Optimize graph
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
// 10. 地图。
mpMap->InformNewBigChange();
// 11. 回环的两个端点。Add loop edge
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
// 12. 全局 BA 优化。Launch a new thread to perform Global Bundle Adjustment
mbRunningGBA = true;
mbFinishedGBA = false;
mbStopGBA = false;
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);
// ...
}
代码第 3 部分的 CorrectedSim3 和 NonCorrectedSim3 如下图左。图中的圆代表关键帧，线代表连接（共视）关系。红色的圆是当前关键帧，绿色的圆是其共视关键帧。CorrectedSim3 是这些关键帧和它们去除该环上累积误差后的位姿，NonCorrectedSim3 是这些关键帧和它们去除该环上累积误差前的位姿。
代码第 8 部分如上图右。以关键帧 j 为例：
- 在代码第 7 部分的融合过程中，关键帧 j 的地图点被回环关键帧 a 及其共视关键帧 b 的地图点取代。这使得关键帧 j 可以和关键帧 a、b 及 a、b 附近的关键帧建立共视关系。当关键帧 j 更新共视关系时，它可能与三种关键帧相连：它自己的共视关键帧(黑色)、回环另一端的关键帧(橙色)、当前关键帧的共视关键帧(紫色)。如代码 8.2 部分。
- 去掉黑色和紫色连接，只保留橙色，得到 LoopConnections。如代码 8.3、8.4 部分。
4. 优化
在 Optimizer.cc 中定义了 5 个优化函数。
class Optimizer {
public:
static void GlobalBundleAdjustemnt(...);
static void BundleAdjustment(...);
static int PoseOptimization(...);
static void LocalBundleAdjustment(...);
static void OptimizeEssentialGraph(...);
static int OptimizeSim3(...);
};
5. 其它
5.1 词袋
在词袋模型中，一个单词对应一个描述子。从若干幅图像上共提取 n 个特征点，得到 n 个描述子，可以创建一个包含 n 个单词的词典。创建词典时对 n 个描述子聚类，这是一个无监督学习过程。词典是深度为 L 的 k 叉树，最多包含 k L k^L kL 个单词。
下面是使用 DBoW3 创建词典的方法。其中，创建特征提取器时，FeatureDetector 和 DescriptorExtractor 是 Feature2D 的别名。
#include <DBoW3/DBoW3.h>
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main() {
// 1. 变量。
Mat img;
Ptr<Feature2D> detector = ORB::create(10, 1.2, 8); // 特征提取器。
vector<KeyPoint> keypoints; // 一幅图像的关键点。
Mat descriptor; // 一幅图像的描述子。
vector<Mat> descriptors; // 多幅图像的描述子。
// 2. 获取多幅图像的描述子。
string images[] = {"1.jpg", "2.jpg", "3.jpg", "4.jpg"};
for (string image : images) {
img = imread(image);
detector->detect(img, keypoints);
detector->compute(img, keypoints, descriptor);
descriptors.push_back(descriptor);
// 缺少这一行会使下面的查询为空。
descriptor.release();
}
// 3. 创建字典。
DBoW3::Vocabulary voc(10, 5); // L=10, k=5。
voc.create(descriptors);
cout << voc << endl;
voc.save("voc3.yml.gz");
// 4. 查询。
img = imread("5.jpg");
detector->detectAndCompute(img, Mat(), keypoints, descriptor);
// Mat 转 vector<Mat>。
vector<Mat> descriptor_vec;
for (size_t i = 0; i < descriptor.rows; i++) {
descriptor_vec.push_back(descriptor.row(i));
}
// 计算该图像的词袋向量和特征向量。
DBoW3::BowVector BowVec;
DBoW3::FeatureVector FeatVec;
voc.transform(descriptor_vec, BowVec, FeatVec, 4);
for (pair<unsigned int, double> bowVec : BowVec) {
cout << "<" << bowVec.first << "," << bowVec.second << ">";
}
cout << endl;
for (pair<unsigned int, vector<unsigned int>> featVec : FeatVec) {
cout << featVec.first << ": ";
for (unsigned int x : featVec.second) {
cout << x << " ";
}
cout << endl;
}
}
DBoW 有两个重要的数据结构 BowVector(bag of word vector) 和 FeatureVector。它们的定义如下：
// Id of words
typedef unsigned int WordId;
// Value of a word
typedef double WordValue;
// Id of nodes in the vocabulary tree
typedef unsigned int NodeId;
// Vector of words to represent images
class DBOW_API BowVector : public std::map<WordId, WordValue> {};
// Vector of nodes with indexes of local features
class DBOW_API FeatureVector : public std::map<NodeId, std::vector<unsigned int>> {};
下图是程序运行结果。
单词向量 BowVector 代表一幅图像，如上图的第三行。它由词典中的单词以及对应的权重组成。
特征向量 FeatureVector 是对一幅图像的特征进行聚类。上图最后一行显示一幅图像的 10 个特征被分成 6 类。当一幅图像的特征较多时，它的大部分特征会被分成 L(树的深度)类，剩余的小部分特征可能不具有代表性。利用 FeatureVector 可以快速进行特征匹配：上图的 6、7、8 号特征被划为第 4 类，另一个图像能与 6、7、8 这三个特征相匹配的特征也在第 4 类。
下面是使用 DBow2 创建词典的方法。可以看出，使用 DBoW3 更简便。
#include <DBoW2/DBoW2.h>
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main() {
// 1. 变量。
Mat img;
Ptr<Feature2D> detector = ORB::create(); // 特征提取器。
vector<KeyPoint> keypoints; // 一幅图像的关键点。
Mat descriptor; // 一幅图像的描述子。
vector<Mat> descriptor_vec; // 一幅图像的描述子。
vector<vector<Mat>> descriptors; // 多幅图像的描述子。
// 2. 获取多幅图像的描述子。
string images[] = {"1.jpg", "2.jpg", "3.jpg", "4.jpg"};
for (string image : images) {
img = imread(image);
detector->detect(img, keypoints);
detector->compute(img, keypoints, descriptor);
// Mat 转 vec<Mat>。
for (size_t i = 0; i < descriptor.rows; i++) {
descriptor_vec.push_back(descriptor.row(i));
}
descriptors.push_back(descriptor_vec);
descriptor.release();
descriptor_vec.clear();
}
// 3. 创建字典。
DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB> voc;
voc.create(descriptors);
cout << voc << endl;
voc.save("voc2.yml.gz");
// 4. 查询。
img = imread("5.jpg");
detector->detectAndCompute(img, Mat(), keypoints, descriptor);
// Mat 转 vec<Mat>。
for (size_t i = 0; i < descriptor.rows; i++) {
descriptor_vec.push_back(descriptor.row(i));
}
DBoW2::BowVector BowVec;
DBoW2::FeatureVector FeatVec;
voc.transform(descriptor_vec, BowVec, FeatVec, 4);
cout << BowVec.size() << " " << FeatVec.size() << endl;
}
5.2 随机采样一致性算法
Sim3Solver::iterate
5.3 特征金字塔
OpenCV 检测的每个关键点都有一个尺度，表示这个关键点位于哪个特征金字塔。由于 OpenCV 默认从 8 个尺度检测关键点，所以用 octave 表示尺度。如下面代码的第 9 行：从 8 个尺度提取 500 个特征，0 尺度是原图，1 尺度是原图边长除 1.2，…，7 尺度是原图边长除 1. 2 7 1.2^7 1.27。
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main() {
Mat img = imread("1.jpg");
Ptr<Feature2D> detector = ORB::create(500, 1.2, 8);
vector<KeyPoint> keypoints;
detector->detect(img, keypoints);
for (KeyPoint kp : keypoints) {
cout << kp.octave << " " << kp.pt << endl;
}
}
ORBmatcher::SearchBySim3(...);
MapPoint::PredictScale(...);
6. 参考
- ORB-SLAM1 论文，Arxiv
- ORB-SLAM2 论文，Arxiv
- ORB-SLAM 源码，Github
- ORB_SLAM2 泡泡机器人注释和 PPT，gitee。
- 词袋，csdn。
扫一扫
1067
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
