老版本的滤波器用卡尔曼滤波 ，也就是做位姿预估的时候用卡尔曼
新版本改成位姿差值器
有些新特性
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相当于当前搜索过的所有解中最优解的得分 如果当前的解得分比这个分小 那这颗子树就全删掉 不再搜这个树
分支：把一个节点分成四个节点 放入队列进行打分
继续迭代
然后判断约束是否达成回环 进行优化