Cartographer 的前端算法思路

---

---

前一篇博客里面提到的是 Cartographer 前端实现中非常小的一个部分的算法思路，参照了《Real time correlative scan matching》里的算法实现了一部分实时scan match 的功能，不过这并不是Cartographer中前端的全部，甚至是可以通过参数disable的一部分功能。
在 Cartographer 对应的论文《Real-Time Loop Closure in 2D LIDAR SLAM》中提到的前端算法中只有Ceres scan matching，其实就是基于ceres solver实现的非线性优化模型，今天我们看一下具体的算法模型。同样，在读懂算法和代码之前需要一些基础知识：

Ceres Solver tutorial & 最小二乘求解非线性优化问题
Cartographer Git
双三次插值

论文内容简单翻译

这里我们主要看论文的”IV. Local 2D SLAM” —— “C. ceres scan matching”部分：

Prior to inserting a scan into a submap, the scan pose ξ is optimized relative to the current local submap using a Ceres-based [14] scan matcher. The scan matcher is responsible for finding a scan pose that maximizes the probabilities at the scan points in the submap. We cast this as a nonlinear least squares problem
我们用一个基于 ceres solver 的 scan matcher 优化获得当前的 scan pose $\xi$，这个 scan matcher 负责找到一个位姿使得 scan 中的所有点在当前 local map 中的概率和最大，于是我们定义一下最小二乘问题：

argminξ∑k=1K(1−Msmooth(Tξ