1 介绍

1.1 概述

• Cartographer是一款可以跨多个平台和传感器配置提供2D和3D实时同步定位和绘图(SLAM)的系统。这个项目提供了Cartographer的ROS集成。
• Cartographer是google推出的一套基于图优化的SLAM算法。Cartographer算法并没有给人惊艳的感觉，但该算法的主要目标是实现低计算资源消耗，达到实时SLAM的目的。
• 该算法主要分为两个部分，第一个部分称为Local SLAM, 该部分通过一帧帧的Laser Scan建立并维护一系列的Submap，而所谓的submap就是一系列的Grid Map。当再有新的Laser Scan中会通过Ceres Scan Matching的方法将其插入到子图中的最佳位置。但是submap会产生误差累积的问题，因此，算法的第二个部分，称为Global SLAM的部分，就是通过Loop Closure来进行闭环检测，来消除累积误差：当一个submap构建完成，也就是不会再有新的laser scan插入到该submap时，算法会将该submap加入到闭环检测中。闭环检测的本质也是一个优化问题，该优化问题被表达成了一个pixel-accurate match的形式，解决优化问题的方法是Branch-and-Bound Approach.
• Cartographer本身是一个C++的库，虽然可以不依赖ROS的环境运行，但为了快速的上手，google还是提供了一个ROS环境下的封装cartographer_ros。
  
  
  

1.2 评价

梅卡曼德（北京）机器人科技有限公司 CEO 邵天兰 评 Cartographer

我认为Cartographer这个库最重要的东西还不是算法，而是实现。
2D/3D的SLAM的核心部分仅仅依赖于以下几个库：

• Boost：准标准的C++库。
• Eigen3： 准标准的线性代数库。
• Lua：非常轻量的脚本语言，主要用来做Configuration
• Ceres：这是Google开源的做非线性优化的库，仅依赖于Lapack和Blas
• Protobuf：这是Google开源的很流行的跨平台通信库

没有PCL，g2o, iSAM, sophus, OpenCV, ROS 等等，所有轮子都是自己造的。这明显不是搞科研的玩儿法，就是奔着产品去的。前面说过，算法需要的计算资源少，而且因为依赖很少，因此几乎可以直接应用在一个产品级的嵌入式系统上。以前学术界出来的开源2D/3D SLAM算法不少，但能几乎直接拿来就用在产品上的，恕我孤陋寡闻还真想不出来。因此，我认为进入相关领域SLAM算法的门槛被显著降低了。

1.3 特点

• 工程化代码，依赖少，算法设计精妙，能在嵌入式主控上运行。适合产品级应用；
• 支持 ROS
• 优秀的软件架构设计，便于接入IMU、（单/多线）雷达、里程计、甚至为二维码辅助等视觉识别方式也预留了接口（Landmark）。
• Cartographer支持2D和3D激光雷达的输入，实现机器人定位，并构建栅格地图。
• 2D-SLAM：基于2D栅格地图，可以直接用于导航。
• 3D-SLAM：基于hybridGrid，译为混合概率地图。

2 框架

官方

3 代码结构

heimazaifei 解读

https://www.cnblogs.com/heimazaifei/p/12392231.html

• mapBuilder：实现整个地图构建，包括前端local slam和后端 global slam。
• 轨迹（trajectory): 可以理解为一次SLAM 从起点到终点过程中的机器人行走轨迹，建图中可以通过startTrajectory和finishTrajectory控制。在轨迹生成的过程中，完成sensor到sumap的生成，以及pose_graph的构建。TrajectoryBuilder(globalTrajectory类)主要通过sensor_collator(localTrajecory类)和pose_graph构成 。sensor_collator实现局部地图构建，最终结果传递给pose_graph；
• 节点图(poseGraph)：具体参考图优化的知识。简单理解图优化（如果没接触过图优化，下面估计看不懂，后续会详细讲）：每个插入的激光和生成sumap，以及landmark（后续讲）都可以理解为图优化的一个节点，建图过程中，生成这些点之间的关系，这些关系便是图中的线，最终优化，就是调整点的位置，得到最优值。可以理解为PoseGraph主要实现全局优化（global slam）功能。
• 代码流程：最终代码运行通过ROS node 方式实现。node中对应topic和service订阅和发布等功能通过MapBuilderBridge类实现。MapBuilderBridge顾名思义，实现ROS节点代码和cartographer功能代码之间的桥接，也可以理解为对cartographer主体代码的接口封装。cartographer主体代码主要功能通过构建地图MapBuilder类实现。此外，MapBuilderBridge 还包含sensorBridge类，实现传感信息ros格式和cartographer自定义格式之间的转换，这些传感器主要包括scan,imu,odom,tf 等。

linyicheng 解读

https://github.com/linyicheng1/OpenSLAM-Notes/tree/main/cartographer-master

Xiaotu 解读

https://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=index

cartographer_ros

重点	说明
Cartographer的总体框架与安装试用	本文中，我根据自己的理解解释一下官方的系统框图，然后介绍如何快速的安装试用Cartographer，并简单的分析一下系统运行时的节点和主题。
官方的ROS封装——cartographer_ros	本文中我们简略的查看了cartographer_ros的目录结构，分析了示例demo的launch脚本发现实际与定位建图相关的节点只有cartographer_node， 然后追踪编译系统了解到cartographer_node的入口main函数定义在node_main.cc中。
系统入口——cartographer_node	我们了解到实际与定位建图相关的节点只有cartographer_node， 而其入口main函数则定义在node_main.cc中。 通过对该文件的分析，我们猜测对象node实际控制着系统的业务逻辑，而对象map_builder则用于完成建图。
cartographer_node的外墙——node对象	本文中我们研读与node对象相关的源码，分析其构造函数以及订阅传感器消息的机制，浏览node对象的消息回调函数与服务响应函数。 将会发现ROS系统与Cartographer内核之间的信息交换基本上都是通过对象map_builder_bridge_来完成的。
通往Cartographer的桥梁——map_builder_bridge_	本文中，我们将要分析map_builder_bridge_对象的构造函数和一些重要的成员变量。该对象通过SensorBridge将ROS系统中的传感器数据转换到Cartographer中。
使用SensorBridge转换IMU数据	本文将详细分析ROS系统和Cartographer中IMU数据的表示方式，并研究数据转换函数ToImuData。最后会看到SensorBridge对象直接将转换后的IMU数据喂给了路径跟踪器。

地图构建器map_builder

实际完成地图构建的就是在系统运行之初所构建的map_builder对象。 它是Catographer的顶层对象， 为用户提供了一套统一的接口MapBuilderInterface。 封装了对象pose_graph_和trajectory_builders_来分别完成闭环检测和子图构建。还构建了一个具有固定数量的线程池，用于管理多线程。

