1 Hector简介

来源:从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析_hector 论文_李太白lx的博客

Hector是2011年开源的二维激光SLAM的项目,非常创新地使用scan-to-map的匹配方式.

首先其论文提出了如何对离散的栅格地图求导,然后说明了如何使用 高斯牛顿法 进行 scan-to-map 的计算.并且使用了多分辨率地图使计算结果更加准确.

hector的wiki主页为 http://wiki.ros.org/hector_mapping?distro=noetic

github上有人对hector的源码进行了注释,其地址为 https://github.com/tu-darmstadt-ros-pkg/hector_slam

2 代码实现

2.1 获取代码

代码已经提交在github上了,如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。

推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.

本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson4/src/hector_mapping/ 与 Creating-2D-laser-slam-from-scratch/lesson4/include/hector_mapping/

2.2 回调函数

接下来的代码 位于 Creating-2D-laser-slam-from-scratch/lesson4/src/hector_mapping/hector_mapping.cc

激光的回调函数里有4个步骤

  • 对hector_map_进行初始化与内存空间分配
  • 将雷达的数据类型转成hector需要的格式
  • 将雷达的数据类型写成hector的地图
  • 将hector的地图转换成ros中的地图格式并发布出去

这里的hector_map_每次都是重新进行初始化与析构,这也就导致了地图只是这一帧雷达数据的地图,不会与之前的雷达数据相关联.

可以将这里的new delete两行注释掉,然后将构造函数中对hector_map_进行初始化的语句解除注释.

这样的效果就是始终维护一个地图了,这个将在后续进行实验.

`// 回调函数
void HectorMapping::ScanCallback(const sensor_msgs::LaserScan &scan)
{
    if (!got_first_scan_)       // 如果是第一次接收scan
    {
        got_first_scan_ = true; // 改变第一帧的标志位
        map_frame_ = scan.header.frame_id;
    }

    // 0 初始化与申请内存空间
    // 当雷达数据到来时新建地图
    hector_map_ = new hectorslam::GridMap(resolution_, map_size_, offset_);

    // 1 将雷达的数据类型转成hector需要的格式
    ROSLaserScanToDataContainer(scan, laserScan_container_, resolution_);
    
    // 2 将雷达的数据类型写成hector的地图
    ComputeMap();

    // 3 将hector的地图转换成ros中的地图格式并发布出去
    PublishMap();
    
    // 4 析构与释放内存空间
    delete hector_map_;
}`

2.3 ComputeMap()

调用 updateByScanJustOnce() 进行hector地图的生成

`// 将雷达数据写成hector的地图
void HectorMapping::ComputeMap()
{
    Eigen::Vector3f robotPoseWorld(0, 0, 0);
    hector_map_mutex_.lock();
    hector_map_->updateByScanJustOnce(laserScan_container_, robotPoseWorld);
    hector_map_mutex_.unlock();
}`

2.4 PublishMap()

将hector格式的地图转换成ROS格式的地图.

这里的循环有个技巧,就是先用 memset() 函数将所有值设置为 -1 ,这样就可以只考虑 占用与空闲两种情况了.

这里的格式转换比上篇文章中的格式转换 快很多,不知道是不是上篇文章中在for循环内部声明变量的问题.

`// 将hector的地图转成ROS格式的地图并发布出去
void HectorMapping::PublishMap()
{
    int sizeX = hector_map_->getSizeX();
    int sizeY = hector_map_->getSizeY();
    int size = sizeX * sizeY;

    std::vector<int8_t> &data = map_.data;

    //std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop
    memset(&data[0], -1, sizeof(int8_t) * size);

    ros_map_mutex_.lock();

    for (int i = 0; i < size; ++i)
    {
        if (hector_map_->isFree(i))
        {
            data[i] = 0;
        }
        else if (hector_map_->isOccupied(i))
        {
            data[i] = 100;
        }
    }

    ros_map_mutex_.unlock();

    // 添加当前的时间戳
    map_.header.stamp = ros::Time::now();
    map_.header.frame_id = map_frame_;

    // 发布map和map_metadata
    map_publisher_.publish(map_);
    map_publisher_metadata_.publish(map_.info);
}`

