从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建

上篇文章讲解了如何在ROS中发布栅格地图，以及如何向栅格地图赋值．

这篇文章来讲讲如何将激光雷达的数据构建成栅格地图．

雷达的数据点所在位置表示为占用，从雷达开始到这点之间的区域表示为空闲．

1 GMapping简介

GMapping是ROS中navigation导航包集中推荐的二维建图算法包，由于其实现时间早，所以各种书中的demo使用的SLAM基本都是GMapping，同时GMapping网上的教程也是最多的．

GMapping是基于粒子滤波算法实现的SLAM，通过里程计数据获取粒子群的先验位姿，再通过雷达数据与地图的匹配程度对所有粒子进行打分，通过分数高的粒子群来近似机器人的真实位姿．

GMapping的具体实现是在open_gmapping包里，后来又在ROS中做了个封装包slam_gmapping．gmapping在ROS中的wiki地址为 http://wiki.ros.org/gmapping

2 代码

open_gmapping的代码比较复杂，比较乱． csdn博主 白茶-清欢 对 open_gmapping 与 slam_gmapping 两个包进行了重写，整理了代码使得代码结构更加清晰，同时添加了注释，还增加了激光雷达数据的畸变校正功能．

本篇文章的代码实现是参考于 csdn博主 白茶-清欢 注释简化之后的GMapping．其地址为 csdn 白茶-清欢: https://blog.csdn.net/zhao_ke_xue/article/details/109712355

2.1 获取代码

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

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

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

2.2 回调函数

这个函数先进行角度值的cos与sin的计算，然后调用PublishMap()计算地图并发布出去．

// 回调函数 进行数据处理
void GMapping::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
     
    static ros::Time last_map_update(0, 0); //存储上一次地图更新的时间

    if (!got_first_scan_) //如果是第一次接收scan
    {
     
        // 将雷达各个角度的sin与cos值保存下来，以节约计算量
        CreateCache(scan_msg);
        got_first_scan_ = true; //改变第一帧的标志位
    }

    start_time_ = std::chrono::steady_clock::now();

    // 计算当前雷达数据对应的栅格地图并发布出去
    PublishMap(scan_msg);

    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "\n转换一次地图用时: " << time_used_.count() << " 秒。" << std::endl;
}

2.3 PublishMap()

这里的ScanMatcherMap为GMapping中存储地图的数据类型，先声明了一个ScanMatcherMap的对象gmapping_map_，然后通过ComputeMap()为gmapping_map_赋值，然后再将gmapping_map_中存储的值赋值到ros的栅格地图的数据类型中．

gmapping认为ros的栅格地图数据只需要有3个值

• -1 代表栅格状态未知
• 0 代表栅格是空闲的，代表可通过区域
• 100 代表栅格是占用的，代表障碍物，不可通过

#define GMAPPING_UNKNOWN (-1)
#define GMAPPING_FREE (0)
#define GMAPPING_OCC (100)

// 计算当前雷达数据对应的栅格地图并发布出去
void GMapping::PublishMap(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
     
    // 地图的中点
    Point center;
    center.x = (xmin_ + xmax_) / 2.0;
    center.y = (ymin_ + ymax_) / 2.0;

    // ScanMatcherMap为GMapping中存储地图的数据类型
    ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_);
    
    // 使用当前雷达数据更新GMapping地图中栅格的值
    ComputeMap(gmapping_map_, scan_msg);

    // 将gmapping_map_中的存储的栅格值 赋值到 ros的map中
    for (int x = 0; x < gmapping_map_.getMapSizeX(); x++)
    {
     
        for (int y = 0; y < gmapping_map_.getMapSizeY(); y++)
        {
     
            IntPoint p(x, y);
            // 获取这点栅格的值，只有大于occ_thresh_时才认为是占用
            double occ = gmapping_map_.cell(p); 

            // 未知
            if (occ < 0)
                map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_UNKNOWN;
            // 占用
            else if (occ > occ_thresh_) // 默认0.25
                map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_OCC;
            // 空闲
            else
                map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_FREE;
        }
    }

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

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

