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

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

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

简洁过程

论文中总结

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

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

void SignedDistanceField2D::EuclideanDistanceTransform(
    std::array<int, 2> dim,
    std::function<bool(const int x, const int y)> is_occupied,
    DistanceMap* output_map) {
  int inf = dim[0] + dim[1] + 10;

  std::vector<std::vector<int>> g(dim[0], std::vector<int>(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<int> s(dim[0], 0);
      std::vector<int> 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，首先定义一下久远的符号 $\mathrm{MIN}(k:P(k):f(k))$ 是指 $k$在 $P(k)$范围内使得 $f(k)$ 最小的值

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

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

第二阶段首先y是fixed，也就是一个column看下来，所以简化一下公式二就是把y去掉
$$\mathrm{DT}(x,y)=\mathrm{MIN}(i:0\le i<m:f(x,i))\text{\hspace{1em}(3)}$$
其中 $f(x,i)$ 的定义，因为全文同时对其他的距离函数也进行了说明，所以原文中分了出来，但是我们在这里仅看着欧式距离计算哈，可以看到和公式(2)的区别就是y被藏起来了，因为y已经是fixed了
$$f(x,i)=\{\begin{array}{ll}(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{array}$$
申明下图Fig 2. $F_i$ 就是 点 $(i,g(i))$ 的曲线连接而成，我们想要的是min 最小的 所以是solid line 实线的部分

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

对应给定了upper bound $u>0$ 定义使 $x$ 最小的新 index $h$ 通常来说x是有大于1的minimizer，定义拿到的最小的那个 $0\le h<u$ 使得对于所有的$i$ 都有：$f(x,h)\le f(x,i)$
$$H(x,u)=\mathrm{MIN}(h:0\le h<u\wedge \forall (i:0\le i<u:f(x,h)\le f(x,i)):h)$$
由此定义处$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 x<m\} \\ T(h, u) &=\{x \mid 0 \leq x<m \wedge H(x, u)=h\} \text { if } 0 \leq h<u \end{aligned} \tag{4} S(u)T(h,u)​={H(x,u)∣0≤x<m}={x∣0≤x<m∧H(x,u)=h} if 0≤h<u​(4)
为了找到 $x^{\ast }$ 我们引入 $\text{Sep}$ ，$\text{Sep}(i,u)$ 指的是第一个不小于水平坐标相交点$F_u$和$F_i$ 的整数，公式表示也就是：
$$F_i(x)\le F_u(x)\iff x\le \mathrm{Sep}(i,u)$$
由此可以获取$x^{\ast }=\text{Sep}(s[l^{\ast }],u)$， 然后$\text{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<u ; x \text { is an integer }\} \\ & x \leq\left(u^{2}-i^{2}+g(u)^{2}-g(i)^{2}\right) \text { div }(2(u-i)) \end{aligned} ⇔⇔​Fi​(x)≤Fu​(x){ definition of Fi​,Fu​}(x−i)2+g(i)2≤(x−u)2+g(h)2{ calculus; i<u;x is an integer }x≤(u2−i2+g(u)2−g(i)2) div (2(u−i))​
总结完就是：
$$\mathrm{Sep}(i,u)=\left(u^2-i^2+g(u{)}^2-g(i{)}^2\right)\mathrm{div}(2(u-i))\text{\hspace{1em}(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<int> s(dim[0], 0);
      std::vector<int> 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;
      }
    }
  }

结果展示

如果我们设一个100x100的 然后对角线设为1的话，得到的图，其中左边是grid map右边是距离转了jet color图，蓝色即代表离被占据（也就是黑色那个1）越近的距离

打印数字的话10x10的grid如下（即欧式距离 每个格的分辨率为1）

测试包内包括了Bilinear进行grid 梯度求解，后面再补充进来，结果如图：