机器人开发-世界杯斯洛文尼亚-世界杯什么时候_法国世界杯

机器人开发--Cartographer详细介绍

1 介绍1.1 概述1.2 评价1.3 特点

2 框架官方

3 代码结构heimazaifei 解读linyicheng 解读Xiaotu 解读cartographer_ros地图构建器map_builderLocal SLAMGlobal SLAM

赵锴解读地图设计匹配方法一阶段解算二阶段解算后端如何检测回环检测回环后该怎么做：优化问题

回顾：重定位中的位姿

4 论文分析Real-Time Loop Closure in 2D LIDAR SLAM扫地机器人增强位姿融合的Cartographer算法及系统实现

5 安装调试5.1 install with ROS李想--从零开始搭建二维激光SLAM雷达对比30000lux光强下的点云效果雷达频率对数据畸变的影响

ROS中雷达数据格式使用单线雷达实现LIO-SAM中的特征点提取使用PCL进行雷达的消息类型转换栅格地图的构建数据集基于g2o的后端优化的代码实现

5.2 install without ROSChatGPT 给的如下回答知乎一回答扫地机器人增强位姿融合的Cartographer算法及系统实现cartographer without ros【CloudFlame】不带 ROS 的快速安装思路和大纲雷达scan数据转换里程计odom数据转换陀螺仪Imu数据转换路标landmark数据转换

参考

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：非常轻量的脚本语言，主要用来做ConfigurationCeres：这是Google开源的做非线性优化的库，仅依赖于Lapack和BlasProtobuf：这是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}

odds(p)=1−pp

(

)

(

−

(

)

(

)

M_{new}(x)=clamp(odds^{-1}(odds(m_{old}(x)).odds(p_{hit})))

Mnew(x)=clamp(odds−1(odds(mold(x)).odds(phit)))

p表示占据概率，当p=0.5时，概率比值odds=1，表示占据和空闲各占一半。

−

odds^{-1}

odds−1表示函数逆运算。

0.55

p_{hit}=0.55

phit=0.55代表该位置被激光打到一次的概率，第一次观测会被直接赋值。

(

)

M_{new}(x)

Mnew(x)表示地图中 x 位置处的概率值。

举例：

初始时刻，栅格未知状态，激光第一次打到了位置 x 处，M(x)概率更新为0.55。随后，激光第二次重复打到了同一个位置：

(

)

0.55

−

0.55

1.22

odds(p_{hit})=\frac{0.55} {1-0.55}=1.22

odds(phit)=1−0.550.55=1.22,

(

)

0.55

1.22

odds(M_{old}(x))=odds_{0.55}=1.22

odds(Mold(x))=odds0.55=1.22

(

)

odds(p_{hit})

odds(phit) 和

(

)

odds(M_{old}(x))

odds(Mold(x)) 相乘为1.484，再求函数逆运算，恢复出更新后的

(

)

0.597

M_{new}(x)=0.597

Mnew(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 \mathop{min} \limits_{T}\sum[1-M(S_i(T))]^2

E(T)=argTmin∑[1−M(Si(T))]2

(

)

Si(T)

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

雷达对比

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

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 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 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