【基礎計算】ESDF柵格距離圖計算並行加速版
前言與參考
這一部分僅為路徑規劃原始碼及論文GPIR的一個小部分，但是有程式碼實現，第一次看的時候有些懵，所以特此記錄；主要是設定好了柵格地圖後，添加了障礙物後，對其的歐式距離計算和梯度計算等。原始碼中為了加速，不同以往的直接每個點逐個計算遍歷，而是分了兩個步驟同時使用多執行緒平行計算進行加速
參考文獻：
- 2002論文：a general algorithm for computing distance transforms in linear time
- 完整GPIR 路徑規劃原始碼 github連結
- 僅sdf_test 的debug及測試程式碼 gitee連結
- openmp平行計算：介紹與簡單測試連結
通常在計算圖/柵格距離時，複雜度為 \(O(m\times n)\) 也就是pixel of image OR dim of map，其中在計算歐式距離時因為還有一些額外的計算操作所以在計算大圖時會有些耗時，此處根據參考1論文中的方法提出一種可以使用多執行緒平行計算的方式，使得理論上覆雜度為 \(O(m\times n/p)\) 其中p為執行緒數量，可以通過結果表得知，在較為大的pixel or dim下，加速較為明顯
現在的主要應用可能是在加速路徑規劃時的柵格地圖及距離圖時效果較為明顯，效果預覽（灰黑為grid圖，彩色為離障礙物距離圖，最右邊為距離梯度示意圖）
以下我們進行簡潔虛擬碼及實際程式碼對應，及整個過程的詳細講解
簡潔過程
論文中總結
- In a first phase each column \(C_x\) is separately scanned. (defined by points \((x , y)\) with x fixed) 其中距離由最近的點決定
- In a second phase each row \(R_y\) is separately scanned. (defined by points \((x , y)\) with y fixed). 對於\(R_y\)上的每個點，最小的EDT值為\((x-x')^2+G(x',y)^2\) 其中 \((x',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，首先定義一下久遠的符號 \(\operatorname{MIN}(k:P(k):f(k))\) 是指 \(k\)在 \(P(k)\)範圍內使得 \(f(k)\) 最小的值
正常的距離計算通常需要逐次計算距離 \(dt[x,y]=\sqrt{EDT(x,y)}\)
\[\operatorname{EDT}(x, y)=\operatorname{MIN}\left(i, j: 0 \leq i<m \wedge 0 \leq j<n \wedge b[i, j]:(x-i)^{2}+(y-j)^{2}\right) \]為了所有的EDT,MDT, CDT的計算方便後續和程式碼中的G 如下：
\[\begin{aligned} \operatorname{EDT}(x, y) &=\operatorname{MIN}\left(i: 0 \leq i<m:(x-i)^{2}+G(i, y)^{2}\right)\\ \text{where } G(i, y)&=\operatorname{MIN}(j: 0 \leq j<n \wedge b[i, j]:|y-j|) \end{aligned}\tag{2} \]然後就進入了第一階段，可以看出第一階段的column scan是可以直接並行的（仔細對著程式碼看看就能看出來了），因為每個column和其他直接互不影響，然後column下在往下直接for row，第一階段算的是G值，比如示例test中的如果把對角線都設為1，其他給0（假設>0即 被佔據），原始佔據圖與G圖如下：
比較難以理解的應該是第二階段，但是第二階段其實就是上面公式(2)所示，直接手推一下，取min，如上圖右邊，只是論文的大部分在介紹如何優雅的寫出這樣的並行for
第二階段首先y是fixed，也就是一個column看下來，所以簡化一下公式二就是把y去掉
\[\operatorname{DT}(x, y)=\operatorname{MIN}(i: 0 \leq i<m: f(x, i)) \tag{3} \]其中 \(f(x,i)\) 的定義，因為全文同時對其他的距離函式也進行了說明，所以原文中分了出來，但是我們在這裡僅看著歐式距離計算哈，可以看到和公式(2)的區別就是y被藏起來了，因為y已經是fixed了
\[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} \]申明下圖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) \leq f(x, i)\)
\[H(x, u)=\operatorname{MIN}(h: 0 \leq h<u \wedge \forall(i: 0 \leq i<u: f(x, h) \leq f(x, i)): h) \]由此定義處\(s(u)\) 即從左到右的scan，拿到minimizers
\[\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} \]為了找到 \(x^*\) 我們引入 \(\text{Sep}\) ，\(\text{Sep}(i,u)\) 指的是第一個不小於水平座標相交點\(F_u\)和\(F_i\) 的整數，公式表示也就是：
\[F_{i}(x) \leq F_{u}(x) \quad \Leftrightarrow \quad x \leq \operatorname{Sep}(i, u) \]由此可以獲取\(x^*=\text{Sep}(s[l^*],u)\)， 然後\(\text{Sep}\)這個取決於我們想要計算的距離是什麼，比如EDT的話就是：
\[\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} \]總結完就是：
\[\operatorname{Sep}(i, u)=\left(u^{2}-i^{2}+g(u)^{2}-g(i)^{2}\right) \operatorname{div}(2(u-i)) \tag{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 梯度求解，後面再補充進來，結果如圖：