Item 2: Heuristic Initial Estimations for ICP Scan Matching.

Initial estimations for ICP scan matching are computed with an octree based heuristic. The algorithm sets initially $\Delta \V P_$best to the 6-vector $(\V t, \V R({\theta_{x,{n}}}, {\theta_{y,{n}}}, {\theta_{z,{n}}})) = (\VNull , \V R(\!\VNull ))$. Then, an octree $\OctB _M$ for the $n$th 3D scan (model set $M$) and an octree $\OctB _D$ for the $(n+1)$th 3D scan (data set $D$) is generated (cf. Fig. 2). The estimation is done for search depth $t \in [t_$Start$,\ldots,t_$End$]$ in the octrees. Hereby a a transformation $\Delta \V P_$best$= (\V t, \V R)$ is computed as follows:

1. Calculate a maximal displacement and rotation $\Delta \V P_$max depending on the search depth $t$ and currently best transformation $\Delta \V P_$best.
2. For all discrete 6-tuples $\Delta \V P_i \in [- \Delta \V P_$max$,\Delta \V P_$max$]$ in the domain $\Delta \V P = (x,y,z,\theta_x, \theta_y,\theta_z)$ displace $\OctB _D$ by $\Delta \M P_i \cdot \Delta \M P \cdot \M P_n$. Evaluate the matching of the two octrees by counting the number of overlapping cubes and save the best transformation as $\Delta \V P_$best.
   

Finally, the scan pose is updated using matrix multiplication, i.e.,

$$\M P_{n+1} = \Delta \M P_ \cdot \Delta \M P \cdot \M P_{n}.$$

Note: Step 2 requires 6 nested loops, but the computational requirements are bounded by the coarse-to-fine strategy inherited from the octree processing. The size of the octree cubes decreases exponentially with increasing $t$. We start the algorithm with a cube size of 75 cm$^3$ and stop when the cube size falls below 10 cm$^3$. Fig. 2 shows two 3D scans and the corresponding octrees. Furthermore, note that this heuristic works best outdoors. Due to the diversity of the environment the match of octree cubes will show a significant maximum, while indoor environments with their many geometry symmetries and similarities, e.g., in a corridor, are in danger of producing many plausible matches.

Figure 2: Left: Two 3D point clouds. Middle: Octree corresponding to the black point cloud. Right: Octree based on the blue points.