Algorithm 3 Scan matching procedure

Input: transformations T, odometry poses P, algorithm iterations N

Output: robot poses X

1:$G\leftarrow$ createGraph(P,T) 2:fori in range (1, N) do 3:    $C\leftarrow$ getICPCandidates(G)         ▹ find scan pairs possible for matching 4:    for c in C do 5:        $t\leftarrow$ ICP_matching($s_i$, $s_j$, G)       ▹ match scans from each candidates pair 6:        G.addICPEdge(t) 7:    G.optimise() 8:$X\leftarrow$G.getRobotPoses() 9:returnX