2.5 进行bresemham画线

接下来的代码 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/OccGridMapBase.h

2.5.1 updateByScanJustOnce()

这段函数首先计算了画线的起始点的坐标索引.

我人为的将起始点坐标设置成了(800, 800), 这样就使得生成的地图始终是固定的了.

之后,进行雷达数据的遍历,求得每个雷达点对应的栅格地图的坐标,作为终点,在起点与终点间进行bresemham画线.

`void updateByScanJustOnce(const DataContainer &dataContainer, const Eigen::Vector3f &robotPoseWorld)
    {
        currMarkFreeIndex = currUpdateIndex + 1;
        currMarkOccIndex = currUpdateIndex + 2;

        //Get pose in map coordinates from pose in world coordinates
        // Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));
        Eigen::Vector3f mapPose(800, 800, 0);
        //Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector
        Eigen::Affine2f poseTransform((Eigen::Translation2f(
                                           mapPose[0], mapPose[1]) *
                                       Eigen::Rotation2Df(mapPose[2])));

        //Get start point of all laser beams in map coordinates (same for alle beams, stored in robot coords in dataContainer)
        Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());

        //Get integer vector of laser beams start point
        Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);

        //Get number of valid beams in current scan
        int numValidElems = dataContainer.getSize();

        //Iterate over all valid laser beams
        for (int i = 0; i < numValidElems; ++i)
        {
            //Get integer map coordinates of current beam endpoint
            Eigen::Vector2i scanEndMapi;
            scanEndMapi[0] = scanBeginMapi[0] + (int)round(dataContainer.getVecEntry(i)[0] / 0.05);
            scanEndMapi[1] = scanBeginMapi[1] + (int)round(dataContainer.getVecEntry(i)[1] / 0.05);

            //Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
            if (scanBeginMapi != scanEndMapi)
            {
                updateLineBresenhami(scanBeginMapi, scanEndMapi);
            }
        }

        //Tell the map that it has been updated
        this->setUpdated();

        //Increase update index (used for updating grid cells only once per incoming scan)
        currUpdateIndex += 3;
    }`

2.5.2 updateLineBresenhami()

这段函数首先验证起始点与终止点的坐标是否合法,如果不合法就跳过.

在正常的SLAM算法中,如果发现有雷达点超过了当前地图的边界,那就应该对地图的尺寸进行扩张,以包含新的雷达点,这一块后续的文章会有讲到.

之后,根据横纵坐标的大小,选定一个主方向,然后调用 bresenham2D() 进行 空闲值 的设置.

对终点额外使用 bresenhamCellOcc() 进行 占用值 的设置.

`// 设置画线的起点与终点
    inline void updateLineBresenhami(const Eigen::Vector2i &beginMap, const Eigen::Vector2i &endMap, unsigned int max_length = UINT_MAX)
    {
        int x0 = beginMap[0];
        int y0 = beginMap[1];

        //check if beam start point is inside map, cancel update if this is not the case
        if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY()))
        {
            return;
        }

        int x1 = endMap[0];
        int y1 = endMap[1];

        //check if beam end point is inside map, cancel update if this is not the case
        if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY()))
        {
            return;
        }

        int dx = x1 - x0;
        int dy = y1 - y0;

        unsigned int abs_dx = abs(dx);
        unsigned int abs_dy = abs(dy);

        int offset_dx = util::sign(dx);
        int offset_dy = util::sign(dy) * this->sizeX;

        unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();

        //if x is dominant
        if (abs_dx >= abs_dy)
        {
            int error_y = abs_dx / 2;
            bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
        }
        else
        {
            //otherwise y is dominant
            int error_x = abs_dy / 2;
            bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
        }

        // 将终点单独拿出来,设置占用
        unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();
        this->bresenhamCellOcc(endOffset);
    }`