重点	说明
初识map_builder	本文中通过对map_builder对象的分析，我们可以看到它用对象pose_graph_在后台完成闭环检测和全局的地图优化，并用trajectory_builders_在前台跟踪运动轨迹完成局部的子图构建。
map_builder的接口实现	本文中我们简单看了一下map_builder对象的接口实现，重点分析了AddTrajectoryBuilder函数，看到实际完成局部建图任务的是一个LocalTrajectoryBuilder2D类型的对象， 而与轨迹规划器相关的类型还有CollatedTrajectoryBuilder、GlobalTrajectoryBuilder、TrajectoryBuilderInterface它们应该说是一种封装。
连接前端与后端的桥梁——GlobalTrajectoryBuilder	我们会在GlobalTrajectoryBuilder的成员变量中，看到了前端和后端的核心LocalTrajectoryBuilder2D和PoseGraph2D。 也将在它处理点云数据的接口中，看到该类是如何把前端的输出结果喂给后端的。

Local SLAM

实际完成Local SLAM的扫描匹配、更新子图的功能是在类LocalTrajectoryBuilder2D中完成的。 扫描匹配负责根据地图信息和激光扫描数据估计机器人的位姿，然后根据位姿估计将传感器数据插入到子图中。不断的重复这两个过程就可以得到一个较为准确的地图， 但是对于比较大的场景每次扫描匹配都对整个地图进行搜索的话，其计算量太大。出于实时性的考虑Cartographer只在一个较小的子图中完成这一更新过程，所以是Local SLAM。 在一些资料中被称为前端。

重点	说明
Local SLAM的核心——LocalTrajectoryBuilder2D	本文中，通过对类LocalTrajectoryBuilder2D的分析，大体上能够找到进行Local SLAM的一些核心要素以及实现这些要素的对象。 走到这里，我们终于看到了算法的一点影子了。
子图的维护与封装	本文中，我们顺着LocalTrajectoryBuilder2D中维护的子图对象active_submaps_，分析其数据类型ActiveSubmaps2D，以及封装子图的类Submap2D。 ActiveSubmaps2D使用了一种类似双缓存的机制，在旧图上进行扫描匹配，同时孕育着新图。
占用栅格的数据结构	子图也是一种占用栅格形式的地图，本文从ProbabilityGrid开始分析占用栅格的数据结构和接口。将会看到Cartographer通过查表的方式更新栅格单元的占用概率。
查找表与占用栅格更新	本文中我们分析插入器对象range_data_inserter_，介绍栅格单元占用概率的更新原理，并详细分析查找表的构建和使用方法。
Local SLAM的业务主线——AddRangeData	本文中我们将详细分析类LocalTrajectoryBuilder2D的成员函数AddRangeData，以及它直接或者间接调用的函数，具体完成了扫描匹配、更新子图的任务。
基于Ceres库的扫描匹配器	本文我们将简单的介绍使用Ceres库的套路。再分析扫描匹配器CeresScanMatcher2D，研究它是如何评价激光扫描数据与栅格地图的匹配程度的。
基于实时相关性分析的扫描匹配器	这个扫描匹配器的作用是初步优化位姿估计器输出的位姿估计，给Ceres匹配器提供一个较好的迭代初值。 由于可以通过配置文件选择关闭它，暂时不详细分析，等整体分析完Cartographer，介绍了闭环检测之后再补上。

Global SLAM

Global SLAM在后端为Cartographer提供闭环检测全局优化的功能。它的主要工作有三个方面：(1) 通过闭环检测构建子图与路径节点之间的约束关系，进而描述成一个位姿图。 (2) 然后通过SPA(Sparse Pose Adjustment)在后台进行优化。(3) 由于这个SPA优化是一个很耗时的过程，期间前端可能会产生新的子图和路径节点，所以在完成一次优化之后应当调整新增的路径节点的位姿估计。

重点	说明
Global SLAM的核心——PoseGraph2D	在Cartographer中的位姿图是由轨迹节点和子图构成的二部图，图中的约束描述的是轨迹节点与子图之间的位置关系。后端优化就是估计轨迹节点与子图在世界坐标系下的位姿， 最小化全局估计与局部估计之间的偏差。本文我们将研究一下PoseGraph2D的成员变量，以及与位姿图相关的一些数据结构。
位姿图的创建与更新	本文我们将详细分析位姿图PoseGraph2D的构造与更新过程。将看到PoseGraph2D通过对象constraint_builder_在后台完成闭环检测构建约束。 它还有一个工作队列来协调后台的任务。
约束构建器——constraint_builder_	本文中我们将详细分析约束构建器constraint_builder_，将看到它是如何通过MaybeAdd-WhenDone调用循环，借助线程池来组织闭环检测和约束的并行计算。
分支定界闭环检测的原理和实现	Cartographer使用类FastCorrelativeScanMatcher2D具体实现了深度优先的分支定界搜索算法，该算法能够高效地进行扫描匹配，计算路径节点与子图之间的约束关系。
后端优化过程	本文我们将回到Global SLAM的系统框图，来了解经过闭环检测构建了子图与路径节点之间的约束之后，都做了些什么工作。 通过简要的分析后端优化求解器的数据结构以及基于Ceres库进行SPA(Sparse Pose Adjustment)的优化方法，来了解后端优化的过程。

赵锴 解读

SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别

地图设计

• Cartographer的地图（map）以子地图（submap）的形式组成。
• 分为前端和后端。 前端：根据帧间匹配算法（scan-match），实时根据激光(scan)来推测累积的scan相对于submap的位姿。 后端：检测回环（发现在已到达的位置附近），修正各个submap之间的位姿。
• 根据代码可以判断，2D和3D基于的是同一套思路，但是在实现上有一定区别。 接下来结合2D和3D部分，对比介绍实现定位和建图的方法。

在介绍定位和建图思路之前，先介绍一下地图的更新方式：

以方格代表地图块，内部存储数据用来表示被占据的概率。
发出一束激光，打到一个障碍物点，被打到的称为hit点，中间连线上的空区域，称为miss点。

2d和3d都是存储的这样的地图。3d相当于是3维的栅格地图。

宏观上：多次观测到，提升其概率。
问题是，如何用公式表达这个“多次观测”来实现“概率提升”？

$odds(p)=\frac{p}{1-p}$
$M_{new}(x)=clamp(odd{s}^{-1}(odds(m_{old}(x)).odds(p_{hit})))$

• p表示占据概率，当p=0.5时，概率比值odds=1，表示占据和空闲各占一半。$odd{s}^{-1}$表示函数逆运算。
• $p_{hit}=0.55$代表该位置被激光打到一次的概率，第一次观测会被直接赋值。
• $M_{new}(x)$表示地图中 x 位置处的概率值。

举例：

• 初始时刻，栅格未知状态，激光第一次打到了位置 x 处，M(x)概率更新为0.55。
• 随后，激光第二次重复打到了同一个位置：
  $odds(p_{hit})=\frac{0.55}{1-0.55}=1.22$,
  $odds(M_{old}(x))=odd{s}_{0.55}=1.22$
  $odds(p_{hit})$ 和 $odds(M_{old}(x))$ 相乘为1.484，再求函数逆运算，恢复出更新后的$M_{new}(x)=0.597$。
