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

一、前言

在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文件：（注意修改文件目录名）

  // 保存多分辨栅格地图为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
    for(int j=0; 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 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脚本，已上传到个人资源：

 https://download.csdn.net/download/Jeff_zjf/88369796