Algorithm 2 Scan matching procedure

Input: Scans $s_m,s_n$, initial transformation guess t, number of ICP iterations N

Output: transformation t

1:fori in range (1, N) do 2:    transformScan($s_n$, t) 3:    $L_m,L_n\leftarrow$ extract line features from $s_m$ and $s_n$ 4:    $P_m,P_n\leftarrow$ find corresponding lines and points in $L_m,L_n$ sets 5:    $t\leftarrow$ ICP(t, $CP$)       ▹ match corresponding points using ICP 6:returnt