Hector SLAM算法学习与代码解析

Hector SLAM算法学习与代码解析

最初了解到Hector SLAM 是从https://www.youtube.com/embed/F8pdObV_df4看到手持激光建图，被吸引到了也就想看一下具体的实现。关于Hector SLAM算法有一篇论文：A Flexible and Scalable SLAM System with Full 3D Motion Estimation；以及开源代码：https://github.com/tu-darmstadt-ros-pkg/hector_slam。对照着论文和代码，一睹算法真容，主要分享对论文的第四部分2D SLAM的理解。

---

Occupancy Grid Map

Map Access

初始化的时候，将获取的激光数据作为第一帧处理映射到地图中。在t时刻，激光获取到新的激光数据，想要与t-1时刻的地图匹配，首先要把激光数据变换到栅格地图之中；例如激光点Pm变换到栅格地图中，我们希望的是激光点能够到一个“被占据”的栅格（灰色）中，如果t时刻所有的激光点都能够变换到被占据的栅格中，也就意味着匹配成功。
为了刻画激光点映射到栅格地图中是否对应被占据栅格中以及对应的程度，采用了双线性插值的方法，求出M(Pm)。M（P00)->M(P11)的范围是0 到 1，表示被占据的概率。

对应的程序实现：

 Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)
  {
    //check if coords are within map limits.
    if (concreteGridMap->pointOutOfMapBounds(coords)){
      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
    }

    //map coords are always positive, floor them by casting to int
    Eigen::Vector2i indMin(coords.cast<int>());

    //get factors for bilinear interpolation
    Eigen::Vector2f factors(coords - indMin.cast<float>());

    int sizeX = concreteGridMap->getSizeX();

    int index = indMin[1] * sizeX + indMin[0];

    // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
    // filter gridPoint with gaussian