Algorithm 4 New node initialization

Require:  cell positions of one frontier segment $cells\_frontier$, the current node’s ID $i{d}_{cur}$, a new frontier node’s ID $i{d}_{new}$

Ensure:  $nod{e}_{i{d}_{new}}$, $\mathbf{t}{\mathbf{r}}_{i{d}_{cur},i{d}_{new}}$

1: function Initialize-New-Node($cells\_frontier$, $i{d}_{cur}$, $i{d}_{new}$)

2:     ${\left[r_m,c_m\right]}^T\leftarrow \mathrm{calculate}\_\mathrm{median}\_\mathrm{position}\left(cells\_frontier\right)$

3:     ${\left[r_{rep},c_{rep}\right]}^T\leftarrow \underset{{\left[p,q\right]}^T\in cells\_frontier}{argmin}∥{\left(p-r_m\right)}^2+{\left(q-c_m\right)}^2∥$

4:     ${\left[x_{rep},y_{rep}\right]}^T\leftarrow \mathrm{find}\_\mathrm{metric}\_\mathrm{position}\left({\left[r_{rep},c_{rep}\right]}^T,gridma{p}_{cur}\right)$

5:     $\mathbf{t}{\mathbf{r}}_{i{d}_{cur},i{d}_{new}}\leftarrow {\left[x_{rep},y_{rep},0\right]}^T$

6:     $cell{s}_{i{d}_{new}}\leftarrow \varnothing$

7:     for all ${\left[p,q\right]}^T\in cells\_frontier$ do

8:         ${\left[x,y\right]}^T\leftarrow \mathrm{find}\_\mathrm{metric}\_\mathrm{position}\left({\left[p,q\right]}^T,gridma{p}_{cur}\right)$

9:         $cell{s}_{i{d}_{new}}\leftarrow cell{s}_{i{d}_{new}}\cup \left\{{\left[x,y\right]}^T\right\}$

10:     end for

11:     $nod{e}_{i{d}_{new}}\leftarrow \left\{i{d}_{new},i{d}_{cur},cell{s}_{i{d}_{new}},0,FALSE,FALSE,\varnothing \right\}$

12:     return $\left\{nod{e}_{i{d}_{new}},\mathbf{t}{\mathbf{r}}_{i{d}_{cur},i{d}_{new}}\right\}$

13: end function