Approximate $k$d-trees

Since the ICP algorithm, and therefore our 6D SLAM method, extensively computes nearest neigbours, approximating the nearest neigbours will speed up the algorithm. S. Arya and D. Mount introduce the following notion for approximating the nearest neighbor [3]: Given an $\eps > 0$, then the point $\V p \in D$ is the $(1 + \eps )$-approximate nearest neighbour of the point $\V p_q$, iff

$$\norm {\V p - \V q} \leq (1+\eps ) \norm {\V p^* - \V q},$$

where $\V p^*$ denotes the true nearest neighbour, i.e., $\V p$ has a maximal distance of $\eps$ to the true nearest neighbour. Using this notation in every step the algorithm records the closest point $\V p$. The search terminates if the distance to the unanalyzed leaves is larger than

$$\norm {\V p_q - \V p}/(1+\eps ).$$

Fig. (left) shows an example where the gray cell needs not to be analyzed, since the point $\V p$ satisfies the approximation criterion.