【工程记录】关于ROS导航二维栅格概率地图的简单融合

【创作背景】

针对多个车型机器人，对多个区域进行联合探索建图，且需要对各机器人创建的二维概率地图进行正确合并，形成一个大地图进行保存的使用情形。

【效果一览】

• 各车子地图

• 融合大地图

 【工程技术栈】

• ROS Navigation OccupancyGrid
• OpenCV

【实现思路】

1、ROS导航包使用的二维概率地图都是有分辨率的，也就是地图 yaml 文件里写的 resolution；换句话说，这个就是地图像素对应的真实长度，比如“ resolution: 0.05 “，意思就是map图像中每个像素点对应真实世界中 5 厘米的长度；同理，yaml文件里还有一个 “origin: [-8.000000, -12.200000, 0.000000]”，其实这个表示的就是map图像中建图起点所代表的真实世界坐标，换言之，我们通过分辨率和原点坐标，就可以推算出各车map图像之间的相对关系

2、那么如何把 nav_msg/OccupancyGrid 消息转换成一个opencv的 Mat 呢？我们知道，ros map有几种模式，其中导航包使用的是三元值模式，也就是说OccupancyGrid的data区，只有-1，0，100三种可能，所以我们需要使用一个signed char的Mat容器来放map图像

cv::Mat img = cv::Mat( grids.info.height, grids.info.width, CV_8SC1,
                           const_cast<signed char*>(grids.data.data()) );

3、如果你很细心，你会发现，这样转换得到的图像 mat 和 rviz 中看到的map不太一样！没错，这是因为 OccupancyGrid 消息存放 data 的顺序不是如 mat 那样，从左到右，从上到下；而是以左下角为原点，且右为x轴正向，上为y轴正向；所以这里需要把 mat 进行翻面和旋转，方能得到正确的图像

4、接下来就是根据每个子图的尺寸和锚点，确定大地图画板的大小了，并且把每个子图 copyto 到画板指定的 rect 区域即可

5、最后把融合后的 map 图像转换成 OccupancyGrid 消息发布出来即可，这里要注意OccupancyGrid 消息的 data 遍历方式。

【待完善功能】

1、目前的地图融合版本只支持起始建图相同朝向的小车，且需要比较精确的小车间相对位置关系

2、各地图叠加的时候，使用的是粗暴的取像素max值，而 ROS 导航概率地图非常坑的一点的是，100表示的是准障碍物，这就意味着 子图 交叠区域的地方，很可能出现错误的 黑色像素（黑色表示障碍物），但这一点目前没有想到比较好的解决办法，因为每个车的建图都是独立的，我们对他们的信任程度也是一样的，无法判断谁的地图更准确，从而去优化地图融合

3、现在缺少一种更智能的寻找地图相对转换关系的方式，有人用 传统图像匹配 寻找转换矩阵，但是我看了效果不好，估计还得上深度学习的方法