slam java_SLAM——hector-slam算法原理解析

1、hector-slam代码框架概述

下载源码：git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

原理解读参照：https://blog.csdn.net/weixin_40047925/article/details/80679496

其中包含了许多用于仿真的文件，hector slam算法主要集中在hector mapping包中，同时核心的位姿估计和点云建立栅格图操作主要在文件HectorSlamProcessor.h的函数update中。如下：

函数newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov))根据输入的激光点云数据和现有的栅格图进行对齐，这部分是整个代码的核心，主要思想是使用高斯牛顿优化代价函数，求取让代价函数达到最优的位姿偏量，从而求出当前机器人的位姿态。

当然，如果已经知道机器人的准确位姿不需要进行上面的位姿解算，直接用函数mapRep->updateByScan(dataContainer, newPoseEstimateWorld)更新栅格地图。这部分算法原理比较简单，主要两个方面：一是，用对数表达每一个子栅格的概率值；二是，用Bresenham 画线算法来更新子栅格的概率值。

为了说明上述算法原理，先介绍一部分的预备知识，然后再切入正题。

2、hector-slam预备知识

2-1 占用栅格地图

这里直接使用上述参考文章进行说明，假设机器人的位姿已知，那么经过每一次激光数据扫描，真实障碍物在栅格地