2.4 ComputeMap()

这部分代码分为2个部分，第一部分为计算出地图的储存空间，并且计算出从雷达到激光点这条线在gmapping栅格地图中的坐标，以及雷达点的坐标．第二部分为将计算好的直线与点在gmapping地图中进行栅格值的更新．

// 使用当前雷达数据更新GMapping地图中栅格的值
void GMapping::ComputeMap(ScanMatcherMap &map, const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
     
    line_lists_.clear();
    hit_lists_.clear();
    
    // lp为地图坐标系下的激光雷达坐标系的位姿
    OrientedPoint lp(0, 0, 0.0);
    // 将位姿lp转换成地图坐标系下的位置
    IntPoint p0 = map.world2map(lp);
    
	// 第一部分
    // 地图的有效区域(地图坐标系)
    HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
    // 通过激光雷达的数据，找出地图的有效区域
    for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
    {
     
        // 排除错误的激光点
        double d = scan_msg->ranges[i];
        if (d > max_range_ || d == 0.0 || !std::isfinite(d))
            continue;
        if (d > max_use_range_)
            d = max_use_range_;

        // p1为激光雷达的数据点在地图坐标系下的坐标
        Point phit = lp;
        phit.x += d * a_cos_[i];
        phit.y += d * a_sin_[i];
        IntPoint p1 = map.world2map(phit);

        // 使用bresenham算法来计算 从激光位置到激光点 要经过的栅格的坐标
        GridLineTraversalLine line;
        GridLineTraversal::gridLine(p0, p1, &line);
        // 将line保存起来以备后用
        line_lists_.push_back(line);

        // 计算活动区域的大小
        for (int i = 0; i < line.num_points - 1; i++)
        {
     
            activeArea.insert(map.storage().patchIndexes(line.points[i]));
        }
        // 如果d
        // 同时如果d==max_use_range_那么说明这个值只用来进行标记空闲区域　不用来进行标记障碍物
        if (d < max_use_range_)
        {
     
            IntPoint cp = map.storage().patchIndexes(p1);
            activeArea.insert(cp);
            hit_lists_.push_back(phit);
        }
    }

    // 为activeArea分配内存
    map.storage().setActiveArea(activeArea, true);
    map.storage().allocActiveArea();

	// 第二部分
    // 在map上更新空闲点
    for (auto line : line_lists_)
    {
     
        // 更新空闲位置
        for (int k = 0; k < line.num_points - 1; k++)
        {
     
            // 未击中，就不记录击中的位置了，所以传入参数Point(0,0)
            map.cell(line.points[k]).update(false, Point(0, 0));
        }
    }
    // 在map上添加hit点
    for (auto hit : hit_lists_)
    {
     
        IntPoint p1 = map.world2map(hit);
        map.cell(p1).update(true, hit);
    }
}

代码中的注释已经说明的很清楚了，这里简要说明一下．

第一部分

首先生成了一个activeArea变量，用于记录需要区域的范围．

之后遍历雷达数据点，使用bresemham画线算法生成从激光位置到激光点的连线在栅格地图中的坐标，并将这些点放入activeArea用于计算区域，同时保存下来以备后用．

再判断雷达数据点也就是 phit ．判断这个点与预先设定的参数 max_use_range_(雷达数据最大使用距离)，如果phit的距离小于max_use_range_则认为是好的数据点，放入hit_lists_中以备后用．

最后，设置区域的大小以及分配内存．

第二部分

第二部分做了两个工作，一个是更新空闲点，代表可通过区域，另一个是更新hit点，也就是激光雷达数据点在地图中的位置，代表着障碍物．

这部分之前是进行了2次for循环，做了很多无用的计算，我进行优化了一下，现在只需要进行一次for循环．