• 实际代码中，采用了多种工程技巧加速运算。包括：映射到整数范围，预计算，查表等方法，此处不深入展开了。

匹配方法

scan-scan： 这个意味着利用两帧激光数据（每帧激光束的数目相同），计算二者之间的变换。典型方法：ICP。
scan-map： 利用一帧激光数据和地图数据，找到激光数据在地图中的位置。
map-map： 利用一个子地图数据，在一个更大的地图中找到它合适的位置。

• 2D和3D的前端，Cartographer采用的是scan-map的匹配方法。
• 不管是2D还是3D，首先要有一个初始的位姿，在此基础上进行优化：
• • 有IMU，则采纳其角速度积分作为初始姿态。不信任IMU任何加速度信息。
• • 有里程计，则采纳里程计的线速度积分作为初始平移。
• • 二者都没有，根据之前的运动做一个匀速的假设。
• • 可以看出，cartographer的多传感器融合是一个松耦合，主要依赖激光来定位。IMU和里程计数据并没有被构建到真正优化的目标函数中。

一阶段解算

• 在得到了初始位姿以后，初始位姿要经过第一阶段解算：CSM（Correlation Scan Match 相关扫描匹配）——构建似然场。
• 即对原先的地图map进行一个高斯模糊，让它膨胀一些，然后把激光scan在一个搜索窗口内暴力匹配，计算得分。

注意，这里有两个问题：
1.得分怎么算？
如果scan的点落在障碍物模糊区域内，落的越多，得分越高。
2.地图不是无限大的吗，你怎么保证在搜索窗口里就能找到位姿呢？
因为有初始位姿。误差肯定在一个范围内而不会马上发散到很远，所以可以在一个位姿的窗口内，对位姿进行暴力匹配搜索。（初始位姿估计中，里程计数据不会突然激增；imu的加速度信息会漂移，但是算法对于imu加速度数据选择直接丢弃不看；而根据之前位姿匀速假设也不会飘走）

这时候我们就要考虑：

• 什么是位姿？位置+姿态。
• 对于2D SLAM而言，有三个变量，x，y，yaw角。 对于3D SLAM而言，有x，y，z，roll，pitch，yaw六个变量。
• • 2dslam中，采用三层循环，（最外层为θ，减小sin和cos的频繁计算），对x，y，θ在给定大小的搜索窗口内进行穷举，计算最高得分的x，y，θ作为一阶段解算的输出位姿。
• • 3dslam中，采用六层循环，对x，y，z，roll，pitch，yaw六个变量在搜索窗口内穷举，计算得分最高的作为一阶段解算输出位姿。
• • 很显然，3d-slam的这种方式对于计算资源依赖较大，复杂度达到O(n^6)级别。因此3d-slam的CSM方法，作为一个配置选项，默认是不开启的。当然如果用户机器比较牛逼，也可以选择开启。

二阶段解算

我们可以看出，第一阶段CSM解算中，位姿在其中是一个离散的变量，通过暴力枚举获得输出结果；

但是暴力枚举也是存在分辨率的，例如：如果角度步长设为1度，但如果刚好真正的角度是5.5度，那么CSM只能搜索到5或6度，而无法进一步细化，逐步累积将会造成误差。 因此，引入第二阶段位姿解算：非线性优化。

$E(T)=arg\underset{T}{\min }\sum [1-M(S_i(T)){]}^2$

$Si(T)$ 表示把激光数据 S 用位姿 T 进行转换，
M(x) 表示得到坐标 x 的地图占用概率。

思路：S代表了激光击中障碍物，将激光点在机器人坐标系下的位置，经过T转换到世界坐标系下以后，应该尽可能的落在已有地图的障碍物上。

第二阶段的位姿求解，显然位姿在其中是一个连续的变量，通过梯度下降的方法求解目标函数。
由于地图是离散的，因此需要对地图进行插值处理，使地图也变成一个可以求导的连续变量，这样才能优化前述目标函数。

线性插值：已知数据 (x0, y0) 与 (x1, y1)，要计算 [x0, x1] 区间内某一位置 x 在直线上的y值；

双线性插值本质上就是在两个方向上做线性插值。

双三次插值：更加复杂的插值方式，它能创造出比双线性插值更平滑的图像边缘。使用最近16个点插值。

Cartographer用的应该是这一种 并且采用Ceres自带的双三插值器。

阅读比较了代码，我判断2D和3D对于此部分内容基本相同。

• 2D：三个误差项：位姿转换误差+ 旋转误差+平移误差 ，后二者限制了旋转和平移的修改不能距离初始位姿太大。
• 3D：四个误差项：低分辨率位姿转换误差+ 高分辨率位姿转换误差+旋转误差+平移误差。低分辨率位姿转换误差权重低于高分辨率。
• 旋转和平移的权重也可以在配置文件中调参。

后端

Cartographer 在后端主要寻找回环，并根据建立的约束对所有的sumap进行统一优化。

回环检测目的是：检测当前位置是否曾经来过，即采用当前scan在历史中搜索，确认是否匹配。

为什么要有回环检测呢？原因有二：1. 已有地图时位姿初始化，不知道当前帧初始位姿，也就不清楚在地图中哪个位置，无法做定位。 2.有累积误差，仅靠前端递推，不进行修正的话，地图很容易变形。

因此接下来我们探讨两个问题：1.如何检测回环。2.检测回环后该怎么做。

如何检测回环

检测回环和前端的思路也比较相似，先通过穷举暴力匹配，再通过优化精细修正。

但是，前端的暴力穷举，是在有个初始位姿的基础上在一个小窗口内穷举。

后端重定位，没有初始位姿了，暴力匹配的范围变成了整个地图。

因此必须采用算法加速处理：多分辨率地图+分支定界操作。

假设有一帧激光：

蓝色代表障碍物：

在高分辨率的地图上，四个点命中3个；
在低分辨率的地图上，四个点全部命中。
激光在低分辨率的地图上匹配情况： 代表得分的上界 （再往精细展开，匹配得分只能更低，不能更高）
在高分辨率的地图上匹配情况： 代表得分的下界（ 再往粗略缩放，匹配得分只能更高，不会更低）

分支定界：

• 先把整个地图中的一个区域展开到底（最高分辨率），得到一个匹配分数（得分下界）；
• 然后把其他区域不展开，算匹配分数。（得分上界）
• 如果低分辨率区域的得分上界，还没有已展开到底的高分辨率区域的下界高，这个低分辨率区域就不再展开了，统统丢掉不要。

A图四个命中3个，得分75；
B图四个命中2个，得分50；
那么激光打在子区域A的可能性就要大于B，因此B就无需继续展开成更精细的地图了。

• 2D-slam的思路比较简单
• • 前端：小范围内穷举+非线性优化方法修正位姿。
• • 回环检测：大范围内穷举（利用分支定界加速） +非线性优化修正位姿。
• 3D-slam有所不同。
• • 前端的暴力匹配方法，是直接6层循环暴力枚举的，因此配置文件中默认不开启，而是在初始通过IMU、里程计等预测位姿基础上，直接非线性优化修正位姿。
• • 回环检测：大范围内6个循环穷举+分支定界的话，小范围都嫌慢，大范围更别提。 直接对位姿非线性优化？ 1.没有初值，会算到猴年马月。2.会落入局部最优值。

