Fast Planner——ESDF地图中距离计算（欧几里得距离转换EDT）

本文是Fast Planner构建ESDF地图部分中距离场计算相关函数的说明。ESDF中距离场的计算过程其实就是计算出地图更新范围内每个空闲体素到附近障碍物体素的最小距离的过程。

Fast Planner采用了文献[1]中提出的方法，来进行ESDF地图中距离的计算。该方法通过多个1-D维度上的距离计算，最终得到3-D空间上的距离值。

1. 算法原理

对于1-D的情况，首先计算一系列抛物线的下包络集合，然后 $q$ 点在下包络上对应的数值 $f(q)$ 即为该点到最近障碍物的距离 ${\mathcal{D}}_f(p)$。$${\mathcal{D}}_f(p)=\underset{q\in \mathcal{G}}{\min }\left((p-q{)}^2+f(q)\right)$$ 其中，$\mathcal{G}$ 表示一系列的一维栅格；$q$ 表示这些一维栅格中是障碍物的部分；$p$ 是需要计算最小障碍物距离的位置；$f(q)$ 是一个偏置量，对于仅有1-D的情况 $f(q)=0$，若存在多个维度 $f(q)$ 的数值为上一维度计算出来的 ${\mathcal{D}}_f(p)$。

在一系列抛物线构成的下包络中，用$k$ 表示用来构成下包络的抛物线个数；$v[k]$ 表示第 $k$ 个抛物线的顶点；$z[k]$ 和 $z[k+1]$ 表示第 $k$ 个抛物线在整个下包络中的有效范围，其中 $z[k]$ 表示的是第 $k$ 个抛物线与第 $k-1$ 个抛物线的交点；

假设一顶点为 $q$ 的新抛物线与原有下包络线最右侧抛物线 $v[k]$ 的交点为 $s$，该交点的位置只存在两种可能：交点 $s$ 在 $z[k]$ 左边或在 $z[k]$ 右面，如下图所示。需要注意的是，该情况下任意两个抛物线有且仅有一个交点 $s$ ，且该交点的水平位置为
$$s=\frac{(f(r)+r^2)-(f(q)+q^2)}{2r-2q}$$ 其中，$q$ 和 $r$分别表示对应的两个抛物线顶点的水平位置。

（1）若交点 $s$ 在 $z[k]$ 右边，即 $s>z[k]$，则将抛物线 $q$ 添加为下包络最右边的抛物线，有$k=k+1$，$v[k]=q$，$z[k]=s$，$z[k+1]=+\infty$；

（2）若交点 $s$ 在 $z[k]$ 左边，即 $s<z[k]$，则将下包络原有的抛物线 $v[k]$ 删除，并有$k=k-1$，然后重新计算抛物线 $q$ 与下包络此时最右侧的抛物线 $v[k]$ 的交点 $s$，并重复上述过程，直到交点 $s$ 在 $z[k]$ 右边。

上述计算1-D情况下，距离计算的算法伪代码如下图所示。

2. 代码实现

伪代码对应的C++代码如下所示。

template <typename F_get_val, typename F_set_val>
void SDFMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim) {
  int v[mp_.map_voxel_num_(dim)];
  double z[mp_.map_voxel_num_(dim) + 1];

  int k = start;
  v[start] = start;
  z[start] = -std::numeric_limits<double>::max();
  z[start + 1] = std::numeric_limits<double>::max();

  for (int q = start + 1; q <= end; q++) {
    k++;
    double s;

    do {
      k--;
      s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]);
    } while (s <= z[k]);

    k++;

    v[k] = q;
    z[k] = s;
    z[k + 1] = std::numeric_limits<double>::max();
  }

  k = start;

  for (int q = start; q <= end; q++) {
    while (z[k + 1] < q) k++;
    double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
    f_set_val(q, val);
  }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

上述算法仅对多维空间的一维进行了计算，在实际应用中需要计算3D空间中的距离。在Fast Planner中，通过将上述算法分别在3个维度中计算一次，并将上一个维度的计算出的数值 ${\mathcal{D}}_f(p)$ 赋值给下一个维度计算时的$f(q)$，经过3次遍历以后就实现了 3D 空间中空闲体素到最近障碍物物栅格距离的计算。该计算过程详见函数 SDFMap::updateESDF3d() ，其 3 次调用函数 fillESDF() 对每个维度单独计算距离（维度计算顺序：$z\to y\to x$）。

