源码阅读：使用Cartographer进行毫米波SLAM(番外篇)：最后整体过一下代码

老版本的滤波器用卡尔曼滤波 ，也就是做位姿预估的时候用卡尔曼

新版本改成位姿差值器   

有些新特性 

common  普通常用文件 不用看

transform和sensor 常用数据结构

mapping  一些基类   

主要看mapping2d和3d

mapping2d   :

scan match和sparse pose graph部分

最主要关注的文件 global_trajectory_builder    local_trajectory_builder    sparse_pose_graph   constraint_builder

另外还有 scanmatch里的  ceres_scan_matcher    csm: correlative scan matcher    fast_csm   real_time_csm

从 global_trajectory_builder开始：
主要函数：里程计数据的加入  IMU数据的加入  以及激光雷达数据的加入  或者是range传感器  前两个数据的加入都只是为了更新位姿跟踪器

增加水平激光雷达：分为两部分  一、local trajectory builder  ：前端匹配

               二、s pose graph  ：全局

进入前端匹配：

首先：进行位姿估计          有一个投影矩阵 把三维的位姿投影到2D  但一般不需要 因为是平面车辆

接下来，进行scan match    扫描匹配 也就是帧间匹配

计算完之后 得到一个位姿的方差 更新滤波器

后面都是 返回数据和数据处理

进入scanmatch   

首先 位姿预估器给你一个初始位姿 然后调用real time correlative scan match 进行扫描匹配

这个位姿又作为初始位姿来调用 ceres scan match得到最终位姿   原理在于： 第一个匹配对初值不敏感 但精度低  第二个优化对初始敏感 但精度高

具体流程：

得到拿来做匹配的地图   

得到预测的位姿

进行csm匹配   （correlative scan match）

调用优化  ceres scan match   

结束

然后用位姿更新一下位姿预估器

只讲csm

根据搜索参数  生成所有的解  给所有的解打分  打分就是占用概率的和

帧间匹配过程 没有加速策略  因为运动比较小  

简单说一下优化：

构造残差： 栅格得分  平移惩罚  旋转惩罚 三个

求解器

结束

local部分结束

进入global   addscan   

addworkItem   关键函数   可以认为是个队列  存储的是待执行的函数   在另一个线程里 从队列里取出来函数 来执行

存的是：  computeConstrainsForScan    

此计算为：  1、计算和insertion_submaps之间的约束   这里枚举执行了插入操作的submap  实际上没有计算 因为数据已经有了 求个逆就行了

2、计算和每个已经完成的submap的约束  实际上就是回环检测   枚举每一个完成了的submap     每个submap都调用MaybeAddConstrainsForScan函数来计算约束

此函数：最终实际上调用的是   ComputeConstraint  来计算一个submap和一个scan之间的约束关系

而这个函数调用的是fast Csm  也就是分支定界   做完之后再优化一次  

看来fast csm

多尺度的金字塔方法   在不同分辨率进行搜索  每个分辨率都对应一个栅格地图

最终搜索函数都是matchWithsearchParameters

    把scan数据进行旋转  离散化  然后生成最低分辨率的候选解  也就是最粗糙 然后进行打分和排序

    进行分支定界搜索

                BranchAndBound

                                        定界： 枚举每个解  如果候选解的得分比最小的得分还小     min_score相当于当前搜索过的所有解中最优解的得分  如果当前的解得分比这个分小 那这颗子树就全删掉 不再搜这个树

                                        分支：把一个节点分成四个节点 放入队列进行打分

                                        继续迭代

然后判断约束是否达成回环   进行优化