SLAM之回环检测与优化 | 您所在的位置:网站首页 › 电脑回环检测怎么做 › SLAM之回环检测与优化 |
学习目标:
回环检测的目的及主要作用 回环检测的实现方法及主要优缺点 回环优化的实现方法及主要优缺点 例如: 回环检测的目的及主要作用 回环检测的实现方法及主要优缺点 回环优化的实现方法及主要优缺点 回环检测的意义前端提供特征点的提取和轨迹、地图的初值,而后端负责对所有这些数据进行优化。然而,如果像视觉里程计那样仅考虑相邻时间上的关键帧,那么,之前产生的误差将不可避免地累积到下一个时刻,使得整个SLAM出现累积误差,长期估计的结果将不可靠,或者说,我们无法构建全局一致的轨迹和地图。 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的邻域窗口。在窗口内随机选取一对(两个)点,比较二者像素的大小,进行如下二进制赋值。![]() ![]() 一种回环检测的思路是:像视觉里程计那样使用特征点来做回环检测,我们对两幅图像的特征点进行匹配,只要匹配数量大于一定值,就认为出现了回环。这种做法存在着一些问题。 词袋,也就是Bag-of-Word(BoW),目的是用“图像上有哪几种特征”来描述一副图像。 确定“人”、“车”、“狗”等概念——对应于BoW中的word,许多单词放在一起,组成了dictionary。确定一副图像中出现了那些在字典中定义的概念——我们用单词出现的情况(或直方图)描述整幅图像。这就把一幅图像转换成一个向量的描述。比较上一步中的描述的相似程度。 字典是固定的,所以只用一个向量就可以描述整幅图像。 字典由很多单词组成,而每一个单词代表了一个概念。一个单词与一个单独的特征点不同,它不是从单幅图像上提取出来的,而是某一类特征的组合。所以,字典生成问题,类似于一个聚类(Clustering)问题。 用k叉树来表示字典。在根节点,用K-means把所有样本聚成k类(实际中为保证聚类均匀性会使用K-means++)。这样就得到了第一层。对第一层的每个节点,把属于该节点的样本再聚成k类,得到下一层。依此类推,最后得到叶子层。叶子层即为所谓的Words。![]() 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点的位置,以及它们在某个相机中的投影位置,求该相机的位姿。
接下来对有相似或重叠的部分的两幅图像进行配准。 特征匹配是利用的汉明距离进行判决: step1:两个特征编码对应bit位上相同元素的个数小于128的,一定不是配对的;step2:一幅图上特征点与另一幅图上特征编码对应bit位上相同元素的个数最多的特征点匹配成一对。 优缺点:优点:计算速度快缺点:1、对噪声敏感(因为二进制编码是通过比较具体像素值来判定的);2、不具备旋转不变性;3、不具备尺度不变性。 PnPRANSACopencv中的一个函数可以用来计算旋转和平移矩阵。 快速重定位FAST_RELOCALIZATION是opencv库中的一个函数。 滑窗优化https://zhuanlan.zhihu.com/p/92100386 https://blog.csdn.net/lovely_yoshino/article/details/123623151 https://blog.csdn.net/qq_41694024/article/details/124955119 https://guyuehome.com/37309 为什么需要滑窗: 在SLAM过程中,随着关键帧和路标点的增多,后端BA的计算效率会不断下降。为了避免这种情况,便使用滑窗(Sliding window)将待优化的关键帧限制在一定的数量来控制BA的规模。 在滑动窗口中,当窗口结构发生改变,状态变量应该如何变化。区别于全局BA,滑窗的窗口结构变化可以分为以下两部分讨论: 在窗口中新增一个关键帧,以及它观测到的路标点;把窗口中的一个旧的关键帧删除,也可能删除它观测到的路标点。新增关键帧的操作和全局BA相同。 删除旧的关键帧:首先要明确的是,在窗口中删除关键帧的技术并不是字面上的直接删除,而是前面提到的边缘化,因为直接丢掉变量,就导致损失了信息,关键帧可能能更多地约束相邻的关键帧,直接丢掉的方式就破坏了这些约束。但是边缘化也带来了一些问题:首先,边缘化会将被边缘化掉的变量所带有的信息转化为先验增加到H矩阵中(Fill-in),这样会破坏BA的稀疏结构;而且,在滑动窗口中使用标准的边缘化操作会导致在新窗口中,与被边缘化的变量相关的变量会在不同的线性化点计算雅可比(因为每次BA的线性化点不同),破坏了系统的一致性。 Fill-in问题 无论是全局BA还是滑窗优化,在进行舒尔消元后,H矩阵不可避免地发生了改变,以边缘化掉某些变量。因此舒尔消元操作在下一次线性化前并没有引入先验信息。但在窗口优化中,被边缘化掉变量后在后续不会再进行更新,因此会对相关块Fill-in,产生先验信息,所以需要额外的操作处理先验块。一致性问题 总的来说,在滑窗边缘化过程中,需要不断迭代计算H矩阵和残差b,而迭代过程中,在窗口中状态变量会被不断更新。但是对于相同状态的两个不同估计被用来计算估计器的雅可比矩阵导致了估计的协方差矩阵掺入了人为的信息,降低了估计的精度,最终导致了一致性问题。作为主流框架的前端中常用的方法,滑窗优化是很常见的迭代策略。因为随着SLAM系统的运行,状态变量规模不断增大,如果使用滑动窗口,只对窗口内的相关变量进行优化便可以大大减小计算量。 而作为滑窗优化,我们除了创建滑动窗口的存储空间外,我们还要通过边缘化的方法保留滑窗外的状态,我们可以不去优化滑窗外的参数,但也不能直接丢掉,这样会破坏原有的约束关系,损失约束信息。采用边缘化的技巧,将约束信息转化为待优化变量的先验分布,实际上是从一个联合分布中获得变量子集概率分布的问题。 操作步骤: vins_estimator/src/factor/marginalization_factor.cpp:边缘化的具体实现;vins_estimator/src/estimator.cpp(部分);函数optimization负责利用边缘化残差构建优化模型函数slidewindoe负责维护滑动窗口 BA&滑动窗口带有相机位姿和空间点的图优化称为BA,它能有效地求解大规模的定位与建图问题。 这在SfM问题中十分有用,但是在SLAM过程中,我们往往需要控制BA的规模,保持计算的实时性。倘若计算能力无限,那不妨每时每刻都计算整个BA——但是那不符合现实需要。现实条件是,我们必须限制后端的计算时间,比如BA规模不能超过1玩个路标点,迭代次数不超过20次、用时不超过0.5秒,等等。像SfM那样用一周时间重建一个城市地图的算法,在SLAM里不见得有效。 控制计算规模的做法有很多,必如从连续的视频中抽出一部分作为关键帧,仅构造关键帧与路标点之间的BA,于是非关键帧只用于定位,对建图则没有贡献。 即便如此,随着时间的流逝,关键帧的数量会越来越多,地图规模也将不断增长。像BA这样的批量优化方法,计算效率会不断下降。为了避免这种情况,我们需要用一定手段控制后端BA的规模。这些手段可以是理论上的,也可以是工程上的。 例如,最简单的控制BA规模的思路,是仅保留离当前时刻最近的N个关键帧,去掉时间上更早的关键帧。于是,我们的BA将被固定在一个时间窗口内,离开这个窗口的则被丢弃。这种方法称为滑动窗口法。 取这N个关键帧的具体方法可以有一些改变,例如,不见得必须取时间上最近的,而可以按照某种原则,取时间上靠近,空间上又可以展开的关键帧,从而保证相机即使在停止不动时,BA的结构也不至于缩成一团(很明显,这容易导致一些糟糕的退化情况)。如果我们在帧与帧的结构上再考虑得深一些,也可以像ORB-SLAM2那样,定义一种称为“共视图”(covisibility graph)的结构。所谓共视图,就是指那些“与现在的相机存在共同观测的关键帧”构成的图。 于是,在BA优化时,我们按照某此原则在共视图内取一些关键帧和路标进行优化。例如,仅优化与当前帧有20个以上共视路标的关键帧,其余部分固定不变。当共视图关系能够正确构造的时候,基于共视图的优化也会在更长时间内保持最优。 滑动窗口也好,共视图也好,大体而言,都是我们对实时计算的某种工程上的折中。 位姿图我们发现特征点在优化问题中占据了绝大部分。实际上,经过若干次观测之后,收敛的特征点位置变化很小,发散的外点则已被剔除。对收敛点再进行优化,似乎是有些费力不讨好的。因此,我们更倾向于在优化几次之后就把特征点固定住,只把它们看作位姿估计的约束,而不再实际地优化它们的位置估计。 沿着这个思路继续思考,我们会想到:是否能够完全不管路标而只管轨迹呢?我们完全可以构建一个只有轨迹的图优化,而位姿节点之间的边,可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是,一旦初始估计完成,我们就不再优化那些路标点的位置,而只关心所有的相机位姿之间的联系。通过这种方式,我们省去了大量的特征点优化的计算,只保留了关键帧的轨迹,从而构建了所谓的位姿图(Pose Graph)。 我们知道,在BA中特征点数量远大于位姿节点。一个关键帧往往关联了数百个关键点,而实时BA的最大计算规模,即使利用稀疏性,在当前的主流CPU上一般也就是几万个点左右。这就限制了SLAM的应用场景。所以,当机器人在更大范围的时间和空间中运动时,必须考虑一些解决方式。要么像滑动窗口法那样,丢弃一些历史数据;要么像位姿图的做法那样,舍弃对路标点的优化,只保留Pose之间的边。此外,如果我们有额外测量Pose的传感器,那么位姿图也是一种常见的融合Pose测量的方法。 位姿图优化中,节点表示相机位姿,而边,则是两个位姿节点之间相对运动的估计,该估计可以来自于特征点法或直接法,也可以来自GPS或IMU积分。 理论上,即使在优化之后,由于每条边给定的观测数据并不一致,误差也不见得近似于零。 了解雅可比求导后,剩下的部分就和普通的图优化一样了。简而言之,所有的位姿顶点和位姿——位姿边构成了一个图优化,本质上是一个最小二乘问题,优化变量为各个顶点的位姿,边来自于位姿观测约束。 滑窗优化作为主流框架的前端中常用的方法,滑窗优化是很常见迭代策略。因为随着SLAM系统的运行,状态变量不断增大,如果使用滑动窗口,只对窗口内的相关变量进行优化便可以大大减小计算量。 而作为滑窗优化,我们除了创建滑动窗口的存储空间外,我们还要通过边缘化的方法保留滑窗外的状态,我们可以不去优化滑窗外的参数,但也不能直接丢掉,这样会破坏原有的约束关系,损失约束信息。采用边缘化的技巧,将约束信息转化为待优化变量的先验分布,实际上一个从联合分布中获得变量子集概率分布的问题。 图优化我们看到,前端视觉里程计能给出一个短时间内的轨迹和地图,但由于不可避免的误差累积,这个地图在长时间内是不准确的。所以,在视觉里程计的基础上,我们还希望构建一个尺度、规模更大的优化问题,以考虑长时间内的最优轨迹和地图。不过,考虑到精度与性能的平衡,实际中存在着许多不同的做法。 21世纪视觉SLAM的一个重要进展是认识到了矩阵H的稀疏结构,并发现该结构可以自然、显式地用图优化来表示。 H矩阵的稀疏性是由雅可比矩阵***J(x)***引起的。H矩阵中的非对角部分的非零矩阵块可以理解为其对应的两个变量之间存在联系,或者可以称之为约束。于是,我们发现图优化结构与增量方程的稀疏性存在着明显的联系。 鲁棒核函数在前面的BA问题中,我们将最小化误差项的二范数平方和作为目标函数。这种直观的做法,存在一个严重的问题:如果出于误匹配等原因,某个误差项给的数据是错误的,我们会把一条原本不应该加到图中的边给加进去,然而优化算法并不能辨别出这是个错误的数据,它会把所有的数据都当作误差来处理。在算法看来,这相当于我们突然观测到了一次很不可能产生的数据。这时,在图优化中会有一条误差很大的边,它的梯度也很大,意味着调整与它相关的变量会使目标函数下降更多。所以,算法将试图优先调整这条边所连接的节点的估计值,使它们顺应这条边的无理要求。由于这条边的误差真的很大,往往会抹平其他正确边的影响,使优化算法专注于调整一个错误的值。 出现这种问题的原因是,当误差很大时,二范数增长得太快。于是就有了核函数的存在。核函数保证每条边的误差都不会大得没边而掩盖其他的边。具体的方式是,把原先误差的二范数度量替换成一个增长没有那么快的函数,同时保证自己的光滑性质(不然无法求导)。因为它们使得整个优化结果更为稳健,所以又叫它们鲁棒核函数(Robust Kernel)。 滤波和优化的区别
所谓的Bundle Adjustment(BA),是指从视觉图像中提炼出最优的3D模型和相机参数(内参数和外参数)。考虑从任意特征点发射出来的几束光线(bundles of light rays),它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整(adjustment)各相机姿态和各特征点的空间位置,使得这些光线最终收束到相机的光心,就称为BA。 代价函数中的误差项eij描述ki看到pj这件事。 VIO视觉惯性里程计 visual-inertial odometry 一致性的含义A recursive estimator is termed consistent when the state estimation errors are zero mean, and their covariance equals the one reported by the estimator. |
CopyRight 2018-2019 实验室设备网 版权所有 |