Algorithm 1 Environment mapping procedure

Input: Scans $S=[s_0\cdots s_N]$, Odometry measurements $O=[o_0\cdots o_N]$

Output: Occupancy GridMap of the area m

1:$P\leftarrow$ odometryConstraints(O) 2:$T\leftarrow \left[\right]$      ▹ initialise empty list for matching results 3:fori in range (0, N-1) do 4:    $t\leftarrow$ ICP_matching($s_i$, $s_{i+1}$, $P_i$)        ▹ See Algorithm 2 5:    T.insert(t) 6:$X\leftarrow$ graphSLAM(S, T, P)            ▹ See Algorithm 3 7:$m\leftarrow$ occupancyGridMap(S, X)