这块没有论文发表，是我个人的理解，可能在细节上不够准确，这段比较复杂，仅供参考。（欢迎纠错，有错误我将立即更正）

• 首先，Cartographer3D用IMU确定重力方向，让要匹配的点云的 Z 轴方向和重力方向对齐。（Cartographer3D必须使用IMU） 这样之后就无需搜索 roll 和 pitch 两个方向的旋转，仅暴力搜索沿 z 轴旋转的 yaw 角方向即可。这样暴力匹配的范围，就从x，y，z，roll，pitch，yaw 六个维度降为x，y，z，yaw四个维度。
• 相比2Dslam是把点云投影到地图，计算匹配。3d-slam在yaw角搜索维度上，不是直接对庞大的点云进行旋转匹配的，而是通过直方图匹配，设定阈值，先过滤了大量的不匹配yaw角之后，再暴力搜索x，y，z（仍然用分支定界加速）。

1.首先，在z轴方向对点云切成n个片；
2.对每个切片中的点，求解质心；
3.计算每个点，与质心连线，和x轴所成的角度，并依据角度排序。

之后： 1. A点作为参考点：计算AB和x轴的夹角，映射到直方图。 相当于：把点云分类，分成size个类， 分类标准为当前点B与参考点A连线AB和x轴的夹角。 2.再计算一下质心到AB的夹角OBA，作为直方图中这个点对应的权重。

B点被映射到某一个分类中，分类依据为AB和X的夹角；（AB代表“参考点与当前点”，当参考点B距离A很远时，参考点则会被重新选择） B在此分类中的权重，由角OBA的大小决定（变换至-90～90度，主要是想看垂直程度）。

我们可以知道，一圈360度激光，求质心实际就是激光发射器的位置， OB和AB的夹角越垂直，证明一束激光OB，更接近垂直的打到了AB构成的障碍物平面上，反射强度更高，更可信。越不垂直，说明激光OB以侧面的形式打到平面AB上，探测误差可能更大。

回到原先问题上： 刚刚准备在yaw，x，y，z四个维度对当前点云在地图中暴力搜索一个位置初值； 提取直方图的目的，相当于是对当前点云进行一个降维，提取特征。 根据直方图中提取的特征（根据切片上每个点与参考点的直线AB与x轴的夹角分成n个类，类的值是OBA的大小）, 和历史数据进行匹配，筛选掉一批不够阈值的yaw角。

• 2d-slam在yaw角的搜索上，是直接枚举yaw角，投影点云，判断点云和地图匹配程度。
• 3d-slam则是先根据当前时刻的点云，通过直方图的形式计算一个特征，然后枚举依次yaw角，对特征进行旋转，然后和历史特征进行匹配，筛掉一批yaw角，拿剩下的yaw角再进行yaw，x，y，z维度的枚举。

细节问题：
二维激光点云或者三维点云，拥有xy或xyz三个坐标。因此点云可以被旋转。但是一个抽象的特征怎么进行旋转？
答：直方图横坐标代表AB和机器人坐标系x轴的夹角，如果对点云进行旋转，也就代表了对这个夹角进行了旋转。旋转点云，对应其实也就是对整个直方图的横坐标向右平移。其实旋转的本质上仍然是点云。

因此整个流程为：
1.把当前时刻的点云直方图进行旋转，代表得到一系列的候选直方图；
2.候选直方图和地图中各个位置的点云直方图进行匹配，计算余弦距离。（直方图可以用一个向量表示，向量的余弦距离就是向量的夹角。）
3.筛掉未达阈值的候选直方图，其对应的yaw角直接丢掉，不进行x，y，z三个方向上的展开。
4.达到阈值的yaw角，进入深层循环：对点云进行xyz三个方向的平移，和三维栅格地图计算匹配得分，仍然遵循分支定界，计算上界和下界。（仅有高、低两个分辨率）

补充理解（仅供参考）：
1.为什么yaw角上的特征不满足，这个候选位姿就可以丢掉，就不需要再展开x，y，z了？

因为特征的匹配方法对距离不敏感。 特征主要依赖角OBA，以及AB和X轴的夹角。 在BO的延长线上任意一点，对应的特征都完全相同。
2.在直方图匹配中，需要对当前时刻点云提取直方图特征，并和历史地图匹配。然而只有点云才能提取直方图，而Cartographer3D使用的是三维栅格地图。历史地图中的点云从哪里来？

• 根据源码我推测，程序在各个submap中的每个位姿节点里存放了特征直方图。
• Cartographer可以三维建图，但是是以rosbag的形式把传感器数据保存下来，再把地图以二进制文件pbstream保存以后，离线调用assets_writer_backpack_3d.launch文件，需要同时提供传感器数据bag和地图pbstream文件，才能离线三维建图。 （关于这点，详见SLAM学习笔记（十八）3D激光SLAM——Cartographer第一视角点云可视化配置与使用方法（最新） ）
• 因此，我推测代码内部应该把历史点云丢掉了，因此离线点云建图除了需要已经建好栅格地图文件以外，还需要才需要提供原始传感器数据（包含每一时刻获取的点云）。
• 如果想要获取真正的三维栅格地图，应该需要定位并且修改源码，找到数据在代码中的存储位置，把submap中的三维栅格地图从protobuf数据流中修改并解析出来。

检测回环后该怎么做：优化问题

回顾：在Cartographer前端中，不断维护的是scan和submap之间的位姿。整个的map是由一个个submap组成的。

回顾：重定位中的位姿

两个问题：
1.前端后端均采用了暴力枚举的方式，这样做是否科学？（虽然各自以不同方式加速，但是感觉暴力搜索并不是一个聪明的做法）
2.导入一个已知地图，机器人出现在该地图的某个区域，初始位置未知，随后很快确定了自己的位置，在这个重定位的计算过程中，机器人的位姿变化，到底是离散的，还是连续的？

离散：从一个位置跳变到另一个位置； 连续：从一个位置，逐渐平滑的过渡到另一个位置。

假设现在有一个如下的曲线图：

我们可以构成三点共识：
1.已有地图不是无限大的，所以位姿的解空间范围有限；
2.机器人真正的位置处，激光和地图匹配得分最高；
3.实际的曲线可能有着很多的局部最优值。然而，一个局部最优值所在位置，往往和机器人真正的位置天壤地别。

采用随机梯度上升，则大概率落入局部最优值。

因此，必须把整个解空间全都搜索完一遍，才能知道哪个区域上界最高，再从这个区域出发优化，找到真正的最优得分。——采用多分辨率地图，分区域枚举，确定最优解在绿色范围内，再从绿色范围内出发，非线性优化，梯度上升，找到真正的位置。

