【转】【泡泡机器人原创专栏-Cartographer】Cartographer理论及实现浅析

转自【泡泡机器人SLAM】公众号原创专栏:http://mp.weixin.qq.com/s/LdbFp-Zvkr02-_25ILb16g

【作者博客】http://remyspot.blog.51cto.com/

本文概述了Cartographer的理论及简要的梳理了其开源实现代码的逻辑,目的在于起到抛砖引玉的作用,为选择性的研究相关理论及实现提供参考。

1.Cartographer理论概述

Cartographer主要理论是通过闭环检测来消除构图过程中产生的累积误差[1]。用于闭环检测的基本单元是submap。一个submap是由一定数量的laser scan构成。将一个laser scan插入其对应的submap时,会基于submap已有的laser scan及其它传感器数据估计其在该submap中的最佳位置。submap的创建在短时间内的误差累积被认为是足够小的。然而随着时间推移,越来越多的submap被创建后,submap间的误差累积则会越来越大。因此需要通过闭环检测适当的优化这些submap的位姿进而消除这些累积误差,这就将问题转化成一个位姿优化问题。当一个submap的构建完成时,也就是不会再有新的laser scan插入到该submap时,该submap就会加入到闭环检测中。闭环检测会考虑所有的已完成创建的submap。当一个新的laser scan加入到地图中时,如果该laser scan的估计位姿与地图中某个submap的某个laser scan的位姿比较接近的话,那么通过某种 scan match策略就会找到该闭环。Cartographer中的scan match策略通过在新加入地图的laser scan的估计位姿附近取一个窗口,进而在该窗口内寻找该laser scan的一个可能的匹配,如果找到了一个足够好的匹配,则会将该匹配的闭环约束加入到位姿优化问题中。Cartographer的重点内容就是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略的实现。

2.开源代码逻辑

Google开源的代码包含两个部分:cartographer[2]和cartographer_ros[3]。cartographer主要负责处理来自雷达、IMU和里程计的数据并基于这些数据进行地图的构建,是cartographer理论的底层实现。cartographer_ros则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理,与此同时也将cartographer的处理结果发布用于显示或保存,是基于cartographer的上层应用。

3.cartographer代码结构

common:定义了基本数据结构以及一些工具的使用接口。

sensor:定义了雷达数据及点云等相关的数据结构。

transform:定义了位姿的数据结构及其相关的转换。

kalman_filter: 主要通过kalman滤波器完成对IMU、里程计及基于雷达数据的估计位姿的融合,进而估计新进的laser scan的位姿。

mapping:定义了上层应用的调用接口以及局部submap构建和基于闭环检测的位姿优化等的接口。

mapping_2d和mapping_3d:对mapping接口的不同实现。

4.mapping_2d代码逻辑

4.1 cartographer::mapping_2d:: GlobalTrajectoryBuilder

cartographer::mapping_2d::GlobalTrajectoryBuilder类主要实现了接收处理上层应用传递的传感器数据的主要接口:

(1)AddImuData用于接收处理上层应用传递的IMU数据。

(2)AddOdometerPose用于接收处理上层应用传递的里程计数据。

(3)AddHorizontalLaserFan用于接收处理上层应用传递的雷达数据。

其中包含重要的对象成员:

(1)cartographer::mapping_2d::LocalTrajectoryBuilder类的对象local_trajectory_builder_用于完成局部submap的构建。

(2)cartographer::mapping_2d::SparsePoseGraph类的对象sparse_pose_graph_用于完成闭环检测及全局位姿优化。

在AddImuData和AddOdometerPose函数的实现中会将接收的相应传感器数据传递给local_trajectory_builder_对象处理。在AddHorizontalLaserFan函数的实现中则将新进的laser fan传递给local_trajector_builder_对象用于局部submap构建,如果该laser fan被成功插入到某个submap,那么该laser fan被插入后的相关信息则被传递给sparse_pose_graph_对象用于基于闭环检测的全局位姿优化。

4.2 cartographer::mapping_2d::LocalTrajectoryBuilder

cartographer::mapping_2d::LocalTrajectoryBuilder类主要完成局部submap的构建。其提供了接收处理传感器数据的public函数:

(1)AddImuData用于处理IMU数据。

(2)AddOdometerPose用于处理里程计数据。

(3)AddHorizontalLaserFan用于处理雷达数据。

以及包含了一些重要的private成员:

(1)ScanMatch成员函数基于submap已有的laser fan估计当前laser fan在submap中的位置。

