Cartographer 源码解析1.1 —— 算法整体结构
*转载请注明出处 @梦凝小筑
1.1 Cartographer 整体结构
解析：
（1）数据获取（Input Sensor Data）
Input Sensor Data：传感器输入，主要包含——激光雷达数据、底盘odom数据、imu数据、fixed frame pose?
激光雷达数据：2d 扫描点云原生数据 ——> 体素滤波器（Voxel Filter） ——> 自适应体素滤波器 ——> 扫描匹配 (Scan Matching)
底盘odom数据：odom位置信息（包含 x , y , θ ）——> 姿态外推器 (PoseExtrapolator) ——> 扫描匹配
imu数据：imu数据（包含 两个方向的线加速度，角速度）——> 预处理 （ImuTracker）——> 姿态外推器 ——>扫描匹配
体素滤波器（Voxel Filter）：
Voxel Grid 滤波器[1]对点云进行降采样, 即减少点云的数量规模, 同时保持点云的障碍物特征。
基本思路是将点云数据划分在不同的体素栅格内，用体素栅格内所有点的重心来表示该栅格的环境数据。对于一个含有N个点的二维体素, 经过Voxel Grid滤波器处理后得到表示环境数据的点的计算公式为
姿态外推器 (PoseExtrapolator)：
用于给扫描匹配 (Scan Matching)提供初值。
输入：odom、imu、上一次前端匹配完成的 last_pose
输出：下一次前端匹配的初始pose
处理思路：
odom和old_odom 计算 （x , y , θ ）增量 ——>线速度、角速度
imu 预积分——>线速度、角速度
last_pose 和 old_pose 计算（x , y , θ ）增量 ——>线速度、角速度
估算出下一次前端匹配的初始pose，估算方法：
对于角度姿态变化，更信任imu数据，如果有imu数据则使用imu数据
对于位移姿态变化，更信任odom数据，如果有odom数据则使用odom数据
若没有，则使用 last_pose计算出的数据
在短时间内（sick liadar的频率是50hz,20ms）imu提供的角度增量，和odom提供的位移增量理论上是很准的，这和这些传感器的特性有关。
（2）局部建图（Local SLAM）
若不清楚cartographer原理的，建议查看 Google Cartographer SLAM 原理
Local SLAM：外推器初始pose ——> 扫描匹配 ——>运动滤波器（Motion Filter）——>插入子图（submap）中
扫描匹配 (Scan Matching)：
计算出，当前时刻相对于之前的一段时间，所应存在的最优位姿
输入：外推器初始pose
输出：扫描匹配最优位姿
处理思路：非线性优化问题，建立最小二乘问题，该最小二乘问题在cartogrper中通过google自家的Ceres库进行求解
运动滤波器（Motion Filter）：
为避免每个子图插入太多扫描帧（scan）数据，一旦扫描匹配器生成了新的pose，计算这次pose与last_pose 的变化量， 调用Motion Filter， 如果这次的变化不够重要（或者说变化很小），则扫描将被删除。当pose的变化大于某个距离，角度或时间阈值时，才会将scan插入到当前子图中。
motion_filter = {max_time_seconds = 5.,max_distance_meters = 0.2,max_angle_radians = math.rad(1.),},子图（submap）：
Cartographer在前端匹配环节区别与其它建图算法的主要是使用了Submap这一概念，每当或得一次 scan的数据后，便与当前最近建立的Submap去进行扫描匹配，使这一帧的 scan数据插入到Submap上最优的位置。在不断插入新数据帧的同时该Submap也得到了更新。一定量的数据组合成为一个Submap，当不再有新的scan插入到Submap时，就认为这个submap已经创建完成，接着会去创建下一个submap
一个submap是通过几个连续的scan创建而成的，由5cm*5cm大小的概率栅格 构造而成
（3）全局优化（Gloab SLAM）
通过scan matching得到的位姿估计在短时间内是可靠的，但是长时间会有累积误差。因此Cartographer应用了回环检测对累积误差进行优化(全局优化)。
Gloab SLAM：插入子图（submap）的扫描帧（scan）& 之前已创建完成的submap中所有（scan）——>回环检测（这里用优化算法分支定界（branch and bound））——>计算约束（Compute Constraints）——>位姿优化
回环检测：所有创建完成的submap以及当前的laser scan都会用作回环检测的matching。如果当前的scan和所有已创建完成的submap在距离上足够近，那么通过某种 match 策略就会找到该闭环。这里为了减少计算量，提高实时回环检测的效率，Cartographer应用了branch and bound(分支定界)优化方法进行优化搜索。
分支定界（branch and bound）： 一种优化搜索方法，效率优于暴力匹配法。详细介绍参见Google Cartographer SLAM 原理
位姿优化：如果得到一个足够好的匹配，到此处，回环检测部分已经结束了，已经检测到了回环得存在。接下来要根据当前scan的位姿和匹配到得最接近的submap中的某一个位姿来对所有的submap中的位姿进行优化，即使残差E最小。回环检测与回环优化过程中scan和submap的关系如图所示：
回环的优化问题同样为非线性最小二乘问题，该最小二乘问题在cartogrper中通过google自家的Ceres库进行求解 。
总结：相对于其它slam算法，有以下不同
cartographer 引入了子图（submap）的概念，使前端匹配建立在当前帧与当前创建的submap(可理解为在这之前的一系列scan)进行匹配。
采用了回环检测，并利用 branch and bound 进行优化搜索，提高效率，达到实时回环的作用。
笔者理解并不一定正确，仅供参考，如有错误，欢迎指正。
Cartographer 源码解析1.1 —— 算法整体结构相关推荐
- cartographer 源码解析 (五)
相关链接: cartographer 源码解析(一) cartographer 源码解析(二) cartographer 源码解析(三) cartographer 源码解析(四) cartograph ...
- 【cartographer源码解析--外推器】
cartographer源码解析–外推器 文章目录 cartographer源码解析--外推器 前言 一.cartographer中的PoseExtrapolator类 二.接受数据,并分析处理逻辑 ...
- 球球大作战c语言源代码,《球球大作战》源码解析：移动算法
系列文章 <球球大作战>源码解析(7):游戏循环 <球球大作战>源码解析(8):消息广播 服务端处理了游戏的各种游戏逻辑,怎样让小球移动是重点之一.若想做服务端运算的游戏,这一 ...
- cartographer源码解析（二）node_main.cc文件详解
描述 cartographer学习笔记(二) 这一篇主要分析node_main.cc的代码 主函数main() 主函数 int main(int argc, char** argv) {google: ...
- Cartographer源码解析——位姿推测器PoseExtrapolator
这一章主要了解外推器,上一章讲到激光点云畸变的矫正,其中最关键的是推测估计出一帧点云中每一个激光点所对应的机器人的位姿.这里就用到了外推器去推测每个发射点的位置. 具体代码调用如下: // Step: ...
- 以太坊Geth 共识算法源码解析
共识算法 目前以太坊中有两个公式算法的实现,分别为clique和ethash.其中clique是PoA共识的实现,ethash是PoW共识的实现,其相应的代码位于go-ethereum/consens ...
- (02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录 ...
- (02)Cartographer源码无死角解析-(48) 2D点云扫描匹配→扫描匹配基本原理讲解，代码总体框架梳理AddAccumulatedRangeData()
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录 ...
- CTPN源码解析5-文本线构造算法构造文本行
文本检测算法一:CTPN CTPN源码解析1-数据预处理split_label.py CTPN源码解析2-代码整体结构和框架 CTPN源码解析3.1-model()函数解析 CTPN源码解析3.2-l ...
- weiler-atherton多边形裁剪算法_EAST算法超详细源码解析：数据预处理与标签生成...
作者简介 CW,广东深圳人,毕业于中山大学(SYSU)数据科学与计算机学院,毕业后就业于腾讯计算机系统有限公司技术工程与事业群(TEG)从事Devops工作,期间在AI LAB实习过,实操过道路交通元 ...
最新文章
- shell编程基础之基本文本工具集合
- Oracle数据库链Database links
- 分析手机网站的优势思维结构图_写了100多篇原创文章，我常用的在线工具网站推荐给大家...
- Delphi之面向对象的界面复用技术
- 浏览器阻止window.open的解决方案
- 安卓手机系统可删除的自带软件大集合
- 前端学习（2854）：简单秒杀系统学习之settimeout
- java spring入门小程序_springMVC学习笔记(二)-----注解和非注解入门小程序
- DataList嵌套
- matlab 辅助函数 —— 文件下载与文件解压
- linux发行版本中 图形方式的运行级,在大多数 Linux 发行版本中，图形方式的运行级定义为？...
- 显微镜自动聚焦原理是什么_共聚焦显微镜
- Texlive安装宏包
- 微信认证小程序如何绑定微信支付商户平台
- ALSA 中 hw 和 plughw 的区别
- win10重装系统后出现的0xc0000225问题解决
- stm32f407的串口通信传输不了且串口调试软件无法显示的问题
- Kindle：电子书资源
- 用Java实现简单的井字棋程序（α-β剪枝）
- OpenSUSE安装配置TeXLive2015.iso、前端软件Kile、Lyx、TeXstudio及TeX的更新管理界面的使用