2.5 bresemham画线算法

以下函数位于Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/grid/gridlinetraversal.h文件中．

2.5.1 gridLine()

上述代码调用了gridLine()函数，这个函数调用了gridLineCore()进行直线坐标点的计算，

然后判断了计算出的直线坐标点的起始点是否正确，如果不正确，就把直线的所有点顺序反转一下，

void GridLineTraversal::gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{
     
    int i, j;
    int half;
    IntPoint v;
    gridLineCore(start, end, line);
    if (start.x != line->points[0].x || start.y != line->points[0].y)
    {
     
        half = line->num_points / 2;
        for (i = 0, j = line->num_points - 1; i < half; i++, j--)
        {
     
            v = line->points[i];
            line->points[i] = line->points[j];
            line->points[j] = v;
        }
    }
}

2.5.2 gridLineCore()

这个函数就是实际bresemham画线算法的实现，代码挺长，但是挺简单的，只是区分了很多情况：斜率是否大于1，以及增长的顺序的方向判断等等，具体的意思已经在代码注释的很清楚了.

如果您想弄清楚理论过程，理解的更透彻一些，可以参看这篇文章 https://www.jianshu.com/p/d63bf63a0e28

网上的教程也很多，我这就不再讲了．

void GridLineTraversal::gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{
     
    int dx, dy;             // 横纵坐标间距
    int incr1, incr2;       // 增量
    int d;                  // 
    int x, y, xend, yend;   // 直线增长的首末端点坐标
    int xdirflag, ydirflag; // 横纵坐标增长方向
    int cnt = 0;            // 直线过点的点的序号

    dx = abs(end.x - start.x);
    dy = abs(end.y - start.y);

    // 斜率绝对值小于等于1的情况，优先增加x
    if (dy <= dx)
    {
     
        d = 2 * dy - dx;       // 初始点P_m0值
        incr1 = 2 * dy;        // 情况（1）
        incr2 = 2 * (dy - dx); // 情况（2）

        // 将增长起点设置为横坐标小的点处，将 x的增长方向 设置为 向右侧增长
        if (start.x > end.x)
        {
     
            // 起点横坐标比终点横坐标大，ydirflag = -1（负号可以理解为增长方向与直线始终点方向相反）
            x = end.x; 
            y = end.y;
            ydirflag = (-1);
            xend = start.x; // 设置增长终点横坐标
        }
        else
        {
     
            x = start.x;
            y = start.y;
            ydirflag = 1;
            xend = end.x;
        }

        //加入起点坐标
        line->points.push_back(IntPoint(x, y));
        cnt++;

        // 向 右上 方向增长
        if (((end.y - start.y) * ydirflag) > 0)
        {
     
            while (x < xend)
            {
     
                x++;
                if (d < 0)
                {
     
                    d += incr1;
                }
                else
                {
     
                    y++;
                    d += incr2; // 纵坐标向正方向增长
                }
                line->points.push_back(IntPoint(x, y));
                cnt++;
            }
        }
        // 向 右下 方向增长
        else
        {
     
            while (x < xend)
            {
     
                x++;
                if (d < 0)
                {
     
                    d += incr1;
                }
                else
                {
     
                    y--;
                    d += incr2; // 纵坐标向负方向增长
                }
                line->points.push_back(IntPoint(x, y));
                cnt++;
            }
        }
    }
    // 斜率绝对值大于1的情况，优先增加y
    else
    {
     
        // dy > dx，当斜率k的绝对值|k|>1时，在y方向进行单位步进
        d = 2 * dx - dy; 
        incr1 = 2 * dx;  
        incr2 = 2 * (dx - dy);

        // 将增长起点设置为纵坐标小的点处，将 y的增长方向 设置为 向上侧增长
        if (start.y > end.y)
        {
     
            y = end.y; // 取最小的纵坐标作为起点
            x = end.x;
            yend = start.y;
            xdirflag = (-1); 
        }
        else
        {
     
            y = start.y;
            x = start.x;
            yend = end.y;
            xdirflag = 1;
        }
        // 添加起点
        line->points.push_back(IntPoint(x, y));
        cnt++;
        // 向 上右 增长
        if (((end.x - start.x) * xdirflag) > 0)
        {
     
            while (y < yend)
            {
     
                y++;
                if (d < 0)
                {
     
                    d += incr1;
                }
                else
                {
     
                    x++;
                    d += incr2; // 横坐标向正方向增长
                }
                //添加新的点
                line->points.push_back(IntPoint(x, y));
                cnt++;
            }
        }
        // 向 上左 增长
        else
        {
     
            while (y < yend)
            {
     
                y++;
                if (d < 0)
                {
     
                    d += incr1;
                }
                else
                {
     
                    x--;
                    d += incr2; //横坐标向负方向增长
                }
                line->points.push_back(IntPoint(x, y));
                // 记录添加所有点的数目
                cnt++;
            }
        }
    }
    line->num_points = cnt;
}