2.5.3 bresenham2D()

这一块就是 bresenham算法的实现,与上篇文章中的代码差不多,这里不再进行说明.

`// 进行bresenham画线
    inline void bresenham2D(unsigned int abs_da, unsigned int abs_db, int error_b, 
                            int offset_a, int offset_b, 
                            unsigned int offset)
    {
        // https://www.jianshu.com/p/d63bf63a0e28

        // 先把起点格子设置成free
        this->bresenhamCellFree(offset);

        unsigned int end = abs_da - 1;

        for (unsigned int i = 0; i < end; ++i)
        {
            offset += offset_a;

            // 对应 Sub += dy/dx, 这里的实现是对 左右两边同乘 dx 后的结果
            error_b += abs_db;  

            // 判断 Sub > 0 
            if ((unsigned int)error_b >= abs_da)
            {
                offset += offset_b;

                // 对应Sub += dy/dx - 1, dy/dx 在之前加过了,所以这里只减 1 ,再左右两边同乘 dx
                error_b -= abs_da;  
            }
            // 再将路径上的其他点设置成free
            this->bresenhamCellFree(offset);
        }
    }`

2.5.4 bresenhamCellFree() & bresenhamCellOcc()

分别调用 updateSetFree() 与 updateSetOccupied() 对栅格值进行 空闲与占用 的设置.

`// 更新这个格子为空闲格子,只更新这一个格子
    inline void bresenhamCellFree(unsigned int offset)
    {
        ConcreteCellType &cell(this->getCell(offset));

        // 每一轮画线,每个格子只更新一次free
        if (cell.updateIndex < currMarkFreeIndex)
        {
            concreteGridFunctions.updateSetFree(cell);
            cell.updateIndex = currMarkFreeIndex;
        }
    }

    // 更新这个格子为占用格子,只更新这一个格子
    inline void bresenhamCellOcc(unsigned int offset)
    {
        ConcreteCellType &cell(this->getCell(offset));

        // 每一轮画线,每个格子只更新一次占用
        if (cell.updateIndex < currMarkOccIndex)
        {
            // 如果这个格子被设置成free了,先取消free,再设置占用
            if (cell.updateIndex == currMarkFreeIndex)
            {
                concreteGridFunctions.updateUnsetFree(cell);
            }
            concreteGridFunctions.updateSetOccupied(cell);
            cell.updateIndex = currMarkOccIndex;
        }
    }`

2.6 Hector中的地图数据类型

2.6.1 地图类

代码位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/GridMapBase.h

可以看到,栅格地图的表示依然是一个一维指针 ConcreteCellType * mapArray.

ConcreteCellType 在这里是模板参数类型,具体实现在下一节进行讲解.

同时,使用 MapDimensionProperties 类保存栅格地图的尺寸信息.

`template <typename ConcreteCellType>
class GridMapBase
{
protected:
    ConcreteCellType *mapArray; ///< Map representation used with plain pointer array.

    float scaleToMap; ///< Scaling factor from world to map.

    Eigen::Affine2f worldTmap;   ///< Homogenous 2D transform from map to world coordinates.
    Eigen::Affine3f worldTmap3D; ///< Homogenous 3D transform from map to world coordinates.
    Eigen::Affine2f mapTworld;   ///< Homogenous 2D transform from world to map coordinates.

    MapDimensionProperties mapDimensionProperties;
    int sizeX;

private:
    int lastUpdateIndex;`

2.6.2 地图占用值更新的实现

上一篇文章中,GMapping是通过 击中次数与观测次数 的比值作为栅格地图中每个格子的值.

这篇文章展示了一种更方便的表示方式.

代码位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/GridMapLogOdds.h

