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

上周搬家, 导致这篇文章更新的慢了点.

之前的文章我们都是通过scan-to-scan的方式进行位姿变换的计算. 接下来的文章将带领大家体验scan-to-map的计算位姿变换的方式.

首先, 来简要介绍一下什么是map.

1 地图与占用栅格地图

1.1 人眼中的地图

地图对于大部分人来说, 第一时间想到的应该是高德地图中的地图, 是一副平面的或者立体的, 能够标识出从一个地点到另一个地点的可通行路径.

地图对于人来说其实提供了2个方面的功能, 第一个就是提供一个可通行路径, 还有一个其实是定位.

当你下了公交或者出了地铁等, 发现自己不知道自己在哪了, 你的第一反应是什么? 拿出手机, 打开GPS, 看看高德地图中自己的位置在哪, 以及搜一下如何从自己现在的位置到自己想去的位置.这就是GPS定位.

如果没有GPS怎么办呢? 一般情况下, 大家都会在地图中选一个明显的或者好认的建筑, 来判断自己与这个建筑的位置关系, 从而确定出自己在地图中的位置. 确定了位置之后就可以去自己想去的位置去了.

举个例子, 当你朋友给你打电话说我迷路了, 我不知道在哪. 大部分人的第一反应应该是让朋友说一下旁边有什么明显的建筑, 如某某商场, 某某店的名字等等, 然后你通过某某商场, 某某店的名字确定了朋友的大致位置.

1.2 机器人眼中的地图

对于机器人来说也是一样的, 只不过机器人眼中的地图在表示上与高德地图有些不同.

机器人眼中的地图的表示形式有很多, 可以是三维点云地图, 也可以是二维点云地图, 也可以是二维栅格地图, 也可以是拓扑地图, 也可以是语义地图等等.

下面展示了一些地图的图片.
上图为二维激光SLAM构建的二维栅格地图

上图为三维激光SLAM构建的三维点云地图

上图为视觉SLAM(ORB-SLAM2)构建的稀疏点云地图

上图为视觉SLAM构建稠密点云地图

其中, 三维激光SLAM与视觉SLAM构建的地图一般都是三维点云地图, 一副完全由离散的三维空间点组成的地图.

二维激光SLAM构建的地图为二维栅格地图,

1.3 栅格地图

栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义.

ROS的栅格地图使用白色代表空闲，也就是可通过区域，其存储的值为 0；

黑色代表占用，也就是不可通过区域，其存储的值为 100；

灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1．

栅格地图由于其 占用与空闲的表示方法，在ROS中又被称为占用地图．

栅格地图的示意图如下图所示：
图片引用于[csdn 白茶-清欢: https://blog.csdn.net/zhao_ke_xue/article/details/110919883]

2 ROS中的数据类型

接下来看一下ROS中的栅格地图的消息类型，

$ rosmsg show nav_msgs/OccupancyGrid 
std_msgs/Header header          # 数据的消息头
  uint32 seq                    # 数据的序号
  time stamp                    # 数据的时间戳
  string frame_id               # 地图的坐标系
nav_msgs/MapMetaData info       # 地图的一些信息
  time map_load_time            # 加载地图的时间
  float32 resolution            # 地图的分辨率，一个格子代表着多少米，一般为0.05，[m/cell]
  uint32 width                  # 地图的宽度，像素的个数, [cells]
  uint32 height                 # 地图的高度，像素的个数, [cells]
  geometry_msgs/Pose origin     # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
# 地图数据，优先累加行，从（0,0）开始。占用值的范围为[0,100]，未知为-1。
int8[] data    

可以看到，消息可以分为3个部分，消息头header，地图信息info，地图数据data．

地图信息info储存了地图相关的信息，包括 加载地图的时间，地图的分辨率，地图的宽度与高度，以及地图左下角栅格对应的物理坐标．

地图本身是只有像素坐标的，其像素坐标系为坐下角为(0, 0) 的坐标系．通过左下角栅格对应的物理坐标 origin 以及 分辨率，再通过 像素 * 分辨率 + origin , 将像素坐标转成物理世界的坐标，从而确定了整个地图的物理坐标．

地图数据data是一维的，我们在赋值之前要首先对这个一维数组进行初始化，数据的大小就是所有像素的个数．

遍历的时候要注意方向，这个数据是以行为主要递增方向的．也就是说遍历的时候要先遍历第一行的所有数据，然后再遍历第二行的所有数据．

3 代码

接下来，通过代码展示一下如何在ROS中发布一张地图．

3.1 获取代码

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

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

本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson4/src/occupancy_grid/occupancy_grid.cc。

3.2 main

main函数中，首先声明了一个OccupancyGrid类的对象，然后以1Hz的频率重复调用PublishMap()函数．

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson4_make_occupancy_grid_map");
    OccupancyGrid occupancy_grid;
    ros::Rate rate(1);

    while (ros::ok())
    {
        ROS_INFO("publish occupancy map");
        occupancy_grid.PublishMap();
        rate.sleep();
    }
    return (0);
}

3.3 occupancy_grid类

类的声明如下，内容很少．

首先，想要发布栅格地图，要包含 <nav_msgs/OccupancyGrid.h>这个头文件．

声明了2个Publisher，其中map_publisher_是用来发布map完整数据的．