void SDFMap::updateESDF3d() {
  Eigen::Vector3i min_esdf = md_.local_bound_min_;
  Eigen::Vector3i max_esdf = md_.local_bound_max_;

  /* ========== compute positive DT ========== */

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
    for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
      fillESDF(
          [&](int z) {
            return md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 1 ?
                0 :
                std::numeric_limits<double>::max();
          },
          [&](int z, double val) { md_.tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2],
          max_esdf[2], 2);
    }
  }

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
       //md_.tmp_buffer1_是上一维度计算的结果，作为本维度计算的基础赋给 f(q) 
      fillESDF([&](int y) { return md_.tmp_buffer1_[toAddress(x, y, z)]; },
               [&](int y, double val) { md_.tmp_buffer2_[toAddress(x, y, z)] = val; }, min_esdf[1],
               max_esdf[1], 1);  
    }
  }

  for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
       //md_.tmp_buffer2_是上一维度计算的结果，作为本维度计算的基础赋给 f(q) 
      fillESDF([&](int x) { return md_.tmp_buffer2_[toAddress(x, y, z)]; },
               [&](int x, double val) {
               md_.distance_buffer_[toAddress(x, y, z)] = mp_.resolution_ * std::sqrt(val);
                 //  min(mp_.resolution_ * std::sqrt(val),
                 //      md_.distance_buffer_[toAddress(x, y, z)]);
               },
               min_esdf[0], max_esdf[0], 0);
    }
  }

  /* ========== compute negative distance ========== */
  for (int x = min_esdf(0); x <= max_esdf(0); ++x)
    for (int y = min_esdf(1); y <= max_esdf(1); ++y)
      for (int z = min_esdf(2); z <= max_esdf(2); ++z) {

        int idx = toAddress(x, y, z);
        if (md_.occupancy_buffer_inflate_[idx] == 0) {
          md_.occupancy_buffer_neg[idx] = 1;

        } else if (md_.occupancy_buffer_inflate_[idx] == 1) {
          md_.occupancy_buffer_neg[idx] = 0;
        } else {
          ROS_ERROR("what?");
        }
      }

  ros::Time t1, t2;

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
    for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
      fillESDF(
          [&](int z) {
            return md_.occupancy_buffer_neg[x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) +
                                            y * mp_.map_voxel_num_(2) + z] == 1 ?
                0 :
                std::numeric_limits<double>::max();
          },
          [&](int z, double val) { md_.tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2],
          max_esdf[2], 2);
    }
  }

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
      fillESDF([&](int y) { return md_.tmp_buffer1_[toAddress(x, y, z)]; },
               [&](int y, double val) { md_.tmp_buffer2_[toAddress(x, y, z)] = val; }, min_esdf[1],
               max_esdf[1], 1);
    }
  }

  for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++) {
      fillESDF([&](int x) { return md_.tmp_buffer2_[toAddress(x, y, z)]; },
               [&](int x, double val) {
                 md_.distance_buffer_neg_[toAddress(x, y, z)] = mp_.resolution_ * std::sqrt(val);
               },
               min_esdf[0], max_esdf[0], 0);
    }
  }

  /* ========== combine pos and neg DT ========== */
  for (int x = min_esdf(0); x <= max_esdf(0); ++x)
    for (int y = min_esdf(1); y <= max_esdf(1); ++y)
      for (int z = min_esdf(2); z <= max_esdf(2); ++z) {

        int idx = toAddress(x, y, z);
        md_.distance_buffer_all_[idx] = md_.distance_buffer_[idx];

        if (md_.distance_buffer_neg_[idx] > 0.0)
          md_.distance_buffer_all_[idx] += (-md_.distance_buffer_neg_[idx] + mp_.resolution_);
      }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103

3. 计算过程示意

一个2D栅格地图中计算距离的示例，栅格地图中空白栅表示free，黑色实心栅格表示障碍物。

首先对列的维度进行计算，距离的计算结果如下图所示，其中 ${\infty }^2$ 表示该列无障碍物。

然后在列向维度的基础上，将算出来的相应栅格的数值作为行维度计算时 $f(q)$ 的数值。在计算行维度时，所有有具体数值的栅格都被认为是障碍物，均需要在该栅格处绘制抛物线，上一维度计算出来的不同的数值大小即代表该栅格不同的偏置量 $f(q)$。以上图中最后三行为例计算的结果如下图所示。

在列维度计算的基础上，这三行中第一行计算行维度（因为是2D栅格，也即最终距离）中每个栅格距离计算示意如下图所示。

图示2D栅格最终计算出的ESDF地图可视化结果如下图所示，浅色的线条是ESDF距离场中的等高线。

上图的Matlab代码实现方式一

% 生成2D栅格矩阵
bw = zeros(7,10);
% 在栅格的对应位置设置障碍物
bw(2,8) = 1;
bw(4,5) = 1;
bw(4,6) = 1;
bw(6,1) = 1; 
bw(6,9) = 1;
bw(7,10) = 1; 

% 利用matlab自带函数进行欧式变换得到栅格地图对应的ESDF地图，表示地图上每个栅格离其最近障碍物的欧氏距离。
D1 = bwdist(bw,'euclidean');
% 绘图
figure
imagesc(D1), title('Euclidean')
% 添加等高线
hold on, imcontour(D1,'w')
colorbar
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

上图的Matlab代码实现方式二（改进了障碍物的赋值方式），对于大型栅格赋值速度会更快

% 2D栅格地图初始化
bw = zeros(7,10);
% 栅格地图中障碍物对应的行和列索引
obs = [2 8; 4 5; 4 6; 6 1; 6 9; 7 10];
% 将这些索引转换为线性索引，并赋给 bw 矩阵中相应位置上的元素。这样做比对 bw 中每个元素进行单独赋值要快很多。
idx = sub2ind(size(bw), obs(:,1), obs(:,2));
bw(idx) = 1;

figure
contourf(D1), title('Euclidean')
hold on, contour(D1,'w')
colorbar
1
2
3
4
5
6
7
8
9
10
11
12

参考文献

[1] Pedro F. Felzenszwalb, Huttenlocher Daniel P. Distance Transforms of Sampled Functions[J]. Theory of Computing, 2012, 8(1): 415-428.
[2] 欧几里得距离转换（EDT）算法
[3] 深蓝学院移动机器人运动规划