(2)cartographer::kalman_filter::PoseTracker类的对象

pose_tracker_用于融合基于雷达数据的laser fan的局部估计位姿、IMU数据以及里程计数据,进而估计出较优的laser fan的位姿。

在AddImuData和AddOdometerPose函数中会将IMU数据和里程计数据传递给pose_tracker_进行处理。pose_tracker通过UKF不断融合IMU和里程计数据进而更新当前位姿,因此通过pose_tracker可以获取当前laser fan的估计位姿的一个较好的初始化值。进一步的,在AddHorizontalLaserFan函数中会调用ScanMatch,ScanMatch函数中通过在submap中局部匹配得到的当前laser fan的估计位姿被pose_tracker_用来调整该laser fan的初始化值。这样pose_tracker_通过融合多传感器数据,进而能够估计出较优的laser fan的位姿。

4.3 cartographer::mapping_2d::SparsePoseGraph

cartographer::mapping_2d::SparsePoseGraph类主要完成基于闭环检测的全局位姿优化。其提供了接收处理新进被插入到submap的laser fan相关信息的public函数:

(1)AddScan 对新进的laser fan进行闭环检测及在适当的时候进行全局优化。

以及一些重要的私有成员:

(1)ComputeConstraintsForScan对新近laser fan信息进行处理并启动闭环检测scan match以及计算其约束,进而将约束添加到位姿优化目标中。

(2)AddWorkItem将laser fan与ComputeConstraintsForScan绑定,并将任务加入到队列中。

(3)HandleScanQueue依此调度队列中的任务。

(4)sparse_pose_graph::ConstraintBuilder constraint_builder_ 用于完成laser fan的scan match以及约束计算。

(5)RunOptimization优化目标。

在AddScan函数中会将laser fan相关信息与ComputeConstraintsForScan函数绑定,并将绑定好的任务通过AddWorkItem函数加入到队列中。HandleScanQueue函数则依次调度队列中的任务。第一次调用AddWorkItem时会直接启动ComputeConstraintsForScan任务,且在第一次ComputeConstraintsForScan任务时启动HandleScanQueue调度。在ComputeConstrainsScan中,通过constraint_builder_对象完成闭环检测的scan match以及约束计算。当所有约束计算完成时,则会进行RunOptimization优化目标。

4.4 Scan Match

LocalTrajectoryBuilder中的scan match策略与SparsePoseGraph中的scan match策略是不同的。前者使用scan_matching::RealTimeCorrelativeScanMatcher,后者则使用scan_matching::FastCorrelativeScanMatcher。二者的目标优化均是由scan_matching::CeresScanMatcher完成。

5.总结

要将Cartographer的原理及实现详细地讲解清楚并不是短短两千字能完成的。为此,本文概述了Cartographer的理论及简要的梳理了cartographer源码的逻辑,目的在于起到抛砖引玉的作用,进而有利于选择性的研究相关理论及实现。Cartographer的重点内容是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略。重点内容对应的实现是:1)基于UKF的多传感器数据融合对应cartographer/kalman_filter目录下的文件;2)scan match策略对应cartographer/mapping_2d/scan_matching目录下的文件。后续有机会也会对这些重点内容及实现进行详细地梳理。

Reference:

[1] Wolfgang Hess., Damon Kohler., Holger

Rapp., Daniel. Andor. Real-time loop closure in 2D lidar slam. ICRA, 2016.

[2] https://github.com/googlecartographer/cartographer

[3] https://github.com/googlecartographer/cartographer_ros

【作者博客】http://remyspot.blog.51cto.com/

时间: 12-04

【转】【泡泡机器人原创专栏-Cartographer】Cartographer理论及实现浅析的相关文章

Cartographer理论及实现浅析

概述了Cartographer的理论及简要的梳理了其开源实现代码的逻辑,目的在于起到抛砖引玉的作用,为选择性的研究相关理论及实现提供参考. 原文首发在"泡泡机器人SLAM"微信公众号:http://mp.weixin.qq.com/s/LdbFp-Zvkr02-_25ILb16g

谷歌Cartographer学习(2)-原理阐述与源码解析

最近终于写完了毕业论文.想仔细研究下Cartographer.无奈自己学识有限,先看下网上大牛的解析,作一个汇总. 一.泡泡机器人原创专栏-cartographer理论及实践浅析 http://mp.weixin.qq.com/s/LdbFp-Zvkr02-_25ILb16g 二.[SLAM](一)Google Cartographer的初步尝试 三.kint_zhao的博客:http://blog.csdn.net/zyh821351004/article/details/52421005 四

