本博客主要阅读了Distance Transforms of Sampled Functions文献，并将其中的EDT算法利用Python进行了复现，算法细节不进行讨论，若需要进一步了解的可以去看Fast Planner——ESDF地图中距离计算（欧几里得距离转换EDT）-CSDN博客这篇博客。下面附上Python代码以及结果图。

# 参考文献： Distance Transforms of Sampled Functions
# 功能：在 5X5 的网格中随意设置网格作为障碍物，通过EDT算法计算出其他网格距离场
import numpy as np
import matplotlib.pyplot as plt

DF_MAX = 25

# EDT
# 输入：q_array->q节点位置
#      f_q_val->f(q)
def EDT(q_array, f_q_val):
    # 计算下包络线
    k = 0
    v = np.zeros(q_array.shape[0])
    v[0] = q_array[0]
    z = np.zeros(q_array.shape[0] + 1)
    z[0] = -DF_MAX
    z[1] = DF_MAX
    for i in range(1, q_array.shape[0]):
        # 计算抛物线交点
        s = ((f_q_val[int(q_array[i])] + q_array[i] * q_array[i]) - (f_q_val[int(v[k])] + v[k] * v[k])) / (2 * q_array[i] - 2 * v[k])
        while s < z[k]:
            k = k -1
            s = ((f_q_val[int(q_array[i])] + q_array[i] * q_array[i]) - (f_q_val[int(v[k])] + v[k] * v[k])) / (2 * q_array[i] - 2 * v[k])
        k = k + 1
        v[k] = q_array[i]
        z[k] = s
        z[k + 1] = DF_MAX
    
    # 填充每个网格的值
    p_value = np.ones(5) * DF_MAX
    k = 0
    for i in range(5):
        while z[k + 1] < i:
            k = k + 1
        p_value[i] = (i - v[k]) * (i - v[k]) + f_q_val[int(v[k])]
    
    return p_value

# 二维网格地图: 1表示有障碍物，0表示没有障碍物
map = np.array([
    [0, 1, 0, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 1, 0, 0],
    [1, 0, 0, 0, 0]
])

df_map = np.ones((5, 5)) * DF_MAX

# 按列操作
obs_array = np.ones(5)
for i in range(5):
    map_i_col = map[:, i]
    q_array = np.where(map_i_col == obs_array)[0]
    if q_array.shape[0] != 0:
        f_q_value = np.zeros(map_i_col.shape[0])
        df_i_col_map =  EDT(q_array, f_q_value)
        df_map[:, i] = df_i_col_map

# 按行操作
for i in range(5):
    map_i_row = df_map[i, :]
    q_array = np.where(map_i_row < np.ones(5) * DF_MAX)[0]
    if q_array.shape[0] != 0:
        f_q_value = map_i_row
        df_i_row_map =  EDT(q_array, f_q_value)
        df_map[i, :] = df_i_row_map   

# 绘制图像
plt.imshow(df_map, cmap='viridis')
plt.colorbar()
plt.show()