scan matcher

目前开源算法中采取的scanMatching方法主要是以下四种：

1. Gmapping：ICP（simple Gradient Descent）
2. Hector：Guass-Newton（multi-resolution map）
3. karto：Real-time CSM（multi-resolution + 三维窗口遍历寻优）
4. cartographer：Fast CSM（multi-resolution + branch and bound）

scanMatcher主要涉及两个评价函数，一个score用于优化调整粒子pose作为参考，一个likelihoodAndScore用于更新粒子权重：

$$s(x, z, m) = \sum_i s(x, z^i, m)\\ s(x, z^i, m) = e^{d^2/ \sigma}$$

粒子权重根据地图的匹配度更新：

$$laser\ frame \to map\ frame: \hat{z}^i = x \oplus z^i\\ map\ cell: (x,\ y)^T\\ d^2 = (\hat{z}^i - (x,y)^T)^T (\hat{z}^i - (x,y)^T)$$