一、前言
在Cartographer中，利用分支定界法进行回环检测，需要对子图构建多分辨率栅格地图。
其实现源码在fast_correlative_scan_matcher_2d.cc中的PrecomputationGrid2D构造函数中。这部分代码有些难读，但花点时间也能理解。
本文将摆脱繁琐的代码，从原理层面介绍多分辨栅格地图的构建。并在文章结尾处，提供可视化栅格地图的数据代码。
二、原理
1. 栅格地图数据格式
在构建多分辨率栅格地图时，使用的是占据概率p离散化化到0〜255的uint8值，离散化公式为：
在Cartographer中，占据概率
的范围为0.1〜0.9。
在多分辨率栅格地图中，存储的是占据概率离散化到0〜255的uint8变量，离散化公式为：
为四舍五入到整数的函数。
占据概率越大，数值越接近255，占据概率越小，数值越接近0。
将一张子图栅格地图，以
为灰度值，通过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分别为:
其中第一层的滑动窗口width=1，本质上就是原始地图。
接下来，取cartographer中一个子图进行多分辨栅格地图的可视化，如下列图片所示。
可以看到，图像变得越来越粗糙。
三、可视化代码和数据
1. 保存栅格地图为txt文件
在cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc中的PrecomputationGrid2D::PrecomputationGrid2D()函数末尾，添加以下代码，将多分辨率栅格地图保存为txt文件：（注意修改文件目录名）
cpp
// 保存多分辨栅格地图为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进行可视化：
python
import cv2
import numpy as np
def 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)3. 数据
本文可视化所使用的txt文本和python脚本，已上传到个人资源：