To match two 3D scans with the ICP algorithm it is necessary to
have a sufficient starting guess for the second scan pose. In
earlier work we used odometry [23] or the planar HAYAI
scan matching algorithm [16]. However, the latter
cannot be used in arbitrary environments, e.g., the one presented
in Fig. 1 (bad asphalt, lawn,
woodland, etc.). Since the motion models change with different
grounds, odometry alone cannot be used. Here the robot pose is
the 6vector
or, equivalently the tuple containing the rotation
matrix and translation vector, written as 44 OpenGLstyle
matrix [8].^{} The
following heuristic computes a sufficiently good initial
estimation. It is based on two ideas. First, the transformation
found in the previous registration is applied to the pose
estimation  this implements the assumption that the error model
of the pose estimation is locally stable. Second, a pose update
is calculated by matching octree representations of the scan
point sets rather than the point sets themselves  this is done
to speed up calculation:
Therefore, calculating
requires a matrix
inversion. Finally, the 6D pose
is calculated by
best 

Note: Step 5b requires 6 nested loops, but the computational requirements are bounded by the coarsetofine strategy inherited from the octree processing. The size of the octree cubes decreases exponentially with increasing . We start the algorithm with a cube size of 75 cm and stop when the cube size falls below 10 cm. Fig. 2 shows two 3D scans and the corresponding octrees. Furthermore, note that the 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.
After an initial starting guess is found, the range image registration from section 2 proceeds and the 3D scans are precisely matched.