Algorithm 1 Data association

Input:

Initial camera position $\overline{\mathbf{p}}$;

HD Map $\mathcal{M}$;

Extracted features $\mathcal{F}$;

Output:

Correspondence $\mathbf{c}$

1: ${\mathbf{c}}_0=Possible\_correspond(\mathbf{M},\mathcal{F})$

2: for each ${\mathbf{c}}_{1\sim 3}^{\left(L\right)}\in {\mathbf{c}}_0$ do

3:  $\widehat{\mathbf{c}}={\mathbf{c}}_{1\sim 3}^{\left(L\right)}$

4:  Calculate ${\widehat{\mathbf{x}}}^{*}$ based on $\widehat{\mathbf{c}}$ according to Equation (14).

5:  ${\mathbf{c}}^{*}=Closest\_correspond (\mathbf{M},\mathcal{F},{\widehat{\mathbf{x}}}^{*})$

6:  if ${∥\begin{array}{c}{\widehat{\mathbf{x}}}^{*}-\overline{\mathbf{p}}\end{array}∥}_1<D$ then

7:   $\mathbf{C}=\mathbf{C}\cup {\mathbf{c}}^{*}$

8:  end if

9: end for

10: $\mathbf{c}=\mathbf{C}\left(size\right(C)==MAX(size\left(C\right)\left)\right)$