深入浅出Cartographer

        上次说了cartographer的基本配置及如何跑起来，今天就说说它的处理流程，先来一个框架图（自己画了一遍，理解了一下）：

处理流程

        多传感器（lidar、imu、odom）作为数据源，首先将IMU数据和里程计给位置外推器，为接下来的相关性匹配提供初始值。同时，激光雷达点云经过体素滤波器进行处理，进行SLAM前端的匹配（ceres）。如果，位置外推器此时没有提供位姿初始值，应该使用相关性匹配（CSM）进行初值的计算，在进行ceres匹配。 接下来根据一系列策略更新submap。

        后端采用fastCSM+分支定界加速（基于多分辨率地图），进行回环检测，最后将所有约束条件（这里我认为GPS应该也是为后端提供了约束），进行非线性最小二乘优化。

        配置文件结构如下，没啥好说的：

位姿外推器

        对于激光匹配，首先要有一个初始的位姿，在此基础上进行优化：有IMU，则采纳其角速度积分作为初始姿态，不信任IMU任何加速度信息；有里程计，则采纳里程计的线速度积分作为初始平移； 二者都没有，则根据匀速运动模型预测。     
==》可以看出，cartographer的多传感器融合是一个松耦合，主要依赖激光来定位。IMU和里程计数据并没有被构建到真正优化的目标函数中。

注意：松耦合imu的SLAM框架，imu部分一般都是没有真正构建到优化的目标函数之中的；紧耦合，eg：lio_sam，则是采用imu预积分的形式，将imu信息一同构建到优化函数中。

相关扫描匹配

        构建似然场。即对原先的地图进行高斯模糊，让它膨胀一些，然后把激光在一个搜索窗口内暴力匹配，计算得分（前端是在小窗口内进行匹配，后端因为要与大地图匹配，采用了分支定界加速）。

-得分如何计算？

        如果scan的点落在障碍物模糊区域内，落的越多，得分越高。

-地图不是无限大的吗，你怎么保证在搜索窗口里就能找到位姿呢？

        因为有初始位姿。误差肯定在一个范围内而不会马上发散到很远，所以可以在一个位姿的窗口内，对位姿进行暴力匹配搜索。

这部分在前端是可以选择不开启的。（算力要求高）

非线性优化（前端）

        CSM解算中，暴力搜索如果设置的搜索窗（步长）不合适，会造成累计误差。eg：如果角度步长设为1度，但如果刚好真正的角度是5.5度，那么CSM只能搜索到5或6度，而无法进一步细化，逐步累积将会造成误差。 因此，引入前端引入非线性优化ceres；

        在配置文件中调参：

        2D：三个误差项：位姿转换误差+ 旋转惩罚+平移惩罚 ，后二者限制了旋转和平移的修改不能距离初始位姿太大（越相信初始位姿，设置越大）。
        3D：四个误差项：低分辨率位姿转换误差+ 高分辨率位姿转换误差+旋转惩罚+平移惩罚。低分辨率位姿转换误差权重低于高分辨率。

分支定界

        在回环处理，采用分支定界去加速回环（剪枝）的匹配效率。具体原理参考：SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别_zkk9527的博客-CSDN博客_3d slam本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报，现在我把它搬运到博客中。由于参与分享汇报的同事有许多是做其他方向的机器人工程师（包括硬件、控制等各方面并不是专门做SLAM的工程师），加上汇报的内容较多，因此在分享中我尽量使用简介的口语，而不出现复杂的公式。所以本文面向的是3D-slam方向的初学者。内容在整理中参考了许多链接，将放在最后。在文章结束后，我会把原PPT放在上面，需要者自取。另外打个广告，在slam方向有实习意愿的，请发送简历至zkyy8https://blog.csdn.net/zkk9527/article/details/116160508?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166624463716782391847466%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166624463716782391847466&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-8-116160508-null-null.142%5Ev59%5Ejs_top,201%5Ev3%5Eadd_ask&utm_term=slam%E6%80%BB%E7%BB%93&spm=1018.2226.3001.4187

非线性优化（后端）

        前端不断维护当前激光帧与submap的位姿，后端在维护每个submap的位姿，也是构建图优化模型（Ceres）去调整个个submap的位姿，使它尽量满足约束（前端位姿约束、回环约束、gps观测约束）。