(02) Analysis of Cartographer source code without dead ends - (46) 2D grid map → RayToPixelMask() and Bresenham algorithm

Explanation of a series of articles about slam summary links: the most complete slam in history starts from scratch , for this column to explain (02) Analysis of Cartographer source code without dead ends - the link is as follows:
(02) Analysis of Cartographer source code without dead ends - (00) Catalog_Latest None Dead corner explanation: https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$At\; the\; bottom\; of\; the\; article,\; the\; centerprovides\; mycontact\; information.Click\; on\; my\; photo\; to\; displayWX\to Official\; certification$
 

I. Introduction

In the previous blog, the following two questions were answered:

$Question1$ → Why does the grid value need to subtract kUpdateMarker in the Grid2D::FinishUpdate() function?
$Question3$ → The cell update in the ProbabilityGrid::ApplyLookupTable() function is rather strange, the code is: *cell = table[*cell].

However, when analyzing the ProbabilityGridRangeDataInserter2D::Insert() function, it calls a very important function CastRays(). There is no specific analysis of this function, and the next step is to explain this function. But before that, you need to know about the Bresenham algorithm.

What needs to be understood in advance is that inserting point cloud data into a raster map is essentially an update to the raster map. The update step is: according to the latest collected radar points, after matching with the map, insert the radar points into the current map, which is essentially updating the probability of the grid map. The update here includes two parts, one is to update the probability of the hit point, and the other is to complete the update of the laser ray map for CastRay, that is, to update the probability value of the grid that a straight line passes through.

The above process should be well understood. Hit means the point hit by the photon emitted by the laser, indicating that it is an obstacle. Then use the hit_table_ table in the source code to update the cell. The area passed by the ray connecting the origin of the radar sensor to the hit point is recorded as miss, and the corresponding cell grid indicates that there are no obstacles, so the miss_table_ table needs to be used to update the cell. The figure below is a model simulation of a radar scan:

Figure 1

The red part represents the area that has been explored before, and the gray part represents the area that is currently being explored. You can see the rays emitted by the robot (radar origin). The area through which these rays pass is a miss, and its end position is a hit. For a special case, that is, there are no obstacles in a large area, but the radar can only explore a limited distance. We will analyze this situation later.

Obviously, the update of the hit point is very simple, just call the ProbabilityGrid::ApplyLookupTable() function to update. However, the cell corresponding to the miss area must be updated when the hit ray passes through, which is obviously more complicated.

For the update of the cells in the miss area, the main problem to be solved is a point cloud ray emitted from the origin of the radar, which will pass through those cells or grids, as shown in the following figure:

Figure II

If the machine is at the position of the robot grid in the green circle in the above figure, and a photon is shot, that is, a point cloud data is at the position of the hit grid in the red circle in the above figure, then the required distance is the straight line connecting the centers of the two grids. The grid area, which is the miss in the above picture.
 

2. Bresenham Algorithm→Basic Description

The Bresenham algorithm mentioned here is not the original Bresenham algorithm, but the revised Bresenham algorithm in Cartographer. First look at the picture below:

Figure three

For the convenience of everyone's understanding, the above picture is drawn, let's talk about the above picture first:

1、灰浅色小方格(pixel坐标系) → 每个小方格代表一个像素、
2、灰深色大方格(subpixel坐标系) → 每个大方格边长为5个像素
3、蓝色方格 → 起始端点(pixel坐标系)
4、红色方格 → 结束端点(pixel坐标系)
5、黄色圆点 → 交点
6、subpixel_scale → 每个大方格的边长，上图中等于5
7、dx → 两端点x距离，像素为单位(pixel坐标系)
8、dy → 两端点y距离，像素为单位(pixel坐标系)

Originally, according to the principle of the Bresenham algorithm, we need to solve the coordinates of the purple pixel square in the figure below:

Figure four

But there is a problem with this, that is to fill in one pixel. If the resolution of the map is high, then there may be no way to see it with the naked eye, so Cartographer uses the subpixel_scale parameter to construct a new coordinate system, here we call it The subpixel coordinate system, remember the original pixel coordinate system as the pixel coordinate system.

According to Figure 3, it can be understood as subdividing the subpixel coordinate system to become a pixel coordinate system. Cartographer calculates the area where the ray passes through based on the subpixel coordinate system, and the result is shown in the figure below (also the purple area):

Figure five

$Core\; ideas$ Suppose the large blue square in the above picture is 0, and the next square is 1 (purple). According to the above picture, it can be seen directly that it is located on the right side of square 0, so the question arises? How is this calculated? Its core is to compare sub_yx(x=1,2,3,4…) with subpixel_scale. There are three cases of comparison results:

