Enabling Aggressive Motion Estimation at Low-drift and Accurate Mapping in Real-time

Abstract

We present a data processing pipeline to online estimate ego-motion and build a map of the traversed environment, leveraging data from a 3D laser, a camera, and an IMU. Different from traditional methods that use a Kalman filter or factor-graph optimization, the proposed method employs a sequential, multi-layer processing pipeline, solving for motion from coarse to fine. The resulting system enables highfrequency, low-latency ego-motion estimation, along with dense, accurate 3D map registration. Further, the system is capable of handling sensor degradation by automatic reconfiguration bypassing failure modules. Therefore, it can operate in the presence of highly dynamic motion as well as in dark, textureless, and structure-less environments. During experiments, the system demonstrates 0.22% of relative position drift over 9.3km of navigation and robustness w.r.t aggressive motion such as highway speed driving (up to 33m/s).