【基础计算】ESDF栅格距离图计算并行加速版 (附源码)

这一部分仅为路径规划源码及论文GPIR的一个小部分，但是有代码实现，第一次看的时候有些懵，所以特此记录；主要是设置好了栅格地图后，添加了障碍物后，对其的欧式距离计算和梯度计算等。原代码中为了加速，不同以往的直接每个点逐个计算遍历，而是分了两个步骤同时使用多线程并行计算进行加速

参考文献：

1. 2002论文：a general algorithm for computing distance transforms in linear time
2. 完整GPIR 路径规划源码 github链接
3. 仅sdf_test 的debug及测试代码 gitee链接
4. openmp并行计算：介绍与简单测试链接

通常在计算图/栅格距离时，复杂度为 O ( m × n ) O(mtimes n) O(m×n) 也就是pixel of image OR dim of map，其中在计算欧式距离时因为还有一些额外的计算操作所以在计算大图时会有些耗时，此处根据参考1论文中的方法提出一种可以使用多线程并行计算的方式，使得理论上复杂度为 O ( m × n / p ) O(mtimes n/p) O(m×n/p) 其中p为线程数量，可以通过结果表得知，在较为大的pixel or dim下，加速较为明显

现在的主要应用可能是在加速路径规划时的栅格地图及距离图时效果较为明显，效果预览（灰黑为grid图，彩色为离障碍物距离图，最右边为距离梯度示意图）

以下我们进行简洁伪代码及实际代码对应，及整个过程的详细讲解

论文中总结