回答上面的问题：
1.暴力搜索是否科学？从传统求解角度来说，是的。否则会落入局部最优值。但是目前有人也用深度学习的方式去做，主要集中在视觉SLAM中，因为相对于只有距离特征的点云，图像有更多的特征。
2.在重定位过程中位姿变化，是先离散搜索区域，再连续精准确定。

4 论文分析

Real-Time Loop Closure in 2D LIDAR SLAM

算法原理–cartographer–Real-Time Loop Closure in 2D LIDAR SLAM
W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time Loop Closure in 2D LIDAR SLAM, in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016. pp. 1271–1278.

扫地机器人增强位姿融合的Cartographer算法及系统实现

https://www.modb.pro/doc/63227

张亮, 刘智宇, 曹晶瑛, 沈沛意, 蒋得志, 梅林, 朱光明, 苗启广. 扫地机器人增强位姿融合的Cartographer算法及系统实现[J]. 软件学报, 2020, 31(9): 2678-2690.

曹晶瑛：石头科技研发总监？ https://www.linkedin.cn/incareer/in/%E6%99%B6%E7%91%9B-%E6%9B%B9-4b7a48133
其它作者西电的。

5 安装调试

5.1 install with ROS

ROS调测Cartographer，往上教程太多，此处略。

李想–从零开始搭建二维激光SLAM

李想–从零开始搭建二维激光SLAM

雷达对比

2022年，工业AGV使用的一线雷达，国产的导航雷达价格卷到2000级别，避障雷达到1000以下。(from worthsen)

30000lux光强下的点云效果

雷达频率对数据畸变的影响

雷达数据因运动产生畸变的原因：
激光雷达数据不是瞬时获取的，雷达每旋转一定角度获取一个扇面的数据，然后逐次旋转获取一帧数据（360°、270°等）。
当雷达频率太低时，激光雷达的一帧数据会随着雷达的运动逐渐产生偏差。

边建图边旋转，通过生成的地图来判定雷达是否产生了畸变。当雷达数据与地图对应不上，或者是产生的地图出现畸变，就说明雷达数据产生了畸变。









雷达自身的旋转是有方向的，大部分雷达都是逆时针旋转，与ROS中规定的一样，也有少部分雷达是顺时针旋转的，只不过使用起来有点不方便。

ROS中雷达数据格式

ros中激光雷达数据的消息格式

rosmsg show sensor_msgs/LaserScan

std_msgs/Header header	//　数据的消息头
  uint32 seq			//　数据的序号
  time stamp			//　数据的时间戳
  string frame_id		//　数据的坐标系
float32 angle_min		//　雷达数据的起始角度（最小角度）
float32 angle_max		//　雷达数据的终止角度（最大角度）
float32 angle_increment	//　雷达数据的角度分辨率（角度增量）
float32 time_increment	//　雷达数据每个数据点的时间间隔
float32 scan_time		//　当前帧数据与下一帧数据的时间间隔
float32 range_min		//　雷达数据的最小值
float32 range_max		//　雷达数据的最大值
float32[] ranges		//　雷达数据每个点对应的在极坐标系下的距离值
float32[] intensities	//　雷达数据每个点对应的强度值

使用单线雷达实现LIO-SAM中的特征点提取

https://blog.csdn.net/tiancailx/article/details/110502471

对LIO-SAM中特征点提取的部分进行二维激光雷达下的实现。
实现的目的是为了熟悉激光雷达数据的处理方式，体验如何进行激光雷达数据处理，如何进行特征点提取。
LIO-SAM的项目地址为: https://github.com/TixiaoShan/LIO-SAM
LIO-SAM的特征点提取部分与LOAM基本相同,只不过在算曲率值时的具体计算方式稍有不同。

我们首先看一下激光原始数据的样子,如下图所示:

接下来,再看一下经过我们进行特征点提取之后的数据点的样子,如下图所示:

使用PCL进行雷达的消息类型转换

https://blog.csdn.net/tiancailx/article/details/110830731

PCL（Point Cloud Library）里实现了大量的处理点云相关的功能，实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。

栅格地图的构建

https://blog.csdn.net/tiancailx/article/details/112133226

栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义.
ROS的栅格地图：

• 白色代表空闲，也就是可通过区域，其存储的值为 0；
• 黑色代表占用，也就是不可通过区域，其存储的值为 100；
• 灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1．

栅格地图由于其 占用与空闲的表示方法，在ROS中又被称为占用地图．
栅格地图的示意图如下图所示：

ROS中的栅格地图的消息类型：

$ rosmsg show nav_msgs/OccupancyGrid 
std_msgs/Header header          # 数据的消息头
  uint32 seq                    # 数据的序号
  time stamp                    # 数据的时间戳
  string frame_id               # 地图的坐标系
nav_msgs/MapMetaData info       # 地图的一些信息
  time map_load_time            # 加载地图的时间
  float32 resolution            # 地图的分辨率，一个格子代表着多少米，一般为0.05，[m/cell]
  uint32 width                  # 地图的宽度，像素的个数, [cells]
  uint32 height                 # 地图的高度，像素的个数, [cells]
  geometry_msgs/Pose origin     # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
# 地图数据，优先累加行，从（0,0）开始。占用值的范围为[0,100]，未知为-1。
int8[] data    

消息可以分为3个部分，消息头header，地图信息info，地图数据data．

数据集

https://blog.csdn.net/tiancailx/article/details/115120665

基于g2o的后端优化的代码实现

https://blog.csdn.net/tiancailx/article/details/121266020

g2o(General Graphic Optimization)是基于图优化的库。

图优化是把优化问题表现成图的一种方式。一个图由若干个顶点(Vertex)，以及连接这些顶点的边(Edge)组成。用顶点表示优化变量，用边表示误差项。

SLAM本质剖析-G2O：https://www.guyuehome.com/34679

• 优化前
  
• 优化后
  
• 最终地图
  

5.2 install without ROS

ChatGPT 给的如下回答

• 要在没有ROS的情况下运行Cartographer，需要以其他格式提供传感器信息。可以使用外部格式，如protobuf文件或CSV文件，将激光雷达数据、里程计数据或IMU数据导入Cartographer。这可能涉及编写代码来将数据转换为Cartographer可用的数据类型。

• 修改Cartographer源码： 如果愿意编写代码，您可以修改或扩展Cartographer源码以支持从其他库或格式获取传感器数据。这将需要研究Cartographer库的组织结构并理解代码和逻辑。

• 用其他库代替ROS传输数据： 如果系统中已经使用了其他库（如ZeroMQ、DDS、IPC 等）进行通信，可以将传感器数据收集和发送给Cartographer所需的数据，您需要编写代码适配这些库与Cartographer之间的数据交互。

• 由于Cartographer本身高度依赖于ROS的数据和头文件，执行这些修改可能涉及到相当的工作量。

• Google Cartographer项目提供了一个称为cartographer_offline_node的离线版本，允许使用protobuf文件而不是ROS消息。以下是使用protobuf文件运行Cartographer的简要步骤：