1、subpixel_scale > sub_yx : 选择当前方格右边的方格
2、subpixel_scale = sub_yx : 选择当前方格右上边(对角)的方格
3、subpixel_scale < sub_yx : 选择当前方格上边的方格

For $Situation3$ is a bit special. Why is it special? Let’s bring it in according to the above picture and we will know:

$\left(1\right)$ First, the subpixel_scale corresponding to the big blue square 0 is > sub_y1, so the purple square 1 on the right side of the current square should be selected;

$\left(2\right)$ Because subpixel_scale < sub_y2, choose purple square 2 above purple square 1; this is case 3, so you need to judge sub_y2' again, because subpixel_scale > sub_y2', this time is case 1, choose the right Purple square 3 (if this is still case 3, you need to judge sub_y2'' again)

$\left(3\right)$ Now the purple square 3 has been selected, and because subpixel_scale = sub_yx, the purple square 4 on the upper right (diagonal) is selected.

$\left(4\right)$ It is a special case when choosing purple square 5 and purple square 6, so subpixel_scale needs to be compared with sub_y5 and sub_y5'.

$Summary$ If case 3 occurs, sub_y2', sub_y2'', sub_y2''' may exist, and sub_y2-sub_y2'=sub_y2'-sub_y2''=sub_y2''-sub_y2'''=subpixel_scale. In fact, it is easy to understand that when the slope of the always-point line segment is large, it will often appear.

Here is a typical example. The slope of the point line segment is infinite (but not perpendicular to the x-axis). At this time, the x-coordinates of the purple square are the same. You only need to add one of the y-coordinates. Every time you increase the subpixel coordinate system One unit of the y-axis is equivalent to increasing subpixel_scale pixels.
 

3. Source code implementation → x-axis sorting

According to the previous analysis, we already know the general flow of Cartographer's implementation of the Bresenham algorithm, but how is it implemented in the source code? Although we have explained Figure 5 in detail, it is only a way to realize it. Although the principle is not the same in the source code, there are still some differences.

A RayToPixelMask() function is called in CastRays(). This function is the nuclear implementation of the Bresenham algorithm. It needs to pass three parameters, which are the pixel coordinates of the two endpoints and the zoom scale subpixel_scale. Its source code Set in subpixel_scale=kSubpixelScale=1000.

This function is implemented in the src/cartographer/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc file, first execute the following code:

  // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
  // coordinate.
  // 保持起始点的x小于终止点的x,需要注意，这里这样没有对y处理，
  // 也就是scaled_begin.y() 与 scaled_end.y() 谁大谁小还是不确定的 
  if (scaled_begin.x() > scaled_end.x()) {
    
    
    return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale);
  }

  CHECK_GE(scaled_begin.x(), 0);
  CHECK_GE(scaled_begin.y(), 0);
  CHECK_GE(scaled_end.y(), 0);
  std::vector<Eigen::Array2i> pixel_mask;

So for RayToPixelMask, there is no order of the endpoints, because it will be sorted according to the x-axis internally. The small one is scaled_begin, and the big one is scaled_end.

A pixel_mask variable is also created to store the pixel coordinate system, but note that it is based on the coordinates of the subpixel system. Equivalent to the coordinates of the purple square in Figure 5. For example, the coordinates of purple square 1 are (2,1). $Note\; that$ it is not (10,5), because it is based on the subpixel coordinate system. The coordinates of the purple square 1 are (2,2), and so on for the rest.
 

4. Source code implementation → vertical line segment

First of all, it processes the vertical line segment [scaled_begin, scaled_end]. The implementation of this situation is relatively simple. The x coordinates remain unchanged, and the y can be added one by one, but it still needs to be divided. The coordinates returned in the stored pixel_mask are Based on the subpixel coordinate system, the code comments are as follows:

  // Special case: We have to draw a vertical line in full pixels, as
  // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
  // 起点与终点x相等,说明该射线是垂直的，即斜率不存在的情况
  if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
    
    
    // 把起始点subpixel系下的坐标赋值给current，这里需要注意的是,
    // 把y值较小的点任务是起始点
    Eigen::Array2i current(
        scaled_begin.x() / subpixel_scale,
        std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale);
    //把起始点subpixel系下的坐标current添加至pixel_mask之中
    pixel_mask.push_back(current);
    // y值大的点为终止点，计算其subpixel系下的y坐标
    const int end_y =
        std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale;
    // 因为 current 存储的是subpixel系下坐标，所以++操作，是subpixel系下增加了一个单位
    // 当然，也可以认为是 pixel 系下增加了subpixel_scale 个单位
    for (; current.y() <= end_y; ++current.y()) {
    
    
      //如果当前的subpixel系下坐标与上一次添加到pixel_mask的坐标相同，则不会重复添加。
      if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
    }
    //需要注意，其存储的是subpixel系下的坐标。
    return pixel_mask;
  }

 

5. Source code realization → the first pixel

1. Preparatory work

Let's look at the code comments first, as follows, and then explain it in conjunction with the image:

  // 下边就是 breshman 的具体实现
  //为了后续计算方便，避免考虑绝对像素坐标，直接以偏移值的方式进行计算
  //终止点相对于起始点x轴偏移的pixel数(基于pixel坐标系),因为前面做了排序，所以其>0
  const int64 dx = scaled_end.x() - scaled_begin.x(); 

  //终止点相对于起始点y轴偏移的pixel数(基于pixel坐标系),
  //不过未左排序，其可能>0，也可能<0，需要分情况讨论
  const int64 dy = scaled_end.y() - scaled_begin.y(); 

  // 提前计算好的一个数值，其表示在subpixel系下y轴每增加一个单位，
  // 其对应的一维像素坐标系下，y需要偏移多少个单位(像素),这里*2，
  // 是为了方便中心点的表示，详细解释在下面
  const int64 denominator = 2 * subpixel_scale * dx;

  // The current full pixel coordinates. We scaled_begin at 'scaled_begin'.
  // 计算起始点subpixel系下坐标，且添加至pixel_mask之中
  Eigen::Array2i current = scaled_begin / subpixel_scale;
  pixel_mask.push_back(current);

  // To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in
  // the denominator.
  // +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale)
  // | | | |
  // +-+-+-+
  // | | | |
  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale)
  // | | | | -- center of first subpixel = 1 / (2 * subpixel_scale)
  // +-+-+-+ -- 0 = 0 / (2 * subpixel_scale)
  /*上面的类容翻译过来，就是说为了表示 subpixel(一个subpixel含1000个像素) 的中心,
    所以这里乘以2，因为这样：如果使用 (2 * subpixel_scale) 表示一个subpixel的边长，或者说顶点
    那么 1 * subpixel_scale 就表示中心，那么 0=0*subpixel_scale 就表示最小值(点)
  */

  // The center of the subpixel part of 'scaled_begin.y()' assuming the
  // 'denominator', i.e., sub_y / denominator is in (0, 1).
  // sub_y表示一个点在pixel系下的坐标，相对于该点在subpixel系下坐标的偏移 
  // 该偏移以像素维单位，这里的*2与前面保持一致，为方便表示，
  // 另外 sub_y/denominator 是处于(0,1)之间的，可以理解为y坐标的偏移率。
  int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;

  // The distance from the from 'scaled_begin' to the right pixel border, to be
  // divided by 2 * 'subpixel_scale'.
  // 这里要以subpixel的一个方格为参考来理解，其求得的是起始点在pixel系下的坐标，
  // 相对于该点在subpixel系下x坐标的偏移的2倍，以像素为单位
  const int first_pixel =
      2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
      
  // The same from the left pixel border to 'scaled_end'.
  // 其求得的是终止点在pixel系下的坐标，相对于该点在subpixel系下x坐标的偏移，以像素为单位
  const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1;

  // The full pixel x coordinate of 'scaled_end'.
  //计算出结束端subpixel系下的x坐标
  const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;

The main variables appearing in the appeal code are

dx、dy、denominator、current、sub_y、first_pixel、last_pixel、end_x

For dx and dy, it is directly consistent with dx and dy (black bold font) in Figure 5. The denominator here is not easy to understand. It can be directly divided by a 2*dx, which becomes subpixel_scale, and the subsequent selection of purple squares, The source code is compared with denominator, which is essentially compared with subpixel_scale. current is simpler and consistent with the previous one.

The variable sub_y
may be difficult to understand for sub_y in the above code, as shown below:

int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;

Consistent with the analysis of the denominator above, it is also divided by a 2*dx, then the remaining code is scaled_begin.y() % subpixel_scale) + 1, which is relatively easy to understand, as shown in the

figure below: record sub_yf= in the figure above scaled_begin.y() % subpixel_scale. The +1 in the source code is more confusing, so remember it for now. So that means $$sub\_y=(sub\_yf+1)\ast 2\ast dx\text{\hspace{1em}( 01 )}$$

variable first_pixel

  const int first_pixel =2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;

First, let’s take a look at the scaled_begin.x() % subpixel_scale on it, record it as sub_xf, and correspond to the figure as shown below:

then first_pixel can be recorded as
$$first\_pixel=2\ast subpixel\_scale-2\ast sub\_xf-1\text{\hspace{1em}( 02 )}$$

variable end_x

const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;

As shown in the figure below, end indicates the coordinates of the end point in the subpixel system, and end_x indicates the x-coordinate of the point. If the figure below is an example, its coordinates are (6,4).

 

2. Organize ideas

The above analysis mainly draws the following conclusions: $$sub\_y=(sub\_yf+1)\ast 2\ast dx\text{\hspace{1em}(01)}$$$$first\_pixel=2\ast subpixel\_scale-2\ast sub\_xf-1\text{\hspace{1em}( 02 )}$$ where sub_xf and sub_xy are shown in the figure below:

Then there is the following code in the source code:

sub_y += dy * first_pixel;

Indicates that it has updated the initial sub_y, and here it is expanded in combination with (1)(2):
$$sub\_y=(sub\_yf+1)\ast 2\ast dx+(2\ast subpixel\_scale-2\ast sub\_xf-1)dy\text{\hspace{1em}( 04 )}$$ For the convenience of understanding, both sides of the equation are divided by 2*dx, the result is as follows:
$$\frac{sub\_}{2\ast dx}=(sub\_yf+1)+(subpixel\_scale-sub\_xf-0.5)\frac{d}{dx}\text{\hspace{1em}( 05 )}$$ where$\frac{d}{dx}$Indicates the slope of the line segment, which is convenient for understanding and the drawing is as follows:

According to the above figure, it is easy to understand, $\frac{sub\_}{2\ast dx}$It means sub_y1 in Figure 5, but obviously, the intersection point 1 (yellow circle in the above figure) between the line from the start point to the end point in the above figure and the blue square is drawn in the wrong position, so we need to adjust it again, as shown in the figure below It should be fully compatible with the implementation of the source code:

Figure six

There are two main differences between it and our previous drawing. First, the line connecting the start point and the end point needs to be translated up by one unit, and then a purple square 3' needs to be added. that's fine.
 

6. Source code implementation → Discussion by situation

After sorting out the ideas, and drawing the above figure 6, I learned that $\frac{sub\_}{2\ast dx}$It is the sub_y1 we need to solve, as shown in the figure below:

The next step is to analyze how to solve sub_y2, sub_y2', sub_y3, sub_y3' in the source code $\cdots$$\cdots$ . It has been mentioned before. Because scaled_begin.x() and scaled_end.x() are sorted in the source code, dx is always greater than that. But there are two cases of dy>0 or dy<0. The examples listed above are all cases of dy>0.

1、dy > 0

For the situation of dy>0, according to the previous introduction, it can be divided into the following three situations (x is 1,2,2',... in the above figure):

1、sub_yx < subpixel_scale : 选择当前方格右边的方格
2、sub_yx = sub_yxsubpixel_scale : 选择当前方格右上边(对角)的方格
3、sub_yx > subpixel_scale : 选择当前方格上边的方格

And it is mentioned that case 3 is a special case (it will enter a cyclic state). It should be noted that the comparison between sub_y and denominator in the source code is equivalent to the comparison between sub_yx and subpixel_scale above. Now it is easier to understand the source code. The code comments are as follows:

  // dy > 0 时的情况subpixel系下的坐标至多被添加一次
  if (dy > 0) {
    
     
    while (true) {
    
    
      //只有current与上一次存储在pixel_mask中坐标不一样时，才进行存储。
      //这样可以保证，每个坐标至多被存储一次
      if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);

      // 情况3: 本质上就是 sub_yx > subpixel_scale，且是一个循环
      // 可以 把 sub_y 与 denominator 都除以一个2*dx进行理解
      while (sub_y > denominator) {
    
    //sub_y<denominator时结束循环

        sub_y -= denominator; //等价价于 sub_yx移动至sub_y(x+1)

        ++current.y(); //选择当前方格上边的方格
        //保证每个坐标至多添加一次
        if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
      }
      ++current.x();//x轴移动到下一个方格(基于subpixel系)

      //情况2: 本质上就是 sub_yx==subpixel_scale，
      if (sub_y == denominator) {
    
    
        //选择当前方格右上边(对角)的方格
        sub_y -= denominator;
        ++current.y();
      }

      //遍历到最后一个方格跳出循环
      if (current.x() == end_x) {
    
    
        break;
      }

      // 如果情况2与情况3都没有处理，则该处表示情况1
      // Move from one pixel border to the next.
      sub_y += dy * 2 * subpixel_scale;
    }

I believe it is very simple for everyone to understand with the illustrations.

2、dy < 0

This situation is basically the same as the situation of dy > 0, so I won’t repeat the explanation here. Friends who are interested can draw and analyze it by themselves.

 

7. Conclusion

This blog first answers the following two questions:

$Question1$ → Why does the grid value need to subtract kUpdateMarker in the Grid2D::FinishUpdate() function?
$Question3$ → The cell update in the ProbabilityGrid::ApplyLookupTable() function is rather strange, the code is: *cell = table[*cell].

Then the RayToPixelMask() function is analyzed in detail. However, the caller of the RayToPixelMask() function, CastRays(), has not been analyzed yet. Since this blog post is too long, I will put it in the next one.