`/**
 * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map.
 */
class GridMapLogOddsFunctions
{
public:
    /**
   * Constructor, sets parameters like free and occupied log odds ratios.
   */
    GridMapLogOddsFunctions()
    {
        this->setUpdateFreeFactor(0.4f);
        this->setUpdateOccupiedFactor(0.6f);
    }

    /**
   * Update cell as occupied
   * @param cell The cell.
   */
    void updateSetOccupied(LogOddsCell &cell) const
    {
        if (cell.logOddsVal < 50.0f)
        {
            cell.logOddsVal += logOddsOccupied;
        }
    }

    /**
   * Update cell as free
   * @param cell The cell.
   */
    void updateSetFree(LogOddsCell &cell) const
    {

        cell.logOddsVal += logOddsFree;
    }

    void updateUnsetFree(LogOddsCell &cell) const
    {
        cell.logOddsVal -= logOddsFree;
    }

    /**
   * Get the probability value represented by the grid cell.
   * @param cell The cell.
   * @return The probability
   */
    float getGridProbability(const LogOddsCell &cell) const
    {
        float odds = exp(cell.logOddsVal);
        return odds / (odds + 1.0f);
    }

    void setUpdateFreeFactor(float factor)
    {
        logOddsFree = probToLogOdds(factor);
    }

    void setUpdateOccupiedFactor(float factor)
    {
        logOddsOccupied = probToLogOdds(factor);
    }

protected:
    float probToLogOdds(float prob)
    {
        float odds = prob / (1.0f - prob);
        return log(odds);
    }

    float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied
    float logOddsFree;     /// < The log odds representation of probability used for updating cells as free
};`

栅格地图中每个格子存储的值 表示这个格子是障碍物的概率.但是将这个概率值 通过 probToLogOdds() 函数进行变换了,转换后的形式在更新数值方面更加方便,只需要进行加法就行了.

鉴于有更好的文章对这块进行了讲解,我就不再进行详细说明了,推荐阅读https://zhuanlan.zhihu.com/p/21738718

接下来,举个例子说明一下栅格地图的占用值更新的方式.

例如:

上述代码里将 更新占用值的因子 设置为0.6,将 更新空闲值的因子 设置为0.4, 分别使用 probToLogOdds() 函数进行转换之后得到:

占用值更新量 logOddsOccupied = 0.405465 空闲值更新量 logOddsFree = -0.405465 最后这两个值一正一负.

如果是更新占用,就将这个格子的值加上 logOddsOccupied , 也就是加上 0.405465 ; 如果是更新空闲,就将这个格子的值加上 logOddsFree,也就是减去 -0.405465;

最后通过累加后的值 是否大于零 来决定是否作为障碍物.如果大于零,就当做是障碍物,设置成 黑色格子,如果小于零,就当做是可通过区域,设置成 白色格子.

这样就完成了每个栅格值的累加与更新.

上述代码将 占用值更新量 与 空闲值更新量 设置为数值上大小相等,这样导致的结果就是,格子已经更新成占用了,仅通过一次更新空闲就可以将格子的状态从 占用 转变下去.

实际中,也可以将这两个值设置成大小不一样的,这样在生成地图的时候就会有偏重,可以更容易生成空闲或更容易生成障碍物.

注意

可能有同学不清楚为什么设置一遍栅格状态之后还要更新栅格的值.

这是因为,不管是什么传感器,不管是什么算法,都是存在误差的.

这就导致了不能通过一次两次的更新之后的值,来代表这个栅格是占用还是空闲的,因为这个状态十分有可能是错的.

只有通过长时间的累计之后,最后通过这个值是否大于零,来近似地将这个栅格设置为是否是障碍物,这时候依然不能百分百地确定这个栅格是否就是障碍物.因为,有可能是人腿走过留下的痕迹.

所以,这就是那本非常出名的书叫做 <<概率机器人>>,现在的机器人建图与定位算法基本都是与概率相关,只能按照概率最大的值作为最好值,而不是计算出真实值.