• • 安装Cartographer： 如果您尚未安装，请按照官方文档（https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html)中的说明安装Cartographer。
• • 准备数据： 准备包含传感器数据（如激光雷达扫描、IMU读数和位姿估计）的protobuf文件。这通常包括记录传感器的原始数据并将其转换为适合Cartographer处理的格式。
• • 创建配置文件： 您需要为您的传感器创建一个配置文件，详细说明如何将传感器数据映射到Cartographer中的各个项目。可以参考Cartographer项目中的示例配置文件并根据自己的设备进行修改。关于这个过程的详细解释，可以参考这个帖子：https://google-cartographer-ros.readthedocs.io/en/latest/index.html
• • 在Cartographer离线节点上运行Cartographer： 使用protobuf数据文件、配置文件以及相关数据映射文件运行Cartographer离线节点（cartographer_offline_node）。以下是一个示例命令：

cartographer_offline_node \
  -configuration_directory /path/to/your/config \
  -configuration_basename cartographer_config.lua \
  -urdf_filename /path/to/your/robot/sensor.urdf \
  -bag_filename /path/to/your/sensor/data.pbstream \
  -pose_graph_filename /path/to/output/pose_graph.pbstream

• • 运行完成后，将获得一个包含构建的位姿图（pose graph）的protobuf文件。可以使用各种仿真和可视化工具对位姿图进行分析和评估，如Gazebo、RViz或自定义工具。
    请注意，这些步骤只是离线处理Cartographer SLAM问题的一个基本方法。由于这涉及到从原始传感器数据开始创建自己的protobuf文件、配置和映射文件，可能需要对Cartographer有较深入的了解。不过，这是从ROS中分离Cartographer的一个可行方法。

知乎一回答

如何不在ROS平台下做激光slam？

• 要解决传感器通讯机制问题： 思岚雷达也好或者其它型号的雷达都会提供不基于ros的驱动和SDK，你要做的是手写一个mqtt机制来把雷达数据传输出来。到这一步，你相当于仿写了ros的 subscribe和publish话题功能
• 解决slam框架脱离ros问题： 除非你从头手写一个slam，不然还是要调slam库的。我推荐你对Hectorslam 或者cartographer来进行去ros化，Hector代码十分简单，去ros化也很简单。如果你追求slam的工程性，那么推荐对cartographer进行去ros化，不过这需要花一段时间，我当时是花了一周时间，还需要重构很多数据类型，贼烦！
• 把数据喂给slam： 在slam端写好接收传感器数据函数，启动slam。地图可视化界面书写： 这里要用到qt，去手写一个接收地图并显示的软件。如果你不会，可以用OpenCV显示地图。

扫地机器人增强位姿融合的Cartographer算法及系统实现

张亮, 刘智宇, 曹晶瑛, 沈沛意, 蒋得志, 梅林, 朱光明, 苗启广. 扫地机器人增强位姿融合的Cartographer算法及系统实现[J]. 软件学报, 2020, 31(9): 2678-2690.

SLAM系统以驱动的方式合入到整个Player平台中, 易于修改替换, 通过订阅相应传感器接收其数据进行处理。

cartographer without ros【CloudFlame】

From cartographer without ros

不带 ROS 的快速安装

创建CartographerEnvStart.sh文件

 
cd ~/Code
# Install the required libraries that are available as debs.
sudo apt-get update
sudo apt-get install -y \
    clang \
    cmake \
    g++ \
    git \
    google-mock \
    libboost-all-dev \
    libcairo2-dev \
    libcurl4-openssl-dev \
    libeigen3-dev \
    libgflags-dev \
    libgoogle-glog-dev \
    liblua5.2-dev \
    libsuitesparse-dev \
    lsb-release \
    ninja-build \
    stow \
	librosbag-dev \
	libbz2-dev \
	liblaser-geometry-dev \
	libnav-msgs-dev \
 
# Install Ceres Solver and Protocol Buffers support if available.
# No need to build it ourselves.
if [[ "$(lsb_release -sc)" = "focal" || "$(lsb_release -sc)" = "buster" ]]
then
  sudo apt-get install -y python3-sphinx libgmock-dev libceres-dev protobuf-compiler
else
  sudo apt-get install -y python-sphinx
  if [[ "$(lsb_release -sc)" = "bionic" ]]
  then
    sudo apt-get install -y libceres-dev
  fi
fi
 
 
cd ~/Code
git clone https://github.com/abseil/abseil-cpp.git
cd abseil-cpp
git checkout d902eb869bcfacc1bad14933ed9af4bed006d481
mkdir build
cd build
cmake -G Ninja \
  -DCMAKE_BUILD_TYPE=Release \
  -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
  -DCMAKE_INSTALL_PREFIX=/usr/local/stow/absl \
  ..
ninja
sudo ninja install
cd /usr/local/stow
sudo stow absl
 
 
 
cd ~/Code
VERSION="1.13.0"
 
# Build and install Ceres.
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
git checkout tags/${VERSION}
mkdir build
cd build
cmake .. -G Ninja -DCXX11=ON
ninja
CTEST_OUTPUT_ON_FAILURE=1 ninja test
sudo ninja install
 
 
cd ~/Code
VERSION="v3.4.1"
 
# Build and install proto3.
git clone https://github.com/google/protobuf.git
cd protobuf
git checkout tags/${VERSION}
mkdir build
cd build
cmake -G Ninja \
  -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
  -DCMAKE_BUILD_TYPE=Release \
  -Dprotobuf_BUILD_TESTS=OFF \
  ../cmake
ninja
sudo ninja install
 
cd ~/Code
# Build and install Cartographer.
git clone https://github.com/cartographer-project/cartographer.git
cd cartographer
mkdir build
cd build
cmake .. -G Ninja
ninja
CTEST_OUTPUT_ON_FAILURE=1 ninja test
sudo ninja install

./CartographerEnvStart.sh

思路和大纲

思路：仿cartographer_ros使用cartographer。

分析cartographer_ros代码分析后发现：

• cartographer_node实际控制着整个系统的业务逻辑:
• subscribers_订阅了各类传感器数据
• map_builder_bridge_用于封装map_builder_，并将Ros传感器数据转换cartographer数据类型。
• map_builder_bridge_中的map_builder_用于添加轨迹，建图

大纲：
传感器数据的处理：将各类原始数据转换cartographer数据类型：

scan_format
odom_format
imu_format
landmark_format

map_builder_的使用：

map_builder_node

雷达scan数据转换

激光雷达LaserScan传感数据转换为cartographer中的TimedPointCloudData数据。还有cartographer中的TimedPointCloudData数据和Ros的sensor_msgs::LaserScan数据相互转换，方便引入Rosbag进行储存和读取。

1：TimedPointCloudData数据类型【timed_point_cloud_data.h】

//时间
common::Time time;            
//原点
Eigen::Vector3f origin;
//点云数据
TimedPointCloud ranges;
    //坐标
    Eigen::Vector3f position;
    //帧内时间
    float time;
//反射强度
std::vector<float> intensities;

2：LaserScan传感数据转换cartographer数据

