参考：周治国 等: 3D 激光雷达 SLAM 算法综述
我想知道 cartographer里的分支定界到底是咋回事
1. 从图优化开始
图优化 SLAM 的研究基础是基于图论，图( graph) 是一种数据结构，由顶点( vertex) 与连接顶点的边( edge) 组成表示为 G( V，E) ，其中 G 表示图，顶点的集合表示为 V，边的集合表示为 E，其思想是用顶点表示事物，而连接不同顶点之间的边则用于表示事物之间的关系，如果在图 G 中存在一个顶点上连接两个以上的边，则称该图为超图，在 SLAM 中研究的就是根据已有的观测数据实现超图的构建以及优化的过程。
假设移动载体的位姿节点用 μ = { μ1，μ2，…，μn} 表示，将环境中的地标（比如一个垃圾桶）表示为 S = { S1，S2，…，Sn} ，则移动平台的位姿和地标可以用下图表示。
如果某时刻 k，移动载体在位置 μk通过激光传感器进行扫描观测（观测这个垃圾桶）得到数据 Sk，则传感器的观测方程为:
Sk= F(μk)
由于系统参数和传感器观测存在误差，使得上式不可能精确相等，因此误差
ek= Sk－ F( μk) 便存在，如果把 minFk( μk) = ‖ek‖ 作为目标函数，找到使ek最小的 μk，那这个 μk就是机器人k时刻位姿的估计值，移动轨迹就可以一点点得到了。
顶点是激光雷达的位姿或特征点（垃圾桶）的位姿，边代表观测方程，也就是约束，可以有很多约束，比如imu约束，gps约束，准确的约束越多，得到的机器人位姿就越准确。
2. 基于图优化的 SLAM
基于图优化的slam方案可以分为
扫描匹配
、
闭环检测
、
后端优化
、
点云地图存储表示
4 个部分。
扫描匹配和闭环检测都是为了根据观测信息建立图的节点以及节点间的约束，即完成图的构建，为图优化 SLAM 的前端部分。
后端优化
是对前端构建的图信息进行
非线性优化
，取得
尽量满足所有约束关系的最优解
，最后输出姿态估计结果和全局点云地图，这一部分也被称为 SLAM 后端，与 SLAM 前端共同组成整个图优化 SLAM 框架。
扫描匹配
的分类如下：
基于特征的方法的关键部分为：关键点检测、特征描述符提取、真实匹配、异常值剔除和转换估计。
闭环检测！
闭环检测基于全局数据关联，是实现鲁棒 SLAM 的核心步骤，通过
识别是否到达历史场景
促使地图闭环的能力，能够校正累积误差，从而产生全局一致性的映射地图。
闭环检测的
难点
主要体现在:
- 感知歧义。例如在长廊、隧道、楼梯等结构化十分相似的场景，会加剧判断难度。
-
由于激光传感器本身的稀疏性造成的观测数据鲁棒性和区分性受限问题，即如何建立易于处理的环境有效表
征方式。 -
数据规模会随着运行时间增加而导致需要判断的帧数据不断增长，会降低建图的实时性。
目前的闭环检测技术主要有：
基于 MonteCarlo 的节点搜索算法
依据 GPS 辅助法进行辅助闭环判断;
基于描述子的回环检测算法，局部描述子代表算法为 FPFH，
基于 Gasalt3D 描述符的概率投票方法…
后端优化！
基于图优化 SLAM 的后端优化方法可概括分为 4 类: 基于
最小二乘法
的优化方法、基于
松弛迭代
的优化方法、基于
随机梯度下降
的优化方法以及
基于流形迭代
方法。目前基于图优化的开源优化库有 iSAM( incremental smoothing and mapping)、GTSAM ( georgia tech smoothing and mapping )、G2O( general graph optimization )、Ceres、BA ( bundle adjustment)等，借助于这些优化库可节省后端迭代求解优化值的时间。
最后，
地图表示
、
激光雷达最容易得到的是点云地图，但是由于点云数量过于庞大，需要进行体素滤波（降采样）才能正常显示。
（ 小tips：体素滤波的原理是根据输入的点云，首先
计算一个能够刚好包裹住该点云的立方体
，然后根据
设定的分辨率
，将该大立方体分割成不同的小立方体。对于每一个小立方体内的点，计算他们的质心，并
用该质心的坐标来近似该立方体内的若干点
。）
单纯的点云地图无法直接用于定位。因此有了 稀疏点云地图（将特征分离出来，使用特征进行重定位），占据网格地图（栅格地图，用于导航避障和路径规划），八叉树地图（八叉树地图是一种特殊的占据栅格地图，该结构中占据概率相同的栅格可进行合并，从而降低存储地图的空间，压缩性能更好。）更高层次：语义地图。
3. 分支定界
回到最初的疑问，
分支定界和图优化
是什么关系？分支定界在cartographer中怎么用的？
分支定界法（branch and bound）是一种求解整数规划问题的最常用算法，是一种搜索与迭代的方法。
通俗来讲，就是一棵树，把它根、枝、叶分开。A是树的根，也就是原问题，绿色字母JKLMNOPQ为一个一个的叶子，也就是解，BCDEFGHI为中间的“枝”，可以理解为根问题的一系列子问题。
这样一棵树分好枝叶后，在F上计算JKLM的解，选择一个得分最高的解，记为best1。返回上一层G，计算G的目标函数值（得分）best2，与best1比较，若best1依旧最大，则对G进行剪枝，继续计算H的目标函数值。若best2大于best1，则对G进行分枝NOPQ，计算NOPQ的目标函数最大值，与best2比较，更新best。.
4. cartographer里的分支定界！
在cartographer里 这个分支定界
结合了多分辨率栅格
，
解决子地图的构建以及与全局地图的匹配问题。
Cartographer 生成地图由两部分组成。第一部分是生成基于局部坐标系的子图，第二部分是生成一个全局地图，该地图是由子图组合而成，当识别出回环时对其进行校正。激光雷达的一次扫描提供一帧点云，Cartographer 的子图用
概率网格
表示，每个像素存储网格点被占用的概率值。根据每个点在子图坐标系中的位姿，点对应的子图中像素用
占据概率
更新概率值，点与原点的连线对应的子图中像素(排除点命中的像素)用
空闲概率
更新概率值。基于 Ceres 的扫描匹配器计算激光雷达扫描的点云在子图中的位姿，它将位姿求解转换为非线性最小二乘问题，
目标是找到在子图中插入点云，使点对应像素的概率和最大的位姿
。
在局部 SLAM 中计算位姿的过程始终会由于地图分辨率、传感器噪声等原因而产生误差。因为位姿的计算是基于每个平移和旋转的总和，且点云仅与包含最近连续几次扫描的子图进行匹配，随着行驶距离的增长，漂移误差越来越大。因此，全局 SLAM 使用稀疏位姿调整来消除累积误差。全局优化也被建模成非线性最小二乘问题。基于
分支定界
法的回环检测方法，
当点云的位姿使匹配分数高于设定的阈值时则认为检测到了回环
，将对应的相对位姿添加到非线性最小二乘问题的优化约束中。每插入一定数量的点云，便对优化问题进行求解以计算全局位姿的最优解。
基于
深度优先搜索
(Depth Frist Search，DFS)的分支定界法加速回环检测，
目标是在搜索窗口中找到匹配分数最高的位姿
。该算法实现的关键思路是把搜索窗口建立为
树
。这就和上文中的根、枝、叶联系起来了！根节点为搜索窗口中全部的位姿。每个叶节点表示搜索窗口中的一个位姿。算法根据给定的搜索高度h， 将根节点分支到高度为h 的初始子节点集 C ，并计算 C 中每个子节点的匹配分数。若子节点集C 中最高的匹配得分高于设定的分数阈值则检测到回环，
在具有最高分数的子节点中寻找具有最大上界的叶节点并返回其位姿
，否则为未检测到回环。