02 cartographer简介

    cartographer是一套2D/3D激光雷达SLAM框架，采用栅格化的地图表示（概率栅格或TSDF），cartographer的一大亮点是具有栅格地图表示下的闭环优化功能。该算法主要分为两个部分，第一个部分称为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。BBA就是把搜索窗口表达成一颗树的形式，树上每一个子节点形成了一个对父节点的所有可能性的划分。所以，父节点代表了该子树下的所有解的可能性。对每个节点都有一个得分score(c), 该分值是这个子树上所有解的得分可能性的一个上限（Bound）。因此，如果已经有一个解能比该节点的得分score(c)好，那么最优解肯定不存在该子树上，因此该子树就可以被剪枝，从而大大加快解问题的速度。而在实际问题中，实际上大多数分支（不是最优解的那些分支）的得分是非常低的，通过一个阈值就直接剪枝掉了很多子树。因此，BBA方法实时性提高但存储效率降低。得分score计算方式？预先计算好子图上每个pixel的得分值，直接存储下来，当输入一个节点时，直接lookup即可获取得分分数。

Carto	包括	详细介绍
前端	雷达数据处理	local_trajectory_builder_2d.AddRangeData 多传感器时间同步，点云数据去畸，以及体素滤波
	位姿预测	local_trajectory_builder_2d.AddAccumulatedRangeData.ExtrapolatePose 利用pose，里程计与imu计算线速度和角速度，不同的情况选择参与运算的传感器不同。利用线速度角速度乘以时间预测平移和姿态。 位姿预测： 获取姿态：获取time时刻姿态，通过imu_trakcer获得姿态 获取位姿：利用平移量和旋转量获取当前姿态 获取平移量： 使用里程计：通过里程计计算出的线速度乘以时间 不使用里程计：通过pose数据计算出的线速度乘以时间 获取旋转量： 直接从imu_tracker获取，imu_tracker在AdvanceImuTrancker中进行更新。 使用imu时：通过imu的角速度乘以时间 不使用imu，使用里程计时：通过里程计数据计算出角速度乘以时间 不使用imu，不使用里程计时：通过pose数据计算出的角速度乘以时间                         缺点：依赖IMU精度，没有使用pose对imu和里程计的数据进行融合校准。
	扫描匹配	目的：找到雷达点云在栅格地图中的位置与角度。 扫描匹配：相关性扫描匹配RealTimeCorrelativeScanMatcher2D和ceres扫描匹配，将相关性扫描匹配的位姿作为ceres扫描匹配的初始值。 残差： 地图残差： 地图分为概率地图和TSDF地图 。理想状态下，点云的每个点都是打在地图的障碍物上，这样每个的地图概率都很小。通过添加点云在概率地图上的概率值为残差项，来优化地图 平移残差： 位姿推测器推测出来的值当成是目标值。当前位置，是通过暴力匹配校准得到的 旋转残差：（当前角度-目标值）* 残差因子
	栅格地图更新	目的：将雷达数据写入概率栅格地图 更新过程： 首先来了一帧雷达数据，对应到每一个栅格点，即观测得到该栅格点是occupied或者是Free。 在cartographer中，使用CorrespondenceCostValue（整数表示的空闲概率）表示栅格状态，通过传感器的测量值（occupied/free）对栅格的空闲概率进行更新。栅格分为miss栅格（途经点）和hit栅格(起始点)。 栅格更新过程： 上一时刻的栅格地图的空闲概率（空闲概率[0, 1~32768]），结合当前的hit栅格概率，得到当前的空闲概率[0, 1~32768]，hit的概率是固定值(0.55),那么就可以建立一张表，当上一时刻的空间概率为[0, 1~32768]任意值时，能快速查到到当前空闲概率。 使用查找表加速栅格更新过程： 在cartographer中，采用了时间换空间的策略，使用了两张查找表，hit_table_, miss_table.。这两张表也称概率更新表，概率更新表存储着更新后的值，而对应该行的索引就是更新前的值。 hit_table_解决新来测量值是occupied时，如何更新该点的CorrespondenceCostValue miss_table_解决新来测量值是free时，如何更新该点的CorrespondenceCostValue
后端	线程池任务与调度
	向位姿图添加节点， 计算节点约束	子图内约束： local 坐标系下，子图原点指向tracking_frame的坐标变换 node和insertion_submaps之间的约束，由于在前端是，一个node只插入到了两个submap中，所以一个node最多只有两个约束为子图内约束。 子图间约束： 根据global坐标计算初值，然后通过分支丁界算法粗匹配与ceres的精匹配，获取校准后的位姿，最后计算local坐标系下，子图原点指向校准后的节点间的坐标变换。
	多分辨率地图	是否采用滑动窗口对图像进行滑动操作
	基于分支定界算法的粗匹配， 优化问题的构建与求解	分支定界： 先把整个地图中的一个区域展开到底（最高分辨率），得到一个匹配分数（得分下届），换句话说：就是获取激光在高分辨率地图的匹配分数。（下界） 然后把其他区域不展开，算匹配分数（得分上届），换句话说：就是获取激光在低分辨率的匹配分数。（上界） 若低分辨率得分上界<没有展开到底的高分辨率的下界高，这个低分辨率区域就不展开，直接丢弃。 优化问题求解： 确定2个节点在global坐标系下的相对位姿变换 通过其他方式再次获取这两个节点的相对位姿变换 对这2个先对位姿变换的差的最小二乘问题进行求解 进行求解之后会得到一个增量，将当前位姿加上这个增量后就得到了优化后的位姿