1. In a first phase each column C x C_x Cx​ is separately scanned. (defined by points ( x , y ) (x , y) (x,y) with x fixed) 其中距离由最近的点决定
2. In a second phase each row R y R_y Ry​ is separately scanned. (defined by points ( x , y ) (x , y) (x,y) with y fixed). 对于 R y R_y Ry​上的每个点，最小的EDT值为 ( x − x ′ ) 2 + G ( x ′ , y ) 2 (x-x')^2+G(x',y)^2 (x−x′)2+G(x′,y)2 其中 ( x ′ , y ) (x',y) (x′,y)沿row R y R_y Ry​

可以看到伪代码对应的第一和第二阶段，和实际代码中的 完全对应

void SignedDistanceField2D::EuclideanDistanceTransform(
    std::array dim,
    std::function is_occupied,
    DistanceMap* output_map) {
  int inf = dim[0] + dim[1] + 10;

  std::vector> g(dim[0], std::vector(dim[1], 0));
  omp_set_num_threads(1);
  {
#pragma omp parallel for
    // column scan phase 1====
    for (int x = 0; x < dim[0]; ++x) {
      g[x][0] = is_occupied(x, 0) ? 0 : inf;

      for (int y = 1; y < dim[1]; ++y) {
        g[x][y] = is_occupied(x, y) ? 0 : 1 + g[x][y - 1];
      }
      for (int y = dim[1] - 2; y >= 0; --y) {
        if (g[x][y + 1] < g[x][y]) g[x][y] = 1 + g[x][y + 1];
      }
    }
  }

  // row scan phase 2====
  omp_set_num_threads(1);
  {
#pragma omp parallel for
    for (int y = 0; y < dim[1]; ++y) {
      int q = 0, w;
      std::vector s(dim[0], 0);
      std::vector t(dim[0], 0);

      auto f = [&g, &y](int x, int i) -> double {
        return (x - i) * (x - i) + g[i][y] * g[i][y];
      };

      for (int u = 1; u < dim[0]; ++u) {
        while (q >= 0 && f(t[q], s[q]) > f(t[q], u)) {
          --q;
        }

        if (q < 0) {
          q = 0;
          s[0] = u;
        } else {
          w = 1 + std::floor((u * u - s[q] * s[q] + g[u][y] * g[u][y] -
                              g[s[q]][y] * g[s[q]][y]) /
                             (2 * (u - s[q])));
          if (w < dim[0]) {
            ++q;
            s[q] = u;
            t[q] = w;
          }
        }
      }

      for (int u = dim[0] - 1; u >= 0; --u) {
        output_map->SetValue(u, y, map_resolution_ * std::sqrt(f(u, s[q])));
        if (u == t[q]) --q;
      }
    }
  }
}

因为论文比较久远有些符号的使用并不像现在这样，可以点击上面原文进行观看 EDT全称： Euclidean Distance Transform，首先定义一下久远的符号 MIN ⁡ ( k : P ( k ) : f ( k ) ) operatorname{MIN}(k:P(k):f(k)) MIN(k:P(k):f(k)) 是指 k k k在 P ( k ) P(k) P(k)范围内使得 f ( k ) f(k) f(k) 最小的值

正常的距离计算通常需要逐次计算距离 d t [ x , y ] = E D T ( x , y ) dt[x,y]=sqrt{EDT(x,y)} dt[x,y]=EDT(x,y) ​
EDT ⁡ ( x , y ) = MIN ⁡ ( i , j : 0 ≤ i < m ∧ 0 ≤ j < n ∧ b [ i , j ] : ( x − i ) 2 + ( y − j ) 2 ) operatorname{EDT}(x, y)=operatorname{MIN}left(i, j: 0 leq i 为了所有的EDT,MDT, CDT的计算方便后续和代码中的G 如下：
EDT ⁡ ( x , y ) = MIN ⁡ ( i : 0 ≤ i < m : ( x − i ) 2 + G ( i , y ) 2 ) where  G ( i , y ) = MIN ⁡ ( j : 0 ≤ j < n ∧ b [ i , j ] : ∣ y − j ∣ ) (2) begin{aligned} operatorname{EDT}(x, y) &=operatorname{MIN}left(i: 0 leq i 然后就进入了第一阶段，可以看出第一阶段的column scan是可以直接并行的（仔细对着代码看看就能看出来了），因为每个column和其他直接互不影响，然后column下在往下直接for row，第一阶段算的是G值，比如示例test中的如果把对角线都设为1，其他给0（假设>0即 被占据），原始占据图与G图如下：

比较难以理解的应该是第二阶段，但是第二阶段其实就是上面公式(2)所示，直接手推一下，取min，如上图右边，只是论文的大部分在介绍如何优雅的写出这样的并行for

第二阶段首先y是fixed，也就是一个column看下来，所以简化一下公式二就是把y去掉
DT ⁡ ( x , y ) = MIN ⁡ ( i : 0 ≤ i < m : f ( x , i ) ) (3) operatorname{DT}(x, y)=operatorname{MIN}(i: 0 leq i 其中 f ( x , i ) f(x,i) f(x,i) 的定义，因为全文同时对其他的距离函数也进行了说明，所以原文中分了出来，但是我们在这里仅看着欧式距离计算哈，可以看到和公式(2)的区别就是y被藏起来了，因为y已经是fixed了
f ( x , i ) = { ( x − i ) 2 + g ( i ) 2  for EDT  ∣ x − i ∣ + g ( i )  for MDT  ∣ x − i ∣ max ⁡ g ( i )  for CDT  f(x, i)= begin{cases}(x-i)^{2}+g(i)^{2} & text { for EDT } \ |x-i|+g(i) & text { for MDT } \ |x-i| max g(i) & text { for CDT }end{cases} f(x,i)=⎩⎪⎨⎪⎧​(x−i)2+g(i)2∣x−i∣+g(i)∣x−i∣maxg(i)​ for EDT  for MDT  for CDT ​
申明下图Fig 2. F i F_i Fi​ 就是 点 ( i , g ( i ) ) (i,g(i)) (i,g(i)) 的曲线连接而成，我们想要的是min 最小的 所以是solid line 实线的部分

假设这个实线的一系列点(从左到右)的index是： s [ 0 ] , s [ 1 ] , … , s [ q ] s[0],s[1],dots,s[q] s[0],s[1],…,s[q]

对应给定了upper bound u > 0 u >0 u>0 定义使 x x x 最小的新 index h h h 通常来说x是有大于1的minimizer，定义拿到的最小的那个 0 ≤ h < u 0 le h H ( x , u ) = MIN ⁡ ( h : 0 ≤ h < u ∧ ∀ ( i : 0 ≤ i < u : f ( x , h ) ≤ f ( x , i ) ) : h ) H(x, u)=operatorname{MIN}(h: 0 leq h 由此定义处 s ( u ) s(u) s(u) 即从左到右的scan，拿到minimizers
S ( u ) = { H ( x , u ) ∣ 0 ≤ x < m } T ( h , u ) = { x ∣ 0 ≤ x < m ∧ H ( x , u ) = h }  if  0 ≤ h < u (4) begin{aligned} S(u) &={H(x, u) mid 0 leq xH(x,u)∣0≤xx∣0≤x 为了找到 x ∗ x^* x∗ 我们引入 Sep text{Sep} Sep ， Sep ( i , u ) text{Sep}(i,u) Sep(i,u) 指的是第一个不小于水平坐标相交点 F u F_u Fu​和 F i F_i Fi​ 的整数，公式表示也就是：
F i ( x ) ≤ F u ( x ) ⇔ x ≤ Sep ⁡ ( i , u ) F_{i}(x) leq F_{u}(x) quad Leftrightarrow quad x leq operatorname{Sep}(i, u) Fi​(x)≤Fu​(x)⇔x≤Sep(i,u)
由此可以获取 x ∗ = Sep ( s [ l ∗ ] , u ) x^*=text{Sep}(s[l^*],u) x∗=Sep(s[l∗],u)， 然后 Sep text{Sep} Sep这个取决于我们想要计算的距离是什么，比如EDT的话就是：
F i ( x ) ≤ F u ( x ) ⇔ {  definition of  F i , F u } ( x − i ) 2 + g ( i ) 2 ≤ ( x − u ) 2 + g ( h ) 2 ⇔ {  calculus;  i < u ; x  is an integer  } x ≤ ( u 2 − i 2 + g ( u ) 2 − g ( i ) 2 )  div  ( 2 ( u − i ) ) begin{aligned} & F_{i}(x) leq F_{u}(x) \ Leftrightarrow &left{text { definition of } F_{i}, F_{u}right} \ &(x-i)^{2}+g(i)^{2} leq(x-u)^{2}+g(h)^{2} \ Leftrightarrow &{text { calculus; } i definition of Fi​,Fu​}(x−i)2+g(i)2≤(x−u)2+g(h)2{ calculus; i 总结完就是：
Sep ⁡ ( i , u ) = ( u 2 − i 2 + g ( u ) 2 − g ( i ) 2 ) div ⁡ ( 2 ( u − i ) ) (5) operatorname{Sep}(i, u)=left(u^{2}-i^{2}+g(u)^{2}-g(i)^{2}right) operatorname{div}(2(u-i)) tag{5} Sep(i,u)=(u2−i2+g(u)2−g(i)2)div(2(u−i))(5)
但是感觉好像… 理论还有漏的地方，暂时就先这样吧… 后面有机会再补充… 因为感觉超出了我的…能力范围，总感觉卡在哪里了，可能就是卡在了这for 套for 再套while里把，对着伪代码写出来 emm能用 ok；用ab的话：调用就行了, hhhh

  omp_set_num_threads(4);
  {
#pragma omp parallel for
    for (int y = 0; y < dim[1]; ++y) {
      int q = 0, w;
      std::vector s(dim[0], 0);
      std::vector t(dim[0], 0);

      auto f = [&g, &y](int x, int i) -> double {
        return (x - i) * (x - i) + g[i][y] * g[i][y];
      };//公式2中求距离的

      for (int u = 1; u < dim[0]; ++u) {
        while (q >= 0 && f(t[q], s[q]) > f(t[q], u)) {//公式4对比大小
          --q;
        }

        if (q < 0) {
          q = 0;
          s[0] = u;
        } else {
          w = 1 + std::floor((u * u - s[q] * s[q] + g[u][y] * g[u][y] -
                              g[s[q]][y] * g[s[q]][y]) /
                             (2 * (u - s[q])));//公式5 +1
          if (w < dim[0]) {
            ++q;
            s[q] = u;
            t[q] = w;
          }
        }
      }

      for (int u = dim[0] - 1; u >= 0; --u) {
        output_map->SetValue(u, y, map_resolution_ * std::sqrt(f(u, s[q])));
        if (u == t[q]) --q;
      }
    }
  }