Sequential scan matching with sensor order

ICP (Iterative Closest Point) algorithm plays an important role in sequential scan matching SLAM. In the algorithm, closest point searching is the well-recognized speed bottleneck. We realize that a searching-friendly structure already exists in the raw data sequence from range acquisition device, and the acquisition order, which we called sensor order, is naturally derived from sensor working mechanism. We show that considering these geometric relations, the searching space could be greatly reduced and can lead to an efficient 3D sequential scan matching algorithm.