(02)Cartographer源码无死角解析-(46) 2D栅格地图→RayToPixelMask()与贝汉明(Bresenham)算法

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

在上一篇博客中，解答了如下两个疑问：

$疑问1$→ 为什么Grid2D::FinishUpdate()函数中，栅格值为什么需要减去 kUpdateMarker？
$疑问3$ → ProbabilityGrid::ApplyLookupTable() 函数中对于cell的更新比较奇怪，代码为：*cell = table[*cell]。

但是在分析 ProbabilityGridRangeDataInserter2D::Insert() 函数的时候，其调用了一个十分重要的函数 CastRays()，对于该函数并没有进行具体的分析，接下来就是对该函数进行讲解了。不过在这之前，需要了解一下贝汉明(Bresenham)算法。

需要提前理解的是，把点云数据插入到栅格地图，其本质上就是对栅格地图的更新。更新步骤为：根据最新采集到的雷达点，在与地图匹配后，把雷达点插入到当前地图中，其本质是对栅格地图概率的更新。这里的更新包括两部分，其一是对hit点的概率更新，其二是对CastRay完成激光射线状地图更新，即对一条直线所经过的栅格进行概率值更新。

对于上述过程应该还是很好理解的，hit 表示激光发射光子打中的点，说明其是障碍物，那么源码中使用 hit_table_ 表格对 cell 进行更新。从雷达传感器原点到 hit 点连接的射线其经过的区域记为 miss，对应的 cell 栅格表示其没有障碍物，所以需要使用 miss_table_ 表格对 cell 进行更新。下图是雷达扫描的模型仿真：

图一

红色部分表示之前探索过的区域，灰色部分表示目前正在探索的区域。可以看到一条条由机器人(雷达原点)发射而出的一条条射线。这些射线经过的区域为miss，其终点位置为hit。对于一种特殊情况，那就是大范围区域没有障碍物，但是雷达只能探索到有限距离，该情况后续我们再进行具体分析。

那么显然，对于 hit 点的更新十分简单，直接调用 ProbabilityGrid::ApplyLookupTable() 函数更新即可。但是对于hit 射线经过经过区域 miss 所对应的 cell 都要进行更新，显然会复杂一些。

对与 miss 区域 cell 的更新，主要解决的问题，就是一条由雷达原点发射出来的点云射线，其会经过那些 cell 或者说栅格，如下图所示：

图二

若机器上处于上图绿色圆圈Robot栅格位置，打出一个光子，即一个点云数据为上图红色圆圈hit栅格位置，那么要求得，就是这两个栅格中心连接的直线，所经过的栅格区域，也就是上图的 miss。
 

二、贝汉明(Bresenham)算法→基本说明

这里说的贝汉明(Bresenham)算法，并非原版的贝汉明(Bresenham)算法，而是Cartographer中改版过后的贝汉明(Bresenham)算法。先来看下图：

图三

为方便大家的理解，所以绘画了上图，先来说一下上图：

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

本来根据贝汉明(Bresenham)算法的原理，我们需要求解出下图中的紫色像素方格的坐标：

图四

但是这样存在一个问题，那就是按一个像素进行填充，如果地图的分辨率较高的话，那么用肉眼可能根本没有办法看见了，所以 Cartographer 中利用 subpixel_scale 参数构建一个新的坐标系，这里我们称为 subpixel坐标系，记原来的像素坐标系为 pixel坐标系。

根据图图三、可以理解为对subpixel坐标系进行细分，就成了pixel坐标系。Cartographer 求得射线经过的区域是基于subpixel坐标系的，其结果如下图所示(同样为紫色区域)：

图五

$核心思路$ 假设上图中蓝色大方格为0，下一个方格为1(紫色)，根据上图可以直接看出，其位于方格0的右边，那么问题来了？这是如何计算的呢？其核心就是sub_yx(x=1,2,3,4…) 与 subpixel_scale 进行比较，比较的结果有三种情况：

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

对于$情况3$ 会特殊一点，为什么特殊，根据上图我们带入一下就知道了：

$(1)$ 首先蓝色大方格0对应的 subpixel_scale > sub_y1，所以应该选择当前方格右边的紫色方格1；

$(2)$ 又因为 subpixel_scale < sub_y2，所以选择紫色方格1上边的紫色方格2；该为情况3，所以需要再一次判断 sub_y2’，因为 subpixel_scale > sub_y2’，该次为情况一，选择右边的紫色方格3 (如果该次还为情况3，则需要再次判断sub_y2’’)

$(3)$ 现在已经选择了紫色方格3，又因为 subpixel_scale = sub_yx，所以选择右上边(对角)的紫色方格4。

$(4)$ 在选择紫色方格5与紫色方格6的时候又是特殊情况，所以 subpixel_scale 需要与 sub_y5 与 sub_y5’ 进行比较。

$总结$ 如果出现情况3，则可能会存在 sub_y2’、sub_y2’’、sub_y2’’’，并且 sub_y2-sub_y2’=sub_y2’-sub_y2’’=sub_y2’’-sub_y2’’’=subpixel_scale。其实很好理解，当始终点线段斜率较大的时候，就会经常出现情况上。

这里列举一个典型的例子，始终点线段斜率无限大(但不垂直x轴)，此时，紫色方格的x坐标是相同的，只需要把y坐标一个加上去即可，每增加subpixel坐标系y轴的一个单位，等价于增加subpixel_scale个像素。
 

三、源码实现→x轴排序

根据前面的分析，已经知道了 Cartographer 实现贝汉明(Bresenham)算法的大致流程、但是源码中又是如何实现的呢？虽然我们对 图五 进行了详细的讲解，但是那仅仅是实现的一种方式而已，源码中虽然原理上差不过，不过还是存在一些差异的。