关于ntp时间同步理论及配置参数-20170804

[原创]关于ntp时间同步理论及配置参数20170804 前言 NTP(Network Time Protocol,网络时间协议)是用来使网络中的各个计算机时间同步的一种协议.无论是平时使用的私人计算机还是在工作中搭建的服务器集群,时间的统一性和准确性是十分重要的.它是用来同步网络中各个计算机的时间的协议. 在计算机的世界里,时间非常地重要,例如对于火箭发射这种科研活动,对时间的统一性和准确性要求就非常地高,是按照A这台计算机的时间,还是按照B这台计算机的时间?NTP就是用来解决这个问题的,NT

SLAM学习资料汇总

转自 http://www.cnblogs.com/wenhust/ 书籍: 1.必读经典 Thrun S, Burgard W, Fox D. <Probabilistic robotics>[M]. Cambridge, USA: MIT Press, 2005 <Principles of Robot Motion Theory,Algorithms and Implementation> 2.有很多期,跟着会议一起出的文集 <Robotics: Science and

前辈们的话--大疆技术总监的金玉良言

今天在发烧友看到一篇很好的文章,有很大的启发和震撼,这就是别人一毕业就能进大疆,而我们还在为找工作发愁的原因吗?!! 当然文中说的很多东西都不是我们大多数人都能实现的,人家大一就开始接触嵌入式,入手Linux,学习python,而大一的很多人还沉浸在高考后的喜悦,开学后的迷茫中呢!这就是差距. 还是那句话,兴趣是最好的老师.要是 一开始我们就对机器人,电子技术感兴趣的话,这些其实都不用别人说的,自己就会想尽办法去寻找合适的学习路径. 遇山开山,遇水架桥.兴趣会指导我们找到该找到的一切的.所以,还

ORB-SLAM2初步(源码逻辑分析)

今天主要是梳理一下ORB-SLAM2源码的逻辑关系,GitHub和泡泡机器人上有很好的注释版本(吴博),大神请(轻)板砖. 一.文件 如图所示,Examples里面存放的分别是基于单目.双目.RGBD的实例程序,一般都是基于TUM等数据库,还有一个ROS版本的ORB-SLAM2,以及一个应用与AR的Demo程序: include文件夹存放的是头文件,ORB-SLAM2可以被当作一个库来使用,很多函数都可以直接调用: src文件夹存放的是和include对应的源文件,包括主要的Tracking.L

Google开源SLAM软件cartographer中使用的UKF滤波器解析

在Google开源SLAM软件cartographer中,相对<SLAM for dummies>使用了更为复杂.性能更好的Scan匹配与UKF算法,这里简单介绍下cartographer中使用的UKF算法. (一)滤波器参数设定 constexpr static FloatType kAlpha = 1e-3; constexpr static FloatType kKappa = 0.; constexpr static FloatType kBeta = 2.; constexpr st

Cartographer ROS for Turtlebots 初探

今天抽了会时间安装了Cartographer  ROS for Turtlebots 系统版本: Ubuntu 14.04 (Trusty) with ROS Indigo,系统是用的TurtleBot ISO with the following link and create startup USB disk using Startup Disk Creator or similar tools. ubuntu-14.04.2-desktop-amd64-turtlebot-RC0.iso,

谷歌Cartographer学习(1)-快速安装测试

个人理解错误的地方还请不吝赐教,转载请标明出处:http://www.cnblogs.com/hitcm/   代码放到个人github上,https://github.com/hitcm/ 如下,需要安装3个软件包,ceres solver.cartographer和cartographer_ros 1.首先安装ceres solver,选择的版本是1.11,路径随意. git clone https://github.com/hitcm/ceres-solver-1.11.0.git cd

【原创】IPSecVPN理论干货送上,大神勿扰

众所周知,无论是复杂深奥的理论干货,还是晦涩难懂的语言文字,不管对错与否,只是因为我们没有达到那种高度,不能转化为属于自己的东西罢了,于是真正能懂的人还是很少,而且每个人对懂的定义不一样,只能细细去体会,慢慢去品味,只要有着自己的理解角度就好,下面我就以简单的IPSecVPN理论入手,让大家去体验一下外行人中是如何入门的,菜鸟路过,大神勿扰! IPSecVPN两个阶段的协商过程:    第一阶段:    有主模式和积极模式2种.    通过协商让IKE对等体彼此验证对方并确定会话密钥,这个阶段用