学习目标：

回环检测的目的及主要作用
回环检测的实现方法及主要优缺点
回环优化的实现方法及主要优缺点

例如：

• 回环检测的目的及主要作用
• 回环检测的实现方法及主要优缺点
• 回环优化的实现方法及主要优缺点

学习内容：

1. 掌握一些基本名词
2. 掌握 Java 基本语法
3. 掌握条件语句
4. 掌握循环语句

回环检测的意义

前端提供特征点的提取和轨迹、地图的初值，而后端负责对所有这些数据进行优化。然而，如果像视觉里程计那样仅考虑相邻时间上的关键帧，那么，之前产生的误差将不可避免地累积到下一个时刻，使得整个SLAM出现累积误差，长期估计的结果将不可靠，或者说，我们无法构建全局一致的轨迹和地图。

回环检测模块能够给出除了相邻帧的一些时隔更加久远的约束。如果我们能够成功地检测到相机经过同一个地方这件事，就可以为后端的位姿图提供更多的有效数据，使之得到更好的估计，特别是得到一个全局一致的估计。
一方面，回环检测关系到我们估计的轨迹和地图在长时间下的正确性。凌一方面，由于回环检测提供了当前数据与历史数据的关联，我们还可以利用回环检测进行重定位。

回环检测的思路

• 基于里程计(Odometry based)的几何关系
  当我们发现当前相机运动到了之前的某个位置附近时，检测它们有没有回环关系。累积误差较大时无法工作。
• 基于外观(Appearance based)的几何关系
  与前端、后端的估计都无关，仅根据两幅图像的相似性确定回环检测关系。成为视觉SLAM的主流做法。

BRIEF描述子

binary rohust independent elementary features
https://blog.csdn.net/weixin_34910922/article/details/120244876

BRIEF是对已检测到的特征点进行描述，他是一种二进制编码的描述子。BRIEF的特征点位置可利用FAST、Harris检出，或者SIFT、SURF等算法检测特征点的位置，之后使用BRIEF建立特征描述符。

• step1：为减少噪声干扰，先对图像进行高斯滤波(方差为2，高斯窗口为9*9)
• step2：以特征点为中心，取S*S的邻域窗口。在窗口内随机选取一对(两个)点，比较二者像素的大小，进行如下二进制赋值。
  
  其中，p(x)，p(y)分别是随机点x=(u1,v1)，y=(u2,v3)的像素值。
• step3：在窗口中随机选取N对随机点，重复步骤2的二进制赋值，形成一个二进制编码，这个编码就是对特征点的描述，即特征描述子。（一般N=256）
  一对随机点的选择方法，原作者测试了以下5种方法，其中方法(2)比较好。
  
  经过上面的特征提取算法，对于一幅图中的每一个特征点，都得到了一个256bit的二进制编码。

词袋模型DBoW

一种回环检测的思路是：像视觉里程计那样使用特征点来做回环检测，我们对两幅图像的特征点进行匹配，只要匹配数量大于一定值，就认为出现了回环。这种做法存在着一些问题。
词袋，也就是Bag-of-Word(BoW)，目的是用“图像上有哪几种特征”来描述一副图像。

• 确定“人”、“车”、“狗”等概念——对应于BoW中的word，许多单词放在一起，组成了dictionary。
• 确定一副图像中出现了那些在字典中定义的概念——我们用单词出现的情况（或直方图）描述整幅图像。这就把一幅图像转换成一个向量的描述。
• 比较上一步中的描述的相似程度。
  字典是固定的，所以只用一个向量就可以描述整幅图像。
  字典由很多单词组成，而每一个单词代表了一个概念。一个单词与一个单独的特征点不同，它不是从单幅图像上提取出来的，而是某一类特征的组合。所以，字典生成问题，类似于一个聚类(Clustering)问题。
  用k叉树来表示字典。
• 在根节点，用K-means把所有样本聚成k类（实际中为保证聚类均匀性会使用K-means++）。这样就得到了第一层。
• 对第一层的每个节点，把属于该节点的样本再聚成k类，得到下一层。
• 依此类推，最后得到叶子层。叶子层即为所谓的Words。
  
  实际上，最终我们仍在叶子层构建了单词，而树结构中的中间节点仅供快速查找时使用。这样一个k分支、深度为d的树，可以容纳k^d个单词。另外，在查找某个给定特征对应的单词时，只需将它与每个中间节点的聚类中心比较(一共d次),即可找到最后的单词，保证了对数级别的查找效率。
  https://www.cnblogs.com/guoben/p/13339291.html
  DBoW2需要OpenCV和Boost：：dynamic_bitset类才能使用Brief版本。
  DBoW2与DLoopDetector一起在多个真实数据集上进行了测试，执行时间为3毫秒，将图像的Brief特征转换为词袋矢量，执行时间为5毫秒，以在数据库中查找图像匹配项。超过19000张图像。

PnP求解相对位姿

PnP(Perspective-n-Point)是求解3D到2D点对运动的方法。
它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
如果两张图像中的一张特征点的3D位置已知，那么最少只需要3个点对(以及至少一个额外点验证结果)就可以估计相机运动。
特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。
因此，在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，才能使用PnP。3D-2D方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是一种最重要的姿态估计方法。
PnP问题有很多种求解方法，例如，用3对点估计位姿的P3P、直接线性变换(DLT)、EPnP(Efficient PnP)、UPnP，等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的光束法平差(Bundle Adjustment，BA)。
首先是DLT。考虑这样一个问题：已知一组3D点的位置，以及它们在某个相机中的投影位置，求该相机的位姿。

请注意上图中的T和SE(3)中的变换矩阵T是不同的。
推导过程如下：
考虑某个空间点P，它的齐次坐标为P=（X,Y,Z,1)^T。

t一共有12维，因此最少通过6对匹配点即可实现矩阵T的线性求解，这种方法称为DLT。当匹配点大于6对时，也可以使用SVD等方法对超定方程求最小二乘解。

邻域匹配(searchByBRIEF)

PnPRANSAC

快速重定位

滑窗优化

VIO

图优化

重力对齐

四元数&俯仰角翻滚角偏航角

第i帧和第j帧的残差