cartographer 前端两个方法：相干性匹配与非线性优化；以及回环检测方法：利用了分枝定界的相干性匹配
文章目录
- 1.前端
- 1.1 cartographer 两种前端
- 1.2 输出结果
- 2 回环检测
- 2.2 关于分枝定界法的课件
- 2.3 分枝定界对“地图数据”进行相干性匹配CS加速
- 2.3.1 具体实现
- 2.3.1.1 构建问题
- 2.3.1.2 进行分枝
- 2.3.1.3 进行定界
- 2.3.2 计算出格子评分的上界：激光点查其所有hit地图点的评分再求和
- 回环优化 Sparse Pose Adjustment
1.前端
SLAM，那么通常都会分为前端和后端。前端充当里程计的角色，比如我们熟知的VO（视觉里程计）和VIO（视觉惯性里程计）。激光SLAM也需要前端，对于每一帧点云，通常是使用所谓的扫描匹配（scan-match）方法进行位姿估计。
简单地说，扫描匹配就是想办法把两帧点云对齐，对齐过程中的旋转和平移就是两帧之间的相对位姿。这是最最简单的方式，实际中，为了提高鲁棒性，通常把当前帧点云和地图进行扫描匹配，更不容易出错。
扫描匹配的实现方式有两种，第一种是相关性扫描匹配，第二种是基于优化的扫描匹配。
1.1 cartographer 两种前端
- 相关性扫描匹配
- ceres 优化
相关性扫描的代码：
double RealTimeCorrelativeScanMatcher2D::Match(const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud, const Grid2D& grid,transform::Rigid2d* pose_estimate)步骤1：// 解析初始位姿的角度步骤2：/将激光点云转到初始位姿角度(零时刻角度为0)
步骤3：//确定查询窗口（角度+空间平面）
步骤4：// 点云对各个角度旋转 （角度范围 ×2 /角度分辨率 ）
步骤5：//生成平移的候选值， 只记录 x,y的偏移
步骤6：//遍历候选值x,y ,得到点云。再遍历激光点求取得分（点云与地图重合数量）
步骤7：// 取得分最大的候选值的
步骤8：//遍历各个角度
步骤9：//输出最优位姿 + 偏移量
结构图
ceres 优化代码
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud,const Grid2D& grid,transform::Rigid2d* const pose_estimate,ceres::Solver::Summary* const summary)
步骤1：构建观测残差 栅格不连续，三次卷积插值 使其连续可导步骤2：构建平移变化量残差
步骤3：构建旋转变化量残差
步骤4：ceres 求解，得pose+cost
1、实际中，通常是两种方法同时用，先用相关性扫描匹配求初值，再用基于优化的扫描匹配进一步优化。
2、如果由IMU，可以利用IMU估计初值
1.2 输出结果
我们生成了一系列的submap地图。
接下来我们需要进行回环检测，消除前端误差
2 回环检测
后端中使用了相干性匹配进行回环检测
分枝定界
2.2 关于分枝定界法的课件
https://doc.mbalib.com/view/5de9ab034e0b398fe51ecfdb60dea2d8.html
2.3 分枝定界对“地图数据”进行相干性匹配CS加速
分枝定界一般用于搜索离散空间的最优解。
我们这里的解空间显然就是一个离散的空间，而且可以很方便地按照地图分辨率构造树形结构
假设有一张4×4的地图，我们降低其分辨率，每2×2的格子合并成一个，得到一张2×2的地图。地图被分成了2层，如下图所示。
有了多分辨率地图后，我们先在第二层的解空间中搜索，找到最优的位姿对应的格子。然后再在该格子中搜索下一层中对应的小格子，找到最优的位姿。
需要注意一般情况下：
首先，在第二层的格子中搜索最优的位姿，实际上是把位姿放在格子的中心，计算其点云的评分（即hit点的评分之和，下文直接称其为位姿的评分）。但是格子中心的位姿并不能代表格子其它位置的位姿，很有可能格子中心的评分低于其它位置的评分，那么第二层最优的格子中并不一定包含第一层最优的格子。怎么办呢，我们可以想办法让格子中心的评分高于其所有子格子的评分，此时格子的评分是其子格子的评分上界，这样就可以保证子格子的最高评分体现在父格子中。
虽然格子的评分是其子格子的评分上界，但并非上确界。也就是说，可能存在格子的评分很高，但其所有子格子的评分都很低的情况。这样的话，我们仍然无法选取最大评分格子的同时抛弃其它格子。
因此：
- 绝不抛弃任何一个可能存在最优解的格子，只抛弃那些绝对不存在最优解的格子。
2.3.1 具体实现
2.3.1.1 构建问题
激光点和地图点进行hit，计算最高得分
2.3.1.2 进行分枝
具体实现的时候，先从根节点开始遍历，把它拆分成4个子格子。这4个子格子分别计算评分，并由高到低排序。从中选出分数最高的格子，进一步拆分成4个子格子。假设这4个子格子已经到了叶子节点，也就是达到了真实地图的分辨率。此时计算出的4个子格子的评分代表了真实的位姿评分（只有叶子节点的评分是真实评分，其它格子的评分都是上界），找出其最大值，记作best_score。
以上过程，我们体会了分枝是如何实现的。
2.3.1.3 进行定界
接下来，该轮到定界出场了。第二层的格子还有3个未曾探索，我们当然是选择最大的那个开始分支。但别急，先看一下它（其他3个父格子）的评分是否大于best_score，如果是，继续分支，如果否，就可以直接剪枝了，抛弃这个格子及其所有子格子。因为格子的评分代表了其子格子评分的上界，如果上界都小于best_score，就不可能再有子格子的评分大于它了。按照如此方法，大刀阔斧地剪枝即可。注意，当遇到评分大于best_score的叶子节点时，记得更新best_score，这样可以更快地缩小搜索空间。
讲到这里，你是不是有点摸不着头脑？别怕，因为这里有个最最关键的问题我还没有解释，也就是如何能够快速计算出格子评分的上界。
2.3.2 计算出格子评分的上界：激光点查其所有hit地图点的评分再求和
先说一个平凡方法。既然要算子格子评分的上界，那就把每一个子格子拿出来算一遍，求个最大值。对了，子格子还会有子格子，所以这是个递归运算，直到把叶子节点都算出来才行。那这跟直接暴力搜索就没什么两样了，不行不行。
我们还是得追求在O(1)时间复杂度内求出格子的评分。想了想，之所以叶子节点评分可以在O(1)时间内求出，是因为它是直接在地图中查表得到的（严谨地说，是通过查其所有hit点的评分再求和得到的，但由于hit点数量固定，可以认为时间复杂度是O(1)）。但我们构造的** precomputed grids 预计算的地图能不能也维护类似的概率表格**，使得格子的评分可以在对应分辨率地图中查询得到呢？
补充：之前一直没有弄清楚 precomputed grids
- 地图中每个格子的概率用其附近区域内的最大概率代替
- 直接查对应分辨率地图就可以得到格子的评分
- 不同 precomputed grids地图，其实大小是一样的。例如submap是600×600， precomputed grids的map也是600×600，不会降采样尺寸
多分辨率的地图的
不同precomputed grids的地图需要事先计算好：
补充：
作者提到了一篇论文：
《M3RSM: Many-to-Many Multi-Resolution Scan Matching》
用金字塔采样得到不同分辨率的precomputer grids。
作者的态度是我们更喜欢用自己提出的方法而不是使用M3RSM较低分辨率的概率网格，这会导致更糟糕的界限，从而对性能产生负面影响。
回环优化 Sparse Pose Adjustment
检测到回环后，将回环的姿态加入到位姿图中，进行地图图优化
参考：Sparse pose adjustment for 2D mapping
每隔几秒钟 , 我们就会使用 Ceres [ 14 ] 来计算
正文结束
20211110
第一次看这篇论文是18年年底，过了整整三年，又来补充一次内容了
cartographer 前端两个方法：相干性匹配与非线性优化；以及回环检测方法：利用了分枝定界的相干性匹配相关推荐
- 综述 | SLAM回环检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨任旭倩 来源丨计算机视觉life 在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一 ...
- 最全综述 | SLAM中回环检测方法 收藏
在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累积误差.一个消除误差有效的办法是进行回环检测.回环检测判断 ...
- Iris: 比ScanContext更加精确高效的激光回环检测方法（IROS 2020）
论文阅读<LiDAR Iris for Loop-Closure Detection>(IROS 2020 Oral) 代码已开源: https://github.com/JoestarK ...
- 3D激光SLAM的回环检测方法
一.Scan-to-Scan 1.格地图或者说是概率占据栅格地图初步 在Matlab中创建一个1000*1000大小的二维数组,如果数组中每个元素取值为0到255的整数(灰度图),再把这个数组用Mat ...
- SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
- Cartographer分枝定界算法比喻理解
Cartographer的代码非常紧凑,为了加深自己的理解和印象,尝试用比喻的方法去描述出来,欢迎大家交流指正. BnB比喻: 一家公司,有多个职位等级(对应多分辨率地图),现在要评项目奖金. 符合以 ...
- 【cartographer】（2）分枝定界算法
分枝定界方法 分枝定界法(branch and bound)是一种求解整数规划问题的最常用算法,是一种搜索与迭代的方法. 通俗说法: 分枝定界算法始终围绕着一颗搜索树进行的,主要流程就是分枝+定界. ...
- matlab基于ssd的角点匹配_基于关键点的目标检测
0 1 前言:基于锚点的目标检测方法 在基于关键点(key points)的目标检测方法出现之前,主流目标检测方法一般先设置一些预先定义好的 锚点 (anchor boxes). 作为预测物体框的参考 ...
- 国科大提出FreeAnchor，新一代通用目标检测方法，代码已开源
点击我爱计算机视觉标星,更快获取CVML新技术 本文转载自新智元(ID:AI_era),请勿二次转载. 新智元专栏 作者:张小松 (中国科学院大学) [新智元导读]中国科学院大学联合厦门大学和 ...
- 作为SLAM中最常用的闭环检测方法，视觉词袋模型技术详解来了
摘自:https://mp.weixin.qq.com/s/OZnnuA31tEaVt0vnDOy5hQ 作为SLAM中最常用的闭环检测方法,视觉词袋模型技术详解来了 原创 小翼 飞思实验室 今天 基 ...
最新文章
- 基于maven的ssh框架一步一步搭建（一）
- java的语法结构_Java中的语法规范语法结构
- JVM 学习四：类加载之双亲委派机制与沙箱安全机制
- 源码-0105-Autoresizing
- java21天打卡 day10-字符串2
- 用c语言计算1+2+3+......+100
- Logger.getLogger和LogFactory.getLog的区别
- G.8032 ERPS 轻松强固网络安全！
- C#中手动引用COM组建的例子
- java applet插件下载_Java Applet.zip
- Python三种设计模式
- 如何用计算机算cos角度,如何将cosα值换算成角度
- DP算法——打家劫舍系列
- 微信查询四六级成绩代码
- python3 输入一个数然后输出他的平方_汇编实验（输入一个数输出其平方值）
- 大数据时代带来的伦理问题
- 科学计数法 与 普通数字 转换
- 【语音识别】基于MFCC的小波变换DTW实现说话人识别matlab代码
- 时序预测 | MATLAB实现PSO-SVM粒子群优化支持向量机时间序列预测
- JavaSE进阶-day01思维导图,每日总结