qq_21043585的博客

今天讲一篇经典的LIO（LiDAR-Inertial Odometry）SLAM论文——LIO-SAM。雷达+里程计由于其高鲁棒性、较低成本（二维雷达不贵、三维雷达价格也在降、IMU不太贵），可以说是工业机器人应用最广泛的SLAM解决方案了。对于三维LiDAR+IMU的结局方案，在github上的开源算法并不多，甚至可以说就LIO_SAM和FAST_LIO两种（及它们的前身和衍生物）。

第5行，根据目前移动到的位置，以及在这个位置下激光打到的结果，打个分数（比如激光打到地方在地图上也是障碍物+1分这种），这样可以给每个粒子计算一个权重，乘上之前的权重，权重最高的那个粒子就会被认为是目前的结果。8-11行是在进行重采样，因为随着算法进行，有些粒子权重会非常低，几乎不能表示真是情况，且消耗计算资源，因此我们会将权重高的粒子多复制几份，权重低的可能就丢弃了。我们直接从粒子滤波框架开始讲起（我会讲的很浅显，即便你不会粒子滤波，但是要真正掌握还是要认真看推导，比如去看概率机器人）。

它的主要工作在后端，使用分支定界优化了CSM算法，然后构建了位姿和submap之间的约束图，使用SPA（一种利用图稀疏性来加速优化，这可以说是真正将图优化投入实际使用的算法，学习到一定深度了可以再去学习，总之是一种快速进行图优化的算法）进行优化。显然，这种算法复杂度很高。其实我们很容易想到一种简单的算法（我瞎想的，便于理解），假设一开始x搜索范围之[-10,10]，步长为1，搜一百次，取分数最高的10次，这10次分数都在[-1,1]之间，那我改步长为0.1搜索新的区间。就是前端，核心就是优化这个函数，