cartographer::sensor::TimedPointCloudData ScanToTimedPointCloudData()
{
 
    cartographer::sensor::TimedPointCloudData time_point_cloud;
    time_point_cloud.time = cartographer::common::NowTime();
    cartographer::sensor::PointCloudWithIntensities point_cloud;
 
    float angle_min = -M_PI;
    float angle_max = M_PI;
    float angle = angle_min;
    float angle_increment = (angle_max - angle_min) / float(scanszie);
    float range_min = std::atof(driver_->getParametersCached().at("radial_range_min").c_str());
    float range_max = std::atof(driver_->getParametersCached().at("radial_range_max").c_str());
    float range_length = range_min;
 
    cartographer::sensor::TimedRangefinderPoint point;
    for (size_t i = 0; i < scanszie; ++i)
    {
        range_length = float(scandata.distance_data[i]) / 1000.0f;
        if (range_min <= range_length && range_length <= range_max)
        {
            point.position = Eigen::Vector3f(range_length * cos(angle), range_length * sin(angle), 0.);
            point_cloud.intensities.push_back(scandata.amplitude_data[i]);
        }
        else
        {
            point.position = Eigen::Vector3f::Zero();
            point_cloud.intensities.push_back(0.f);
        }
        point.time = 0;
        point_cloud.points.push_back(point);
        angle += angle_increment;
    }
 
    time_point_cloud.origin = Eigen::Vector3f::Zero();
    time_point_cloud.ranges = point_cloud.points;
    time_point_cloud.intensities = point_cloud.intensities;
 
    return time_point_cloud;
}

3：LaserScan传感数据转换 ROS 数据
【cartographer_ros】三： 发布和订阅雷达LaserScan信息

4：cartographer数据转换ROS数据

sensor_msgs::LaserScan ToRosMsg(cartographer::sensor::TimedPointCloudData time_point_cloud)
{
    sensor_msgs::LaserScan scan;
 
    scan.header.stamp = common::RosFromCarto(time_point_cloud.time);
    scan.header.frame_id = "base_link";
    scan.angle_min = -M_PI + ((slam::common::ConfigData.slam_scan_offset * M_PI) / 180.0);
    scan.angle_max = M_PI + ((slam::common::ConfigData.slam_scan_offset * M_PI) / 180.0);
    scan.angle_increment = (scan.angle_max - scan.angle_min) / float(common::ConfigData.scanner_per_scan);
    scan.scan_time = float(1.0 / float(common::ConfigData.scanner_frequency));
    scan.time_increment = float(1.0 / float(common::ConfigData.scanner_frequency * common::ConfigData.scanner_per_scan));
    scan.range_min = 0.0;
    scan.range_max = 100.0;
    scan.ranges.resize(common::ConfigData.scanner_per_scan);
    scan.intensities.resize(common::ConfigData.scanner_per_scan);
    for (int i = 0; i < common::ConfigData.scanner_per_scan; ++i)
    {
        scan.ranges[i] = sqrt(pow(time_point_cloud.ranges[i].position[0], 2) + pow(time_point_cloud.ranges[i].position[1], 2));
        scan.intensities[i] = time_point_cloud.intensities[i];
    }
    return scan;
}

5：ROS 数据转换cartographer数据

cartographer::sensor::TimedPointCloudData ToCartoSensor(sensor_msgs::LaserScan scan)
{
    cartographer::sensor::TimedPointCloudData time_point_cloud;
    cartographer::sensor::PointCloudWithIntensities point_cloud;
    size_t scanszie = scan.ranges.size();
    float angle = scan.angle_min;
    float angle_increment = scan.angle_increment;
    float range_length = scan.range_min;
 
    cartographer::sensor::TimedRangefinderPoint point;
    for (size_t i = 0; i < scanszie; ++i)
    {
        range_length = scan.ranges[i];
        point.position = Eigen::Vector3f(range_length * cos(angle), range_length * sin(angle), 0.);
        point.time = 0;
        point_cloud.intensities.push_back(scan.intensities[i]);
        point_cloud.points.push_back(point);
        angle += angle_increment;
    }
    time_point_cloud.origin = Eigen::Vector3f::Zero();
    time_point_cloud.ranges = point_cloud.points;
    time_point_cloud.intensities = point_cloud.intensities;
    time_point_cloud.time = TimeCartoFromRos(scan.header.stamp);
    return time_point_cloud;
}

里程计odom数据转换

里程计Odom数据转换为cartographer中的OdometryData数据。还有cartographer中的OdometryData数据和ROS的nav_msgs::Odometry数据相互转换，方便引入Rosbag进行储存和读取。

1：OdometryData数据类型【odometry_data.h】

//时间
common::Time time;
//位置
transform::Rigid3d pose;

2：Odom数据转换cartographer数据

cartographer::sensor::OdometryData GetOdometryData()
{
    cartographer::sensor::OdometryData odometry_data;
 
    current_time = ros::Time::now();
    double dt = (current_time - last_time).toSec();
    double dx = (vx * cos(th) - vy * sin(th)) * dt;
    double dy = (vx * sin(th) + vy * cos(th)) * dt;
    double dth = vth * dt;
    last_time = current_time;
 
    x += dx;
    y += dy;
    th += dth;
 
 
    cartographer::common::Time curTime = cartographer::common::NowTime();
    const Eigen::AngleAxisd rotation(th, Eigen::Vector3d::UnitZ());
    const Eigen::Vector3d translation(x, y, 0.0);
    cartographer::transform::Rigid3d pose(translation, rotation);
    odometry_data = cartographer::sensor::OdometryData{curTime, pose};
 
    return odometry_data;
}

3：Odom数据转换 ROS 数据
【cartographer_ros】四： 发布和订阅里程计odom信息

4：cartographer数据转换ROS数据

nav_msgs::Odometry ToRosMsg(cartographer::sensor::OdometryData odometry_data)
{
    nav_msgs::Odometry odom;
 
    odom.header.stamp = TimeRosFromCarto(odometry_data.time);
    odom.header.frame_id = "odom";
    odom.pose.pose.position.x = odometry_data.pose.translation().x();
    odom.pose.pose.position.y = odometry_data.pose.translation().y();
    odom.pose.pose.position.z = odometry_data.pose.translation().z();
    odom.pose.pose.orientation.w = odometry_data.pose.rotation().w();
    odom.pose.pose.orientation.x = odometry_data.pose.rotation().x();
    odom.pose.pose.orientation.y = odometry_data.pose.rotation().y();
    odom.pose.pose.orientation.z = odometry_data.pose.rotation().z();
    odom.child_frame_id = "base_link";
    return odom;
}

5：ROS 数据转换cartographer数据

cartographer::sensor::OdometryData ToCartoSensor(nav_msgs::Odometry odom)
{
    cartographer::sensor::OdometryData odometry_data;
    odometry_data.time = Time::CartoFromRos(odom.header.stamp);
    const Eigen::Vector3d translation(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z);
    const Eigen::Quaterniond rotation(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z);
    odometry_data.pose = cartographer::transform::Rigid3d(translation, rotation);
    return odometry_data;
}

