文章目录
1.简介
本质上是计算当前关键帧和关键帧数据库中的关键帧是否存在公共单词，相似性分数是否满足要求。
bool LoopClosing::NewDetectCommonRegions();
主要包括以下几个模块：
- 寻找回环候选关键帧和融合候选关键帧
- 对候选关键帧进行共视几何验证
- 如果共视几何验证失败了，再进行时序几何验证
2.进行共视区域检测(回环检测)的条件
- 回环开启标志
mbActiveLC=True,该变量在系统初始化函数中创建LoopClosing线程时设置。 - 系统还未创建地图，或者当前子地图IMU模式下还未进行第三次初始化
mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA2() - 双目模式下关键帧数量少于5帧不进行初始化
mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5 - 其他模式下当前活跃子地图中也要保持一定数量的关键帧(12帧)
3.寻找回环候选关键帧和融合候选关键帧DetectNBestCandidates
void DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand,
vector<KeyFrame*> &vpMergeCand, int nNumCandidates)
回环候选和融合候选的区别在于候选关键帧和当前关键帧是否属于同一个活跃子地图，如果是则是回环候选关键帧，反之是融合候选关键帧,关键步骤如下：
- 找出和当前关键帧存在公共单词数的历史关键帧，并进行筛选
- 基于L1范数计算和候选关键帧之间的相似性得分
- 根据共视关系将所有上述候选关键帧进行分组，找出每组相似性得分总分数，以总分数最高的一组里相似性得分最高的关键帧
- 根据候选关键帧与当前关键帧是否存在于同一个子地图，将关键帧放入不同的容器
3.1找出和当前关键帧存在公共单词数的历史关键帧，并进行筛选
- 寻找具备公共单词数的关键帧这里主要依据词袋向量的逆序索引，关键帧数据库维护一个
std::vector<list<KeyFrame*> >类型的成员变量，vector的索引表示单词的索引，索引对应的元素mvInvertedFile[i]表示包含该单词的所有关键帧，因此对于某个单词可以直接获取包含该单词的所有关键帧
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
- 排除和当前关键帧存在连接关系的共视关键帧：共视关键帧必然和当前关键帧处于同一区域，虽然存在公共单词但不是回环关键帧。
- 排除和当前关键帧公共单词数较少的候选关键帧
3.2基于L1范数计算和候选关键帧之间的相似性得分
float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
基于L1范数计算两个词袋向量的距离，实质上就是两个向量对应元素之和减去对应元素之差。
计算相似性得分时并不是根据单词的有无进行0,1的比较,而是根据单词出现的频率计算一个TF-IDF权重,所以词袋向量中每个元素都表征了每个单词在图像中出现的频率.
单词权重和相似性得分的计算方式如下:
3.3通过分组寻找和当前关键帧相似性程度最高的候选关键帧
根据共视关系将所有上述候选关键帧进行分组，找出每组相似性得分最高的关键帧及其对应的分数。
分组方式如下：
最佳相似性关键帧： 假设以下几个节点中与当前KF相似性得分最好的分别为：1,2,2,3,10,则将这五个得分最高的关键帧拿出来作为最佳候选，其中可能有重复的，但无所谓后面会处理。
3.4.根据候选关键帧与当前关键帧是否存在于同一个子地图，将关键帧放入不同的容器
- 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够，将其放入回环候选关键帧容器
vpLoopCand - 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad，将其添加到融合候选关键帧容器
vpMergeCand
while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates)){
KeyFrame* pKFi = it->second;
if(pKFi->isBad())
continue;
// 如果没有被重复添加
if(!spAlreadyAddedKF.count(pKFi)){
// 如果候选帧与当前关键帧在同一个地图里,且候选者数量还不足够，将其放入回环候选关键帧容器
if(pKF->GetMap() == pKFi->GetMap() && vpLoopCand.size() < nNumCandidates){
vpLoopCand.push_back(pKFi);
}
// 如果候选者与当前关键帧不再同一个地图里, 且候选者数量还不足够, 且候选者所在地图不是bad，将其添加到融合候选关键帧容器
else if(pKF->GetMap() != pKFi->GetMap() && vpMergeCand.size() < nNumCandidates && !pKFi->GetMap()->IsBad()){
vpMergeCand.push_back(pKFi);
}
spAlreadyAddedKF.insert(pKFi);
}
i++;
it++;
}
4.几何相似性验证DetectCommonRegionsFromBoW
- 对于每个候选关键帧找出其共视程度最高的10个共视关键帧
- 通过Bow寻找候选帧窗口内的关键帧地图点与当前关键帧的匹配点
- 利用RANSAC寻找候选关键帧窗口与当前关键帧的相对位姿T_cm的初始值
- 利用搜索到的更多的匹配点用Sim3优化投影误差得到的更好的相似变换信息
- 利用地图中的共视关键帧验证(共视几何校验)
函数声明：
/**
* @brief 实现论文第8页的2-5步中的一部分功能(对后面新进来的关键帧的验证没有放在这个函数里进行)
*
* @param[in] vpBowCand bow 给出的一些候选关键帧
* @param[out] pMatchedKF2 最后成功匹配的候选关键帧
* @param[out] pLastCurrentKF 用于记录当前关键帧为上一个关键帧(后续若仍需要时序几何校验需要记录此信息)
* @param[out] g2oScw 候选关键帧世界坐标系到当前关键帧的Sim3变换
* @param[out] nNumCoincidences 成功几何验证的帧数，超过3就认为几何验证成功，不超过继续进行时序验证
* @param[out] vpMPs 所有地图点
* @param[out] vpMatchedMPs 成功匹配的地图点
* @return true 检测到一个合格的共同区域
* @return false 没检测到一个合格的共同区域
*/
bool DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
4.1对于每个候选关键帧找出其共视程度最高的10个共视关键帧
std::vector<KeyFrame*> vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
对于某个候选关键帧的10个共视关键帧，如果其中存在当前帧的共视关键帧，则放弃该候选关键帧。
4.2通过Bow寻找候选帧窗口内的关键帧地图点与当前关键帧的匹配点
int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]);
vvpMatchedMPs是一个std::vector<std::vector<MapPoint*> >类型的变量，其中每个元素储存了每个候选关键帧的共视帧与当前帧的匹配的地图点。
通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配。通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配）。对属于同一node的特征点通过描述子距离进行匹配，通过距离阈值、比例阈值和角度投票进行剔除误匹配。
4.3利用RANSAC寻找候选关键帧窗口与当前关键帧的相对位姿T_cm的初始值
随机选择3对匹配点，计算相似变换，直到算法收敛(基于sim3相似变换矩阵得到的内点数量大于一定阈值)或者达到最大迭代次数。
之所以求解sim3而不是直接求解变换矩阵的原因是，跑那么长时间了可能会出现尺度漂移的情况。
mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge);
求解步骤如下：
1. 分别基于三对匹配3D点建立两个坐标系
X轴：
x
l
^
=
x
l
/
∥
x
l
∥
\hat{x_l} = x_l/\left \| x_l \right \|
xl^=xl/∥xl∥ ， 其中
x
l
=
r
l
,
2
−
r
l
,
1
x_l = r_{l,2} - r_{l,1}
xl=rl,2−rl,1
Y轴：
y
l
^
=
y
l
/
∥
y
l
∥
\hat{y_l} = y_l/\left \| y_l \right \|
yl^=yl/∥yl∥，其中
y
l
=
(
r
l
,
3
−
r
l
,
1
)
−
[
(
r
l
,
3
−
r
l
,
1
)
⋅
x
l
^
]
x
l
^
y_l = \left ( r_{l,3} - r_{l,1} \right ) - \left [ \left ( r_{l,3} - r_{l,1} \right ) \cdot \hat{x_l} \right ]\hat{x_l}
yl=(rl,3−rl,1)−[(rl,3−rl,1)⋅xl^]xl^
Z轴：
z
l
^
=
x
l
^
×
y
l
^
\hat{z_l} = \hat{x_l}\times\hat{y_l}
zl^=xl^×yl^
右坐标系同理，且令： M l = ∣ x l ^ , y l ^ , z l ^ ∣ , M r = ∣ x r ^ , y r ^ , z r ^ ∣ M_l = \left | \hat{x_l}, \hat {y_l}, \hat{z_l} \right | ,M_r = \left | \hat{x_r}, \hat{y_r}, \hat{z_r} \right | Ml=∣xl^,yl^,zl^∣,Mr=∣xr^,yr^,zr^∣
其中， ( r l , 3 − r l , 1 ) \left ( r_{l,3} - r_{l,1} \right ) (rl,3−rl,1)表示斜边向量 r 13 r_{13} r13， x l ^ \hat{x_l} xl^表示x轴方向的单位向量， ( r l , 3 − r l , 1 ) ⋅ x l ^ \left( r_{l,3} - r_{l,1} \right ) \cdot \hat{x_l} (rl,3−rl,1)⋅xl^表示斜边向量在x轴上分量的长度， [ ( r l , 3 − r l , 1 ) ⋅ x l ^ ] x l ^ \left [ \left ( r_{l,3} - r_{l,1} \right ) \cdot \hat{x_l} \right ]\hat{x_l} [(rl,3−rl,1)⋅xl^]xl^表示斜边向量在x轴上的分量大小，最终 y l y_l yl表示斜边向量减去在x轴方向的分量得到在y轴上的分量，归一化处理后得到y轴方向
2.旋转量R计算
旋转矩阵的变换涉及到了同一向量在不同基底(坐标系)下的表示和变换关系.
如果左边坐标系有一个向量 r l r_l rl,那么 M l T r l M_{l}^{T}r_l MlTrl 可以得到 r l r_l rl向量沿着坐标轴的值
M
r
M_r
Mr左乘
M
l
T
r
l
M_{l}^{T}r_l
MlTrl后可以得到该向量在右侧坐标系的表示
r
r
=
M
r
M
l
T
r
l
r_r=M_rM_{l}^{T}r_l
rr=MrMlTrl,因此可以推导出左右坐标系的旋转：
R
=
M
r
M
l
T
R = M_rM_{l}^{T}
R=MrMlT
3.扩展-代码中旋转矩阵的计算
3.平移向量计算
假设左右坐标系中各有n个点，他们在两个坐标系下的坐标分别为:
{
r
l
,
i
}
\left \{r_{l,i} \right \}
{rl,i}和
{
r
r
,
i
}
\left \{r_{r,i} \right \}
{rr,i},
i
i
i的取值为1到n
假设某个向量由左侧坐标系到右侧坐标系的变换可以表示为： r r = s R ( r l ) + r 0 r_r = sR\left ( r_l \right ) + r_0 rr=sR(rl)+r0，其中s为尺度变换， r 0 r_0 r0为平移偏移量。 ( r l ) \left ( r_l \right ) (rl)表示向量从左侧坐标系到右侧坐标系的旋转。
而实际求解时，两个坐标系之间的变换总是会存在误差，因此可以将问题的求解转换为一个最小二乘问题，可以看出这里的误差为:
e
i
=
r
r
,
i
−
s
R
(
r
l
,
i
)
−
r
0
e_i = r_{r,i} - sR\left ( r_{l,i} \right ) - r_0
ei=rr,i−sR(rl,i)−r0
最终最小二乘问题变为求解以下问题，待求解量为平移向量
r
0
r_0
r0：
1
2
min
∑
i
=
1
n
∥
e
i
∥
2
\frac{1}{2}\min \sum_{i=1}^{n} \left \| e_i \right \| ^2
21mini=1∑n∥ei∥2
看到这里可以联想到《十四讲》中关于视觉ICP的求解过程，使用去质心法构造误差函数形式，求解使得误差最小时对应的平移向量 r 0 r_0 r0
step1 分别计算左侧和右侧坐标系所有点的质心
r
l
ˉ
=
1
n
∑
i
=
1
n
r
l
,
i
r
r
ˉ
=
1
n
∑
i
=
1
n
r
r
,
i
\begin{equation}\begin{split} \bar{r_l} = \frac{1}{n}\sum_{i=1}^{n}r_{l,i}\\ \bar{r_r} = \frac{1}{n}\sum_{i=1}^{n}r_{r,i}\end{split}\end{equation}
rlˉ=n1i=1∑nrl,irrˉ=n1i=1∑nrr,i
step2 对每对点的误差作以下处理
e
i
=
r
r
,
i
−
s
R
r
l
,
i
−
r
0
=
r
r
,
i
−
s
R
r
l
,
i
−
r
0
−
r
r
ˉ
+
R
r
l
ˉ
+
r
r
ˉ
−
R
r
l
ˉ
=
r
r
,
i
−
r
r
ˉ
−
s
R
(
r
l
,
i
−
r
l
ˉ
)
+
(
r
r
ˉ
−
R
r
l
ˉ
−
r
0
)
\begin{equation} \begin{split} e_i & = r_{r,i} - sRr_{l,i} - r0\\ & = r_{r,i} - sRr_{l,i} - r0 -\bar{r_r} + R\bar{r_l} + \bar{r_r} - R\bar{r_l}\\ & = r_{r,i} - \bar{r_r} - sR(r_{l,i} -\bar{r_l}) + (\bar{r_r} - R\bar{r_l} -r0)\\ \end{split} \end{equation}
ei=rr,i−sRrl,i−r0=rr,i−sRrl,i−r0−rrˉ+Rrlˉ+rrˉ−Rrlˉ=rr,i−rrˉ−sR(rl,i−rlˉ)+(rrˉ−Rrlˉ−r0)
step3 关于误差的最小二乘函数为
1
2
∑
i
=
1
n
∥
e
i
∥
2
=
1
2
∑
i
=
1
n
∥
r
r
,
i
−
r
r
ˉ
−
s
R
(
r
l
,
i
−
r
l
ˉ
)
+
(
r
r
ˉ
−
R
r
l
ˉ
−
r
0
)
∥
2
=
1
2
∑
i
=
1
n
(
∥
r
r
,
i
−
r
r
ˉ
−
s
R
(
r
l
,
i
−
r
l
ˉ
)
∥
2
+
∥
r
r
ˉ
−
R
r
l
ˉ
−
r
0
∥
2
+
2
(
r
r
,
i
−
r
r
ˉ
−
s
R
(
r
l
,
i
−
r
l
ˉ
)
)
T
(
r
r
ˉ
−
R
r
l
ˉ
−
r
0
)
)
\begin{equation} \begin{split} \frac{1}{2} \sum_{i=1}^{n} \left \| e_i \right \| ^2 & =\frac{1}{2} \sum_{i=1}^{n} \left \| r_{r,i} - \bar{r_r} - sR(r_{l,i} -\bar{r_l}) + (\bar{r_r} - R\bar{r_l} -r0) \right \| ^2\\ & =\frac{1}{2} \sum_{i=1}^{n} (\left \| r_{r,i} - \bar{r_r} - sR(r_{l,i} -\bar{r_l}) \right \| ^2 +\left \| \bar{r_r} - R\bar{r_l} -r0 \right \|^2 +\\ &\quad 2(r_{r,i} - \bar{r_r} - sR(r_{l,i} -\bar{r_l}))^T(\bar{r_r} - R\bar{r_l} -r0))\end{split}\end{equation}
21i=1∑n∥ei∥2=21i=1∑n∥rr,i−rrˉ−sR(rl,i−rlˉ)+(rrˉ−Rrlˉ−r0)∥2=21i=1∑n(∥rr,i−rrˉ−sR(rl,i−rlˉ)∥2+∥rrˉ−Rrlˉ−r0∥2+2(rr,i−rrˉ−sR(rl,i−rlˉ))T(rrˉ−Rrlˉ−r0))
可以看到整个式子可以分为3项，对于第三项交叉项部分
(
r
r
,
i
−
r
r
ˉ
−
s
R
(
r
l
,
i
−
r
l
ˉ
)
)
(r_{r,i} - \bar{r_r} - sR(r_{l,i} -\bar{r_l}))
(rr,i−rrˉ−sR(rl,i−rlˉ))在求和后为0，第一项
∥
r
r
,
i
−
r
r
ˉ
−
s
R
(
r
l
,
i
−
r
l
ˉ
)
∥
2
\left \| r_{r,i} - \bar{r_r} - sR(r_{l,i} -\bar{r_l}) \right \| ^2
∥rr,i−rrˉ−sR(rl,i−rlˉ)∥2和平
r
0
r_0
r0无关，因此可以都删掉，则误差函数可以变为：
1
2
∑
i
=
1
n
∥
e
i
∥
2
=
1
2
∑
i
=
1
n
∥
r
r
ˉ
−
R
r
l
ˉ
−
r
0
∥
2
\begin{equation} \begin{split} \frac{1}{2} \sum_{i=1}^{n} \left \| e_i \right \| ^2 & =\frac{1}{2} \sum_{i=1}^{n}\left \| \bar{r_r} - R\bar{r_l} -r0 \right \|^2\end{split}\end{equation}
21i=1∑n∥ei∥2=21i=1∑n∥rrˉ−Rrlˉ−r0∥2
step4 求解平移向量r0
可以知道当
r
r
ˉ
−
R
r
l
ˉ
−
r
0
=
0
\bar{r_r} - R\bar{r_l} -r0=0
rrˉ−Rrlˉ−r0=0时误差最小，由此解得平移向量为：
r
0
=
r
r
ˉ
−
R
r
l
ˉ
\begin{equation} r0=\bar{r_r} - R\bar{r_l}\end{equation}
r0=rrˉ−Rrlˉ
4.尺度计算
由于长时间运动会使得相机轨迹的尺度也发生一定的漂移，因此假设候选回环帧和当前关键帧之间是相似变换而非刚体变换。
4.4利用初始的Scm信息,进行重投影匹配,并非线性优化得到更精确的Scm
核心思想是将最佳候选关键帧组的地图点投影到当前关键帧，基于网格注册信息加速特征匹配过程，通过计算地图点描述子和关键帧特征点描述子距离得到匹配关系
int numProjMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5);
4.5利用搜索到的更多的匹配点用Sim3优化投影误差得到的更好的相似变换信息
int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pKFi, vpMatchedMP, gScm, 10, mbFixScale, mHessian7x7, true);
4.6共视几何校验:使用当前关键帧的共视帧验证得到的相似变换矩阵
- 取出和当前关键帧共视关系最好的5个关键帧，成为验证组
- 通过之前计算的位姿将候选关键帧及其共视帧的地图点投影到验证组关键帧上, 若匹配点大于一定数目的任务成功验证一次
- 5次中有连续3次验证成功则几何相似性验证成功
/**
* @brief 用来验证候选帧的函数, 这个函数的名字取的不好, 函数的本意是想利用候选帧的共视关键帧来验证候选帧,不如改叫做：DetectCommonRegionsFromCoVKF
*
* @param[in] pCurrentKF 当前关键帧的共视关键帧(验证组关键帧之一)
* @param[in] pMatchedKF 候选帧
* @param[in] gScw 世界坐标系在验证帧下的Sim3
* @param[out] nNumProjMatches 最后匹配的数目
* @param[out] vpMPs 候选帧的窗口内所有的地图点
* @param[out] vpMatchedMPs 候选帧的窗口内被匹配到的地图点
* @return true 验证成功
* @return false 验证失败
*/
bool LoopClosing::DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
5.时序几何校验
如果在共视几何校验时，通过校验的验证组关键帧数量大于0小于3,则认为共视几何校验失败，但没有完全失败，还可以通过时序几何校验抢救一下。
在上一关键帧进行共视几何校验失败时，会暂时保留之前的候选关键帧和相似变换关系，利用当前关键帧与上一关键帧之间的运动变换，可以得到当前新的关键帧与候选关键帧的运动变换。
然后继续把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿。
bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);
如果时序几何验证成功超过三次，则判定找到了共视区域,根据候选帧处于当前活跃子弟图还是旧的非活跃子弟图，区分是回环情况还是地图融合。
6.记录最终的共视区域检测结果为回环矫正或融合作准备
// 记录第二次searchByProjection得到最多匹配点的关键帧的各种信息,最后作为回环帧/融合帧
if(nBestMatchesReproj < numProjOptMatches){
nBestMatchesReproj = numProjOptMatches;// 投影匹配的数量
nBestNumCoindicendes = nNumKFs;// 成功验证的帧数
pBestMatchedKF = pMostBoWMatchesKF;// 记录候选帧窗口内与当前关键帧相似度最高的帧
g2oBestScw = gScw;// 记录最优的位姿(这个位姿是由Tam推到出来的 : Taw = Tam * Tmw,这里a表示c)
vpBestMapPoints = vpMapPoints;// 记录所有的地图点
vpBestMatchedMapPoints = vpMatchedMP;// 记录所有的地图点中被成功匹配的点
}
516
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
