Closing the Loop.

After matching multiple 3D scans, errors have accumulated and loops would normally not be closed. Our algorithm automatically detects a to-be-closed loop by registering the last acquired 3D scan with earlier acquired scans. Hereby we first create a hypothesis based on the maximum laser range and on the robot pose, so that the algorithm does not need to process all previous scans. Then we use the octree based method presented in section 3.1 to revise the hypothesis. Finally, if a registration is possible, the computed error, i.e., the transformation ($\M R$, $\V t$) is distributed over all 3D scans. The respective part is weighted by the distance covered between the scans, i.e.,

$$c_i = \frac{\text{length of path from start of the loop to scan pose } i}{\text{overall length of path}}$$

1. The translational part is calculated as $\V t_i = c_i \V t$.
2. Of the three possibilities of representing rotations, namely, orthonormal matrices, quaternions and Euler angles, quaternions are best suited for our interpolation task. The problem with matrices is to enforce orthonormality and Euler angles show Gimbal locks [8]. A quaternion as used in computer graphics is the 4 vector $\dot {\V q}$. Given a rotation as matrix $\M R$, the corresponding quaternion $\dot {\V q}$ is calculated as follows:

   
   
   

   $$\dot {\V q} = \left( \begin{array}{c} q_0 \\ q_x \\ q_y \\ q_z \end{arra... ...\mbox{with the elements $r_{i,j}$ of $\M R$.\footnotemark [2]}$$ (13)

   
   
   

   The quaternion describes a rotation by an axis $\V a \in \RR ^3$ and an angle $\theta$ that are computed by

   
   
   

   $$\V a = \left( \begin{array}{c} \frac{q_x}{\sqrt{1 - q_0^2}}\\... ...{array}\right) \qquad \mbox{and} \qquad \theta = 2 \arccos q_o.$$

   
   
   

   The angle $\theta$ is distributed over all scans using the factor $c_i$ and the resulting matrix is derived as [8]:

$$\M R_i = \left( \begin{array}{cc} \cos(c_i \theta) + a_x^2 ( 1 - \cos(... ...i \theta) + a_y a_z ( 1 - \cos(c_i \theta)) \end{array}\right.$$

$$end{tex2html_deferred}$$ (14)

$$end{tex2html_deferred}%[2.5ex] \qquad \qquad \qquad \left. \begin{array}{c} -a_y \sin(c_i \t... ..._i \theta) + a_z^2 ( 1 - \cos(c_i \theta)) \end{array} \right).$$ (15)

The next step minimizes the global error.