Algorithm 1 Fast Marching Method

Require: A grid map X of size $m\times n$, source point $x_0$.

Ensure: The grid map X with the T value set for all cells.

Initialization.

1:for all $x\in X$do 2:    $T\left(x\right)\leftarrow \infty ;$ 3:end for 4:$T\left(x_0\right)\leftarrow 0;$ 5:$frozen\leftarrow x_0;$ 6:$open\leftarrow \mathcal{N}\left(x_0\right);$            ▹ Neighbors of $x_0$. 7:$open\leftarrow X\setminus (frozen\cup open);$   Iteration. 8:while$frozen\ne X$do 9:    $x_1\leftarrow \underset{x\in open}{argmin}d\left(x\right);$ 10:    for all ${\mathbf{x}}_{\mathrm{i}}=\mathcal{N}\left(x_1\right)\in T\cap \notin frozen$ do 11:        $\mathrm{UPDATE}\left({\mathbf{x}}_{\mathrm{i}}\right);$ 12:        $open\leftarrow open\cup \left\{{\mathbf{x}}_{\mathrm{i}}\right\};$ 13:    end for 14:    $open\leftarrow open\setminus \left\{x_1\right\};$         ▹ Updating sets. 15:    $frozen\leftarrow frozen\cup \left\{x_1\right\};$ 16:end while