map_publisher_metadata_只是用来发布地图信息的，不包含data数据，所以这个话题下的数据量要明显少于map话题下的数据量．适用于只需要知道地图相关信息或者尺寸，不需要知道地图实际data数值的情况．

#include <iostream>
#include <chrono>

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

class OccupancyGrid
{
private:
    ros::NodeHandle node_handle_;           // ros中的句柄
    ros::Publisher map_publisher_;          // 声明一个Publisher
    ros::Publisher map_publisher_metadata_; // 声明一个Publisher
    nav_msgs::OccupancyGrid map_;           //用来发布map的实体对象

public:
    OccupancyGrid();
    void PublishMap();
};

3.4 OccupancyGrid()构造函数

// 构造函数
OccupancyGrid::OccupancyGrid()
{
    // \033[1;32m，\033[0m 终端显示成绿色
    ROS_INFO_STREAM("\033[1;32m----> Make Occupancy Grid Map by no move started.\033[0m");

    map_publisher_ = node_handle_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    map_publisher_metadata_ = node_handle_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);

    // 对map_进行初始化
    map_.header.frame_id = "map";

    // 地图的分辨率为0.05m,代表一个格子的距离是0.05m
    map_.info.resolution = 0.05;

    // 地图图片像素的大小, width为地图的宽度是多少个像素
    map_.info.width = 30;
    map_.info.height = 30;

    // 如果要表示地图图片为多少米的话,就需要用实际长度除以分辨率,得到像素值
    // map_.info.width = 100 / map_.info.resolution;
    // map_.info.height = 100 / map_.info.resolution;

    // 地图左下角的点对应的物理坐标
    map_.info.origin.position.x = 0.0;
    map_.info.origin.position.y = 0.0;

    // 对数组进行初始化, 数组的大小为实际像素的个数
    map_.data.resize(map_.info.width * map_.info.height);
}

3.5 PublishMap()

代码很简单，只说明其中几点．

• 遍历的时候要注意顺序，是行优先的顺序
• 为data赋值之前要进行初始化，代码里为data赋予了 从-1到254之间的不同的值，实际使用时只需要赋予 -1, 0, 100 这三种值即可

// 构造map并进行发布
void OccupancyGrid::PublishMap()
{
    start_time_ = std::chrono::steady_clock::now();

    // 通过二维索引算出来的一维索引
    int index = 0;

    // 10种情况
    int count = 10;

    // 固定列, 优先对行进行遍历
    for (int j = 0; j < map_.info.height; j++)
    {
        for (int i = 0; i < map_.info.width; i++)
        {
            // 二维坐标转成一维坐标
            index = i + j * map_.info.width;
            // std::cout << " index: " << index ;

            // 0代表空闲, 100代表占用, -1代表未知, 默认值为0

            // 为map赋予不同的值来体验效果, 从-1 到 254
            if (index % count == 0)
                map_.data[index] = -1;
            else if (index % count == 1)
                map_.data[index] = 0;
            else if (index % count == 2)
                map_.data[index] = 30;
            else if (index % count == 3)
                map_.data[index] = 60;
            else if (index % count == 4)
                map_.data[index] = 100;
            else if (index % count == 5)
                map_.data[index] = 140;
            else if (index % count == 6)
                map_.data[index] = 180;
            else if (index % count == 7)
                map_.data[index] = 220;
            else if (index % count == 8)
                map_.data[index] = 240;
            else if (index % count == 9)
                map_.data[index] = 254;
        }
    }
    
    // 设置这一帧地图数据的时间戳
    map_.header.stamp = ros::Time::now();

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

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

4 运行

4.1 launch文件

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

4.2 编译与运行

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

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

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

roslaunch lesson4 make_occupancy_grid_map.launch

4.3 运行结果

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

可以看到，虽然ROS官方中将栅格地图的值限定为 0-100，实际上将大于100的值填入栅格中也是可以在rviz中显示出来的．

值为0-100的颜色变换为从白色到黑色，值为100-254的颜色变化为从红色到黄色．

其实，这个栅格的值可以为任意值，只要接收map信息的节点做相应的处理即可．

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

[ INFO] [1609646595.640337781]: publish occupancy map
发布一次地图用时: 0.00012029 秒。

[ INFO] [1609646596.640299474]: publish occupancy map
发布一次地图用时: 9.2979e-05 秒。

[ INFO] [1609646597.640258873]: publish occupancy map
发布一次地图用时: 9.9907e-05 秒。

可以看的，现在发布一次30*30=900个像素点的地图的时间还是很小的．

但是，当实际SLAM的过程中，如果构建100m * 100m范围的地图，分辨率为0.05的情况下，那地图的像素点将达到400万个，这时发布一次地图所需要的时间将变为 0.09秒，也就是说，当地图再大点的情况下，我们发布地图的频率将不足1Hz.

所以，大部分SLAM算法将发布地图单独设置了一个线程，并让它以较低的频率进行更新．

这样又不会阻塞历程计部分的计算，也不会让地图的更新变得越来越滞后．

5 总结与Next

本篇文章中简单实现了如何发布一个OccupancyGrid类型的地图。

体会了向地图中填入不同数值时地图在rviz中的显示情况，我也是第一次见到彩色的栅格地图。。。

体会了栅格地图遍历一次的用时，地图越大，耗时也越大。

下篇文章将借鉴GMapping中构建地图的方式，将雷达数据填充到栅格地图当中。

---

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

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

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