这篇文章将向大家介绍2D建图的全局优化算法实现分析的第一部分，主要介绍cartographer系统中基于ceres优化框架下的问题定义和求解的具体实现。后面将继续介绍优化过程中更多的议题，如全局位姿图优化实现中的更多的策略技巧，回环检测和全局地图信息如何维护和更新等。
首先Local SLAM的优化结果会插入到全局优化位姿图中，具体调用过程堆栈可以参考如下的代码片段和相关注释分析。
原始的激光点云帧经过一些列的流转变换和聚合后形成一个较为稠密的逻辑点云(如TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160，时间跨度会较长一点，后面处理的时候会通过自适应滤波器进行滤波)。该点云帧优化后的位姿等数据作为位姿图的节点插入，同时一个子图中会存在num_range_data个点云帧数据，但是也并不是每一个逻辑点云帧都会加入到位姿图的节点中(比如扫描匹配效果不太好的或运动变化小的将返回nullptr，不会插入到位姿节点中）。接下来将以问答的形式对相关问题进行说明。
问题：cartographer中位姿图优化和局部SLAM的最小二乘非线性优化的不同点，以及和其他SLAM系统的全局BA优化有哪些不同？
回答：在cartographer的系统实现中，基于位姿图的全局优化和局部SLAM位姿优化都是基于ceres::Problem的优化问题定义及ceres::Solver问题求解器相结合的优化问题求解框架来完成的，但在全局位姿图优化中的问题定义方式和局部SLAM的扫描匹配算法中定义的位姿优化求解的问题定义方式有所不同：（1）、通过参数块来定义待优化的变量，在SLAM系统中一般优化变量为位姿，在前文中介绍的局部SLAM优化中没有添加参数块，也就是在局部SLAM中中的优化问题只有一个扫描匹配的过程，没有图优化的节点和边的概念，(2)、通过定义残差项来定义优化的约束和目标，位姿图优化中通过定义残差块的方式将子图内位姿变换约束(INTRA_SUBMAP)或回环约束(INTER_SUBMAP)添加到位姿图的残差模块定义中来，而局部SLAM则将点云帧和局部地图的匹配对齐程度的度量计算作为残差块加入到求解问题框架中来。虽然都是基于最小二乘的非线性优化算法，算法的定义和求解思路是类似的，但是算法的具体对应的问题和算法的求解问题规模有较大的不同，局部SLAM的优化基于当前点云帧位姿构建的最小二乘的非线性优化问题，问题规模小求解速度块，而全局SLAM优化基于逻辑的位姿图的构建，基于大量的点云帧位姿优化变量，以及子地图位姿优化变量作为位姿图的节点，以及上述的INTRA_SUBMAP和INTER_SUBMAP可以看成节点之间的约束的边，构成了逻辑上的位姿图，具体的求解方法基于高斯牛顿法或 Levenberg-Marquardt算法，求解的具体实现细节已经封装在ceres优化求解器的内部(有兴趣的读者可以检索相关SLAM中的优化算法介绍，后面也将会有相关的SLAM专题介绍)。其中添加参数块的代码和注释如下：
添加约束的代码和注释如下：
其中子图的全局位姿和子图的第一个点云帧的节点位姿的相对变换为Identiy变换的约束，但是仍然在优化的时候采用独立优化进行。
References
- cartographer开源代码仓库： cartographer-project/cartographer
Leave a Reply