Cartographer构建多分辨率栅格地图的原理-编程知识

Cartographer构建多分辨率栅格地图的原理

一、前言

在Cartographer中，利用分支定界法进行回环检测，需要对子图构建多分辨率栅格地图。

其实现源码在fast_correlative_scan_matcher_2d.cc中的PrecomputationGrid2D构造函数中。这部分代码有些难读，但花点时间也能理解。

本文将摆脱繁琐的代码，从原理层面介绍多分辨栅格地图的构建。并在文章结尾处，提供可视化栅格地图的数据代码。

二、原理

1. 栅格地图数据格式

在构建多分辨率栅格地图时，使用的是占据概率p离散化化到0〜255的uint8值，离散化公式为：

在Cartographer中，占据概率 $p$ 的范围为0.1〜0.9。

在多分辨率栅格地图中，存储的是占据概率 $p$ 离散化到0〜255的uint8变量，离散化公式为：

$value=\textup{RoundToInt}(\frac{p-0.1}{0.9-0.1}*255)$

$\textup{RoundToInt}()$ 为四舍五入到整数的函数。

占据概率越大，数值越接近255，占据概率越小，数值越接近0。

将一张子图栅格地图，以 $255-value$ 为灰度值，通过opencv可视化出来，如下图所示：

图中黑色区域为激光点打到的障碍物，白色区域代表完全空闲区域；

灰色区域需分情况讨论：数值小于127时，空闲概率更大；数值大于127时，占用概率更大。

图片与栅格值的对应关系如下：

2. 滑动窗口降采样

定义取最大值滑动窗口，如下图所示。width为窗口尺寸，窗口内的最大值输出到右下角蓝色格子中。

假设有一张7x7的栅格地图，用width=2的滑动窗口进行处理，第一个处理点为地图左上角点，此时窗口内只有一个值，直接保存到结果中，如下图所示：

然后将逐次滑动窗口横向（x方向）移动一格，将窗口内的最大值写入到结果中，直到窗口遍历完该行所有格子。由于窗口本身存在尺寸，输出栅格会多出(width-1)列，如下图所示：

遍历完一行后，将窗口纵向（y方向）移动一格，重复遍历行操作，直到所有行遍历完成。同理，输出栅格也会多出(width-1)行，如下图所示：

用width=4的窗口，对栅格地图进行将采样，结果如下所示：

假设原始栅格地图尺寸为[h, w]，经过width的滑动窗口处理后，将采样后地图尺寸变为[h+width-1, w+width-1]。

3. 多分辨栅格地图可视化

在Cartographer的回环检测中，默认构建7层不同分辨率的栅格地图，层数参数在pose_graph.lua中，如下图。

7层对应的滑动窗口的width分别为:

$2^{0}=1,2^{1}=2,2^{2}=4,2^{3}=8,2^{4}=16,2^{5}=32,2^{6}=64$

其中第一层的滑动窗口width=1，本质上就是原始地图。

接下来，取cartographer中一个子图进行多分辨栅格地图的可视化，如下列图片所示。

可以看到，图像变得越来越粗糙。

三、可视化代码和数据

1. 保存栅格地图为txt文件

在cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc中的PrecomputationGrid2D::PrecomputationGrid2D()函数末尾，添加以下代码，将多分辨率栅格地图保存为txt文件：（注意修改文件目录名）

  // 保存多分辨栅格地图为txt文件static int idx = 0;static int depth = 7;int pre_id = idx / depth;int suffix_id = idx % depth;string f_name = "/home/trail/depth7/" + to_string(pre_id) + "-" + to_string(suffix_id) + ".txt";ofstream f(f_name);f << wide_limits_.num_y_cells << " " << wide_limits_.num_x_cells << endl;for(int i=0; i<wide_limits_.num_y_cells; i++){for(int j=0; j<wide_limits_.num_x_cells; j++){f << setw(4) << (int)cells_[j + i * wide_limits_.num_x_cells];}f << endl;}f << endl;f.close();idx++;

2. txt文件可视化

编写python代码，读取txt文件内容，用cv2进行可视化：

import cv2
import numpy as npdef ReadGrid(txt):with open(txt, 'r') as f:line = f.readline()height = int(line.split(' ')[0])width = int(line.split(' ')[1])print(height, width)img = np.zeros((height, width, 1), np.uint8)lines = f.readlines()for idy, line in enumerate(lines):pixels = [d.strip() for d in line.split()]for idx, data in enumerate(pixels):img[idy, idx] = 255 - int(pixels[idx])return img    submap_id = 0
depth = 7
imgs = [ReadGrid("/home/trail/depth7/%d-%d.txt" % (submap_id, i)) for i in range(depth)]        
while 1:for i in range(depth):cv2.imshow(str(i), imgs[i])key = cv2.waitKey(30)