2.6 GMapping中的地图数据类型

我将GMapping中的地图数据类放在了
Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/grid 文件夹下，以及依赖的点的类文件放在了
Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/utils 文件夹下.

2.6.1 地图类

GMapping的地图相关的文件有3个，array.h，harray2d.h，map.h．

具体的代码实现我就不在这里说明了，代码挺多的，我也没太看懂（哈哈）。

感兴趣的同学可以仔细阅读一下，这里的代码都有注释的，再次感谢白茶清欢小哥的工作．

GMapping的代码比较老了，这块的实现可以使用c++ 11的stl进行更好的实现，这也是我不对这个代码细讲的原因，因为之后会有更好的实现．

2.6.2 地图占用值更新的实现

map.h中有个叫做PointAccumulator的结构体，定义了栅格地图的更新方式以及存储值是如何计算的．其代码如下

//PointAccumulator表示地图中一个cell（栅格）包括的内容
/*
acc：   栅格累计被击中位置
n：     栅格被击中次数
visits：栅格被访问的次数
*/
//PointAccumulator的一个对象，就是一个栅格，gmapping中其他类模板的cell就是这个

struct PointAccumulator
{
     
    //float类型的point
    typedef point<float> FloatPoint;
    //构造函数
    PointAccumulator() : acc(0, 0), n(0), visits(0) {
     }
    PointAccumulator(int i) : acc(0, 0), n(0), visits(0) {
      assert(i == -1); }
    //计算栅格被击中坐标累计值的平均值
    inline Point mean() const {
      return 1. / n * Point(acc.x, acc.y); }
    //返回该栅格被占用的概率，范围是 -1（没有访问过） 、[0,1]
    inline operator double() const {
      return visits ? (double)n * 1 / (double)visits : -1; }
    //更新该栅格成员变量
    inline void update(bool value, const Point &p = Point(0, 0));
    //该栅格被击中的位置累计，最后取累计值的均值
    FloatPoint acc;
    //n表示该栅格被击中的次数，visits表示该栅格被访问的次数
    int n, visits;
};

//更新该栅格成员变量，value表示该栅格是否被击中，击中n++，未击中仅visits++;
void PointAccumulator::update(bool value, const Point &p)
{
     
    if (value)
    {
     
        acc.x += static_cast<float>(p.x);
        acc.y += static_cast<float>(p.y);
        n++;
        visits += 1;
    }
    else
        visits++;
}

可以看到，每个格子都有2个值，一个是visits，一个是n．

更新占用: 在更新被击中的栅格时, 也就是激光点所在的栅格, n与visits都会加1，

更新空闲: 在更新未被击中的栅格时，也就是从激光到激光点之间的栅格，只有visits加1,

通过重载了()，获取获取每个格子的占用值，也可以说成是被占用的概率．占用值的计算公式为 n/visists，如果是没被更新过就返回-1．