在 CastRays() 中调用了一个RayToPixelMask()函数，该函数就是贝汉明(Bresenham)算法的核型实现，其需要传递三个参数，分别为两个端点像素坐标，以及缩放尺度 subpixel_scale，其源码中设置为 subpixel_scale=kSubpixelScale=1000。

该函数实现于 src/cartographer/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc 文件中，首先执行如下代码：

  // 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;

所以对于 RayToPixelMask 连个端点无先后顺序，因为其内部会根据x轴进行排序。小的为 scaled_begin，大的为scaled_end。

还创建了一个 pixel_mask 变量，用于存储像素坐标系，不过注意，其是基于 subpixel 系的坐标。等价于图五中紫色方格的坐标。如紫色方格1的坐标为(2,1)。$注意$ 并不是(10,5)，因为是基于subpixel 坐标系的。紫色方格1的坐标为(2,2)，其余的依此类推。
 

四、源码实现→垂直线段

首先其对垂直线段 [scaled_begin,scaled_end] 进行了处理，该情况的实现还是比较简单的，x 坐标不变，把y一个一个增进去即可，不过依然要除以，存储 pixel_mask 中返回的坐标是基于subpixel 坐标系的，代码注释如下：

  // 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;
  }

五、源码实现→第一个像素

1、预备工作

先来看代码注释，如下，后续再结合图像进行讲解：

  // 下边就是 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;

上诉代码主要出现的变量为

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

对于 dx 与 dy 直接其与图五中的 dx，dy(黑色加粗字体)一致，这里的 denominator 不好理解可以直接除以一个2*dx，则变成了 subpixel_scale ，后续紫色方格的选择，源码中是与 denominator 进行对比，其本质上就是与 subpixel_scale 进行对比。current 较为简单，与前面的一致。

变量sub_y
对于上述代码中的 sub_y 理解起来或许还是比较困难的，如下所示：

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

同上面分析 denominator 一致，同样除以一个 2*dx，那么剩下的就是代码就是 scaled_begin.y() % subpixel_scale) + 1，这个还是比较好理解的，如下图所示：

上图中记 sub_yf=scaled_begin.y() % subpixel_scale。源码中的+1比较令人疑惑，暂时先记一下。那么也就是说 $$sub\_y=(sub\_yf+1)\ast 2\ast dx\text{\hspace{1em}(01)}$$

变量first_pixel

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

首先来看看其上的 scaled_begin.x() % subpixel_scale，记其为 sub_xf，对应到图上如下所示：

那么 first_pixel 可以记为
$$first\_pixel=2\ast subpixel\_scale-2\ast sub\_xf-1\text{\hspace{1em}(02)}$$

变量end_x

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

如下图end表示终点在subpixel系的坐标，end_x 表示该点的x坐标，如果以下图为例，其坐标为(6,4)。

 

2、整理思路

上面的分析，主要得到如下几个结论： $$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)}$$其中 sub_xf 与 sub_xy如下图所示：

接着源码中还存在如下代码：

sub_y += dy * first_pixel;

表示其对初始的 sub_y 进行了更新，这里结合(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)}$$为了方便理解，等式两边都除以2*dx，结果如下：
$$\frac{sub\_y}{2\ast dx}=(sub\_yf+1)+(subpixel\_scale-sub\_xf-0.5)\frac{dy}{dx}\text{\hspace{1em}(05)}$$其中的$\frac{dy}{dx}$ 表示线段的斜率，方便理解作图如下：

根据上图，就很好理解了，$\frac{sub\_y}{2\ast dx}$ 表示的，就是图五中的 sub_y1，不过显然，上图中起点到终点的连线与蓝色方格的交点1(上图黄色圆圈)画错了位置，所以我们需要再调整一下，下图应该才是与源码的实现全贴合：

图六

其与我们前面绘画的主要有2个不同点，首先是起点与终点的连线需要向上平移一个单位，然后需要再增加一个紫色的方格3’。这样就行了。
 

六、源码实现→分情况讨论

整理了思路之后，且绘画出了上图六，了解到 $\frac{sub\_y}{2\ast dx}$ 就是我们需要求解的 sub_y1，如下图所示：

接下来就是需要分析源码中是如何求解 sub_y2、sub_y2’、sub_y3、sub_y3’ $\cdots$$\cdots$。在前面已经提到过。因为源码中对 scaled_begin.x() 与 scaled_end.x() 进行了排序，所以 dx 一直都是大于的。但是存在dy>0或者dy<0两种情形。上面列举的例子都是 dy>0 的情况。

1、dy > 0

对于 dy>0 的情形，根据前面的简介，其又可以分为如下三种情况(x为上图的1,2,2’,…)：

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

并且提到情况3是一种特殊的情况(会进入到一种循环状态)，需要注意的是，源码中 sub_y 与 denominator 的比较等价与上述 sub_yx 与 subpixel_scale 的比较。现在来理解源码就比较简单了。代码注释如下：

  // 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;
    }

相信大家结合图示再进行理解，就十分简单了。

2、dy < 0

该情形与 dy > 0 的情况基本一致，所以这里就不再重复讲解了，有兴趣的朋友可以自己画图分析一下。

七、结语

该篇博客首先对如下2个疑问进行了解答：

$疑问1$→ 为什么Grid2D::FinishUpdate()函数中，栅格值为什么需要减去 kUpdateMarker？
$疑问3$ → ProbabilityGrid::ApplyLookupTable() 函数中对于cell的更新比较奇怪，代码为：*cell = table[*cell]。

然后对 RayToPixelMask() 函数进行了具体的分析。不过 RayToPixelMask() 函数的调用者 CastRays() 还没有进行分析。由于该篇博客篇幅太长，所以就放到下一篇中吧。