3 运行

3.1 运行

本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得,并将launch中的bag_filename更改成您实际的目录名。

通过如下命令运行本篇文章对应的程序

`roslaunch lesson4 make_hector_map.launch`

3.2 运行结果

3.2.1 生成单次地图

启动之后,会在rviz中显示出如下画面.

地图是不断地根据雷达数据进行更新的,每次生成的地图只是这一帧雷达数据转换后的结果,不会累加之前的雷达数据.

在这里插入图片描述 同时会在终端中打印出如下消息.

`初始化与申请内存空间用时: 0.0303726 秒。
转换数据用时: 0.000160402 秒。
计算hector地图用时: 0.00198889 秒。
转换成ros地图用时: 0.0517925 秒。
析构与释放内存空间用时: 8.089e-06 秒。

初始化与申请内存空间用时: 0.0308826 秒。
转换数据用时: 0.000163696 秒。
计算hector地图用时: 0.00205209 秒。
转换成ros地图用时: 0.0492935 秒。
析构与释放内存空间用时: 7.512e-06 秒。`

大概处理一帧雷达数据生成的地图耗时 0.09s 左右,这对比上篇文章中构建GMapping的耗时 0.4s 已经快很多了.

3.2.2 累加的地图

按照 2.2 节所说,将回调函数的 new 与 delete两行注释掉,然后将构造函数中对hector_map_进行初始化的语句解除注释.

这样就是对 hector_map_ 只进行一次初始化,从始至终维持一张地图.

在同一张地图上不断地进行栅格地图的更新.

其结果如下所示,将产生严重的叠图现象.

在这里插入图片描述 终端中打印出如下消息.

`初始化与申请内存空间用时: 2.04e-07 秒。
转换数据用时: 0.000567961 秒。
计算hector地图用时: 0.00381367 秒。
转换成ros地图用时: 0.0547162 秒。
析构与释放内存空间用时: 6.9e-08 秒。

初始化与申请内存空间用时: 1.97e-07 秒。
转换数据用时: 0.000578334 秒。
计算hector地图用时: 0.00362355 秒。
转换成ros地图用时: 0.0509386 秒。
析构与释放内存空间用时: 7.9e-08 秒。`

可以看到,申请内存的时间没有了,程序执行的更快了.

这个实验可以说明SLAM的本质是什么.

就是找到适当的位置(也就是定位),在这个位置上将雷达数据写成栅格地图.如果每次找到的位置都很准确,那就能构建出十分完美的地图.

3.3 结果分析

可以通过如下命令来看下map这个topic的频率

`$ rostopic hz /map /laser_scan
   topic       rate   min_delta   max_delta   std_dev    window
===============================================================
/map          10.12   0.03021     0.1309      0.01243    71    
/laser_scan   10.0    0.09056     0.1037      0.002755   71`

可以看到,同样的地图大小,使用hector的方式构建地图,可以达到10hz的频率,而使用gmapping的方式构建地图,只能达到4hz.

4 总结与Next

本篇文章简要地对Hector中的地图构建模块进行了重新实现.

并说明了Hector中的地图格式以及其地图的更新方式,感受到了其计算地图模块更加快速.

下一篇文章将继续基于Hector进行实现,将进行 scan-to-map 的具体实现,并且对其高斯牛顿求解的过程进行说明.


文章将在 公众号: 从零开始搭SLAM 进行同步更新,欢迎大家关注,可以在公众号中添加我的微信,进激光SLAM交流群,大家一起交流SLAM技术。

同时,也希望您将这个公众号推荐给您身边做激光SLAM的人们,大家共同进步。

如果您对我写的文章有什么建议,或者想要看哪方面功能如何实现的,请直接在公众号中回复,我可以收到,并将认真考虑您的建议。

在这里插入图片描述

最后修改:2023 年 11 月 10 日
如果觉得我的文章对你有用,请随意赞赏