由于visits大于等于n，所以占用值的范围是 [0,1]的，只有当这个值在大于0.25（默认参数）时，在赋值到ros地图的时候才赋值为100，才认为是真正的占用了．

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

例如：当雷达扫到人腿时，会对击中点的栅格更新一次占用，这时在ROS地图下这个点将被表示为障碍物．

如果人腿离开了这个位置，在之后的过程中，有4次在这个栅格更新了空闲，就会将ROS地图中这个栅格的状态更新到空闲，也就是没有障碍物．

原因：

第一次更新地图时，这个格子的占用值为 1/1 = 1 > 0.25 ，就会将ROS地图中这个格子设置为100，表示障碍物．

如果之后4次更新地图，这个栅格都被更新空闲了，则这个格子的占用值将变为 1/5 = 0.2 <0.25了，所以就会将ROS地图中对应的栅格设置为0，变成可通过区域了．

3 运行

3.1 launch文件

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

<launch>

    <!-- bag的地址与名称 -->
    <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>

    <!-- 使用bag的时间戳 -->
    <param name="use_sim_time" value="true" />

    <!-- 启动节点 -->
    <node name="lesson4_gmapping_node"
        pkg="lesson4" type="lesson4_gmapping_node" output="screen" />
    
    <!-- launch rviz -->
    <node name="rviz" pkg="rviz" type="rviz" required="true"
        args="-d $(find lesson4)/config/gmapping.rviz" />

    <!-- play bagfile -->
    <node name="playbag" pkg="rosbag" type="play"
        args="--clock $(arg bag_filename)" />

</launch>

3.2 编译与运行

下载代码后，请放入您自己的工作空间中，通过 catkin_make 进行编译．

由于是新增的包，所以需要通过 rospack profile 命令让ros找到这个新的包．

之后, 使用source命令，添加ros与工作空间的地址到当前终端下，再通过如下命令运行本篇文章对应的程序

roslaunch lesson4 make_gmapping_map.launch

3.3 运行结果

启动之后，会在rviz中显示出如下画面．

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

同时会在终端中打印出如下消息．

转换一次地图用时: 0.403898 秒。

转换一次地图用时: 0.394922 秒。

转换一次地图用时: 0.40791 秒。

3.4 结果分析

通过终端打印出来的信息可以得知，每次计算gmapping地图，再将gmapping地图的值赋值到ros的地图中，再发布出来．这一系列操作大概需要花费0.4秒，这是一个很长的时间了．

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

$ rostopic hz /laser_scan /map

   topic       rate   min_delta   max_delta   std_dev    window
===============================================================
/laser_scan   9.987   0.09058     0.1108      0.003163   50    
/map          2.613   0.1914      0.4549      0.06057    50    

可以看到，雷达数据是10hz的，map数据大概是2.5hz．

我们只用了一个线程，也就是雷达回调函数的这个线程．处理一次回调函数需要用时0.4秒，而雷达数据的间隔是0.1秒．

也就是说，当我们正在计算地图的时候，会又有4帧雷达数据到达，当回调函数的缓冲区设置为1时，那这4帧雷达数据将只保留最后一个，其他3帧数据将丢失掉．

这还只是80m * 80m范围的地图，构建更大范围地图的时间将更久．所以，很多SLAM都将地图的生成单独开一个线程，以保证不耽误对实时性要求较高的前端里程计部分．

4 总结与Next

通过使用GMapping中的地图数据格式，以及GMapping中的地图计算方式，我们实现了将激光雷达数据构建成栅格地图的功能，虽然现在的建图是单次的．

希望你通过这篇文章以及代码，知道了如何将激光雷达数据写成栅格地图，知道了栅格地图更新一次的耗时问题．

下一篇文章还是进行单次栅格地图的构建，只不过下次将使用Hector中构建地图的方式，为我们以后做scan-to-map做个基础．

---

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

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

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