Complex Terrain Mapping with Multi-camera Visual Odometry and Realtime Drift Correction

We present a low-cost navigation system that integrates 3D data in realtime, allowing exploration and mapping of complex terrain and GPS-denied regions with an inexpensive sensor package. Precise integration of 3D data from sensors on a moving platform requires accurate bearing and position estimates, delivered at high frequency. This bar has only been met using expensive, high-end IMUs and/or differential GPS systems - by contrast, our sensor package includes only a low-cost MEMS IMU. We use multi-camera visual odometry to obtain locally accurate position and orientation for integration of sequential scans from a 3D sensor. These intermediate point clouds accumulate in a custom database which features spatial caching and binary serialization for efficient querying and logarithmic retrieval times. Realtime alignment of overlapping 3D point clouds is used to correct drift. The system was tested using a mobile robot and used to map complex indoor and outdoor environments.