Cartographer’s principle of constructing multi-resolution raster maps

1. Foreword

In Cartographer, using the branch-and-bound method for loop closure detection requires constructing a multi-resolution raster map for the subgraphs.

Its implementation source code is in the PrecomputationGrid2D constructor in fast_correlative_scan_matcher_2d.cc. This part of the code is a bit difficult to read, but you can understand it if you take a moment.

This article will get rid of the cumbersome code and introduce the construction of multi-resolution raster maps from the principle level. And at the end of the article, the data code for visualizing the raster map is provided.

2. Principle

1. Raster map data format

When constructing a multi-resolution raster map, the occupation probability p is discretized to a uint8 value of 0 to 255. The discretization formula is:

In Cartographer, the occupancy probability $p$ ranges from 0.1 to 0.9.

In a multi-resolution raster map, what is stored is the occupancy probability $p$ discretization To a uint8 variable from 0 to 255, the discretization formula is:

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

$\textup{RoundToInt}()$ is rounded to an integer The function.

The greater the occupancy probability, the closer the value is to 255; the smaller the occupancy probability, the closer the value is to 0.

Convert a sub-image raster map to $255-value$ as the gray value , visualized through opencv, as shown in the figure below:

The black area in the picture is the obstacle hit by the laser point, and the white area represents the completely free area;

The gray area needs to be discussed on a case-by-case basis: when the value is less than 127, the probability of being idle is greater; when the value is greater than 127, the probability of occupancy is greater.

The corresponding relationship between pictures and raster values is as follows:

2. Sliding window downsampling

Define the maximum sliding window, as shown in the figure below. Width is the window size, and the maximum value in the window is output to the blue grid in the lower right corner.

Suppose there is a 7×7 raster map, which is processed using a sliding window with width=2. The first processing point is the upper left corner of the map. At this time, there is only one value in the window, which is directly saved to the result, as shown in the following figure:

Then the successive sliding window is moved horizontally (x direction) by one grid, and the maximum value in the window is written into the result until the window has traversed all the grids in the row. Due to the size of the window itself, the output raster will have (width-1) more columns, as shown in the following figure:

After traversing one row, move the window one space vertically (y direction) and repeat the row traversal operation until all rows are traversed. In the same way, the output raster will also have (width-1) more rows, as shown in the following figure:

Use a window with width=4 to sample the raster map. The results are as follows:

Assume that the original raster map size is [h, w]. After width sliding window processing, the sampled map size becomes [h + width-1, w + width-1].

3. Multi-resolution raster map visualization

In Cartographer’s loopback detection, 7 layers of raster maps with different resolutions are built by default. The layer number parameters are in pose_graph.lua, as shown below.

The widths of the sliding windows corresponding to the 7 layers are:

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

The sliding window width=1 in the first layer is essentially the original map.

Next, take a subgraph in cartographer to visualize the multi-resolution raster map, as shown in the following picture.

It can be seen that the image becomes rougher and rougher, and the black part is obviously expanded, so the downsampled image is also called an expansion map.

3. Visual code and data

1. Save raster map as txt file

At the end of the PrecomputationGrid2D::PrecomputationGrid2D() function in cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc, add the following code to save the multi-resolution raster map as a txt file: (note that the file directory name is modified)

 // Save multi-resolution raster map as txt file
  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 file visualization

Write python code, read the contents of the txt file, and use cv2 for visualization:

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. Data

The txt text and python script used in the visualization of this article have been uploaded to personal resources:

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