上周搬家, 导致这篇文章更新的慢了点.
之前的文章我们都是通过scan-to-scan的方式进行位姿变换的计算. 接下来的文章将带领大家体验scan-to-map的计算位姿变换的方式.
首先, 来简要介绍一下什么是map.
地图对于大部分人来说, 第一时间想到的应该是高德地图中的地图, 是一副平面的或者立体的, 能够标识出从一个地点到另一个地点的可通行路径.
地图对于人来说其实提供了2个方面的功能, 第一个就是提供一个可通行路径, 还有一个其实是定位.
当你下了公交或者出了地铁等, 发现自己不知道自己在哪了, 你的第一反应是什么? 拿出手机, 打开GPS, 看看高德地图中自己的位置在哪, 以及搜一下如何从自己现在的位置到自己想去的位置.这就是GPS定位.
如果没有GPS怎么办呢? 一般情况下, 大家都会在地图中选一个明显的或者好认的建筑, 来判断自己与这个建筑的位置关系, 从而确定出自己在地图中的位置. 确定了位置之后就可以去自己想去的位置去了.
举个例子, 当你朋友给你打电话说我迷路了, 我不知道在哪. 大部分人的第一反应应该是让朋友说一下旁边有什么明显的建筑, 如某某商场, 某某店的名字等等, 然后你通过某某商场, 某某店的名字确定了朋友的大致位置.
对于机器人来说也是一样的, 只不过机器人眼中的地图在表示上与高德地图有些不同.
机器人眼中的地图的表示形式有很多, 可以是三维点云地图, 也可以是二维点云地图, 也可以是二维栅格地图, 也可以是拓扑地图, 也可以是语义地图等等.
下面展示了一些地图的图片.
上图为二维激光SLAM构建的二维栅格地图
上图为三维激光SLAM构建的三维点云地图
上图为视觉SLAM(ORB-SLAM2)构建的稀疏点云地图
上图为视觉SLAM构建稠密点云地图
其中, 三维激光SLAM与视觉SLAM构建的地图一般都是三维点云地图, 一副完全由离散的三维空间点组成的地图.
二维激光SLAM构建的地图为二维栅格地图,
栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义.
ROS的栅格地图使用白色代表空闲，也就是可通过区域，其存储的值为 0；
黑色代表占用，也就是不可通过区域，其存储的值为 100；
灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1．
栅格地图由于其 占用与空闲的表示方法，在ROS中又被称为占用地图．
栅格地图的示意图如下图所示：
图片引用于[csdn 白茶-清欢: https://blog.csdn.net/zhao_ke_xue/article/details/110919883]
接下来看一下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是一维的，我们在赋值之前要首先对这个一维数组进行初始化，数据的大小就是所有像素的个数．
遍历的时候要注意方向，这个数据是以行为主要递增方向的．也就是说遍历的时候要先遍历第一行的所有数据，然后再遍历第二行的所有数据．
接下来，通过代码展示一下如何在ROS中发布一张地图．
代码已经提交在github上了，如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。
推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.
本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson4/src/occupancy_grid/occupancy_grid.cc。
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);
}
类的声明如下，内容很少．
首先，想要发布栅格地图，要包含 <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();
};
// 构造函数
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);
}
代码很简单，只说明其中几点．
// 构造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;
}
<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>
下载代码后，请放入您自己的工作空间中，通过 catkin_make 进行编译．
由于是新增的包，所以需要通过 rospack profile 命令让ros找到这个新的包．
之后, 使用source命令，添加ros与工作空间的地址到当前终端下，再通过如下命令运行本篇文章对应的程序
roslaunch lesson4 make_occupancy_grid_map.launch
启动之后，会在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算法将发布地图单独设置了一个线程，并让它以较低的频率进行更新．
这样又不会阻塞历程计部分的计算，也不会让地图的更新变得越来越滞后．
本篇文章中简单实现了如何发布一个OccupancyGrid类型的地图。
体会了向地图中填入不同数值时地图在rviz中的显示情况，我也是第一次见到彩色的栅格地图。。。
体会了栅格地图遍历一次的用时，地图越大，耗时也越大。
下篇文章将借鉴GMapping中构建地图的方式，将雷达数据填充到栅格地图当中。
文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，可以在公众号中添加我的微信，进激光SLAM交流群，大家一起交流SLAM技术。
同时，也希望您将这个公众号推荐给您身边做激光SLAM的人们，大家共同进步。
如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。
