这篇文章将向大家介绍cartographer系统中2D SLAM中的扫描匹配算法，该算法基于姿态外推器的车体位姿估计初始值和点云帧信息实现了位姿估计的进一步优化。下面将对其实现代码和注释做具体的说明分析。
首先是ScanMatch算法调用前的一些预处理，其代码和注释说明如下。
ScanMatch算法的流程代码及注释说明如下：
关于ceres的非线性优化方法的一点说明，其采用了自定义的损失函数和正则化的约束损失函数相结合的损失函数定义，自定义的损失函数的总体思路是对点云点基于位姿做坐标变换后和较为准确的地图数据做对照计算，如点云点总体上处理在概率值较大的或TSDF值接近0的栅格位置附近，则损失函数较小，否则偏移越大损失越大，通过调整位姿使得损失函数较小时的位姿为更加和地图匹配和对齐的位姿，具体实现上通过位姿损失函数和正则化约束函数采用ceres非线性优化库实现对位姿的优化求解。代码片段示意如下：
基于占用概率的loss function定义的部分关键代码片段如下。
References
Leave a Reply