陀螺仪Imu数据转换

陀螺仪Imu数据转换为cartographer中的ImuData数据。还有cartographer中的ImuData数据和 ROS 的sensor_msgs::Imu数据相互转换，方便引入Rosbag进行储存和读取。

1：ImuData数据类型【imu_data.h】

//时间
common::Time time;
//线加速度
Eigen::Vector3d linear_acceleration;
//角速度
Eigen::Vector3d angular_velocity;

2：Imu数据转换cartographer数据

cartographer::sensor::ImuData GetImuData()
{
    cartographer::sensor::ImuData imu_data;
 
    //计算
    //线加速度(含重力)
    acc[0] = *((float *)&handle_buf[22]);
    acc[1] = *((float *)&handle_buf[26]);
    acc[2] = *((float *)&handle_buf[30]);
 
    //角速度(陀螺仪I的输出)
    gyo[0] = *((float *)&handle_buf[82]);
    gyo[1] = *((float *)&handle_buf[86]);
    gyo[2] = *((float *)&handle_buf[90]);
 
    //欧拉角
    eular[0] = *((float *)&handle_buf[146]);
    eular[1] = *((float *)&handle_buf[150]);
    eular[2] = *((float *)&handle_buf[154]);
 
    Eigen::Vector3d linear_acceleration(((double)acc[0] * (-9.8)), ((double)acc[1] * (-9.8)), ((double)acc[2] * (-9.8)));
    Eigen::Vector3d angular_velocity(((double)gyo[0] * 3.1415926 / 180), ((double)gyo[1] * 3.1415926 / 180), ((double)gyo[2]) * 3.1415926 / 180);
    Eigen::Vector3d imu_angle(((double)eular[0]), ((double)eular[1]), ((double)eular[2]));
 
    imu_data_.time = cartographer::common::NowTime();
    imu_data_.linear_acceleration = linear_acceleration;
    imu_data_.angular_velocity = angular_velocity;
    return imu;
}

3：Imu数据转换ROS数据
【cartographer_ros】五： 发布和订阅陀螺仪Imu信息

4：cartographer数据转换ROS数据

sensor_msgs::Imu ToRosMsg(cartographer::sensor::ImuData imu_data)
{
    sensor_msgs::Imu imu;
 
    imu.header.stamp = TimeRosFromCarto(imu_data.time);
    imu.header.frame_id = "imu";
    imu.angular_velocity.x = imu_data.angular_velocity.x();
    imu.angular_velocity.y = imu_data.angular_velocity.y();
    imu.angular_velocity.z = imu_data.angular_velocity.z();
    imu.linear_acceleration.x = imu_data.linear_acceleration.x();
    imu.linear_acceleration.y = imu_data.linear_acceleration.y();
    imu.linear_acceleration.z = imu_data.linear_acceleration.z();
    return imu;
}

5：ROS 数据转换cartographer数据

cartographer::sensor::ImuData ToCartoSensor(sensor_msgs::Imu imu)
{
    cartographer::sensor::ImuData imu_data;
    imu_data.time = TimeCartoFromRos(imu.header.stamp);
    imu_data.angular_velocity = Eigen::Vector3d(imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z);
    imu_data.linear_acceleration = Eigen::Vector3d(imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z);
    return imu_data;
}

路标landmark数据转换

cartographer中的LandmarkData数据和Ros中的cartographer_ros_msgs::LandmarkList数据相互转换，方便引入Rosbag进行储存和读取。

1：LandmarkData数据类型【landmark_data.h】

struct LandmarkObservation {
  std::string id;
  transform::Rigid3d landmark_to_tracking_transform;
  double translation_weight;
  double rotation_weight;
};
 
struct LandmarkData {
  common::Time time;
  std::vector<LandmarkObservation> landmark_observations;
};

2：cartographer数据转换ROS数据

cartographer_ros_msgs::LandmarkList ToRosMsg(cartographer::sensor::LandmarkData landmark_data)
    {
        cartographer_ros_msgs::LandmarkList landmarkList;
        cartographer_ros_msgs::LandmarkEntry landmarkEntry;
 
        landmarkList.header.frame_id = "base_link";
        landmarkList.header.stamp = common::RosFromCarto(landmark_data.time);
        landmarkList.landmarks.clear();
 
        for (auto var : landmark_data.landmark_observations)
        {
            landmarkEntry.id = var.id;
            landmarkEntry.tracking_from_landmark_transform.position.x = var.landmark_to_tracking_transform.translation().x();
            landmarkEntry.tracking_from_landmark_transform.position.y = var.landmark_to_tracking_transform.translation().y();
            landmarkEntry.tracking_from_landmark_transform.position.z = 0;
            landmarkEntry.tracking_from_landmark_transform.orientation.x = var.landmark_to_tracking_transform.rotation().x();
            landmarkEntry.tracking_from_landmark_transform.orientation.y = var.landmark_to_tracking_transform.rotation().y();
            landmarkEntry.tracking_from_landmark_transform.orientation.z = var.landmark_to_tracking_transform.rotation().z();
            landmarkEntry.tracking_from_landmark_transform.orientation.w = var.landmark_to_tracking_transform.rotation().w();
            landmarkEntry.translation_weight = var.translation_weight;
            landmarkEntry.rotation_weight = var.rotation_weight;
            landmarkList.landmarks.push_back(landmarkEntry);
        }
        return landmarkList;
    }

3：Ros数据转换cartographer数据

cartographer::sensor::LandmarkData ToCartoSensor(cartographer_ros_msgs::LandmarkList landmark_list)
{
  cartographer::sensor::LandmarkData landmark_data;
  landmark_data.time = FromRos(landmark_list.header.stamp);
  for (const LandmarkEntry& entry : landmark_list.landmarks) {
    landmark_data.landmark_observations.push_back(
        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
         entry.translation_weight, entry.rotation_weight});
  }
  return landmark_data;
}

参考

1、官网–cartographer
2、算法原理–cartographer–Real-Time Loop Closure in 2D LIDAR SLAM
W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time Loop Closure in 2D LIDAR SLAM, in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016. pp. 1271–1278.
3、代码–cartographer
4、SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
5、https://linklab-uva.github.io/autonomousracing/assets/files/SLAM.pdf
6、张亮, 刘智宇, 曹晶瑛, 沈沛意, 蒋得志, 梅林, 朱光明, 苗启广. 扫地机器人增强位姿融合的Cartographer算法及系统实现[J]. 软件学报, 2020, 31(9): 2678-2690.
7、cartographer without ros
8、李想–从零开始搭建二维激光SLAM
9、机器人开发–SLAM详细介绍
10、代码解读–cartographer–linyicheng博士
11、代码解读–cartographer–heimazaifei
12、如何不在ROS平台下做激光slam？
13、梅卡曼德（北京）机器人科技有限公司 CEO 邵天兰 评 Cartographer
14、【Win10+Cartographer+QT+OpenGL】之实时显示3D点云及处理
15、Cartographer ROS Documentation