SLAM前端主要解决两个问题，一是从不同时刻的传感器输入中识别出同一个地图特征，二是计算每个当前时刻机器人相对该特征的位姿。
vSLAM能够轻松解决前者，LidarSLAM解决后者无压力。本文讨论激光SLAM前端问题1——特征点匹配。目前两大常用思路：scan2scan——ICP，scan2map——CSM。
basic的CSM算法思路如下：从前一帧机器人位姿开始，寻找最优刚性变换，使雷达点在栅格地图中的位置尽量对应于占据度为1的栅格。
为了保持定位信息原有的数位精度，使用双线性插值方法来获取雷达点的地图值(占据度)，而不是使用其所在栅格的地图值来直接对应：
$Pm$是雷达点，$P_{00}, P_{01}, P_{10}, P_{11}$是雷达点邻近的四个栅格中心点。于是得到地图值：
雷达点所在位置的地图值变化梯度：
记当前时刻的已有地图$M$，当前帧共输入$n$个雷达点$S_1, …, S_n$，其对应位置的占据度为$M(S_k)$，最优变换定义为$\xi = (\Delta x, \Delta y, \psi)^T$，则最优问题的最小二乘描述为：
scan2map的鲁棒性更强，但是实时性上打了折扣。对此主要有两个改进措施：一是局部搜索，二是分辨率金字塔。
一、局部搜索
实际计算中会选定一个搜索区间，通常只在上一时刻地图位置的附近，对其中包含的全部可能位姿进行评分。
上式（最小二乘表达式）只包含了雷达端点，考虑到传感器的噪点影响，局部极值影响大。
- 基于模版的匹配：雷达端点及射线所在栅格构成的多边形区域，以此作为局部地图，进行map2map匹配。减少局部极值的影响，提高计算代价，同时考虑到动态目标，会引入新的局部极值。
LM算法：迭代的方式求解最小二乘的最优解。
分支界定算法：基于广度优先搜索的算法，通过对解空间搜索树的分支进行扩展和剪枝，不断调整搜索方向，加快找到全局最优解的速度。界定核心：若当前分支的下界$C_{branch}$小于解空间上界$C_{HB}$，则进行拓展，否则进行裁剪，直至到达叶子结点，即找到最小代价解。
二、多分辨率金字塔
两帧雷达点云的相似区域并不会影响匹配的最终结果，但会参与计算，导致搜索效率降低，需要更多的迭代次数达到收敛。
当地图分辨率较低时，部分地图信息会被忽略，这种高、低分辨率下的差异，有助于对地图中的相似场景进行区分。实际使用中，首先将初始位姿对应的雷达点云与最上层（粗分辨率）的地图进行匹配，计算出当前分辨率下的位姿，并作为初始值进入次一级地图进行匹配，以此类推。