Dense Visual-Inertial Navigation System For Mobile Robots

Real-time dense mapping and pose estimation is essential for a wide range of navigation tasks in mobile robotic applications. We propose an odometry and mapping system that leverages the full photometric information from a stereo-vision system as well as inertial measurements in a probabilistic framework while running in real-time on a single low-power Intel CPU core. Instead of performing mapping and localization on a set of sparse image features, we use the complete dense image intensity information in our navigation system. By incorporating a probabilistic model of the stereo sensor and the IMU, we can robustly estimate the ego-motion as well as a dense 3D model of the environment in real-time. The probabilistic formulation of the joint odometry estimation and mapping process enables to efficiently reject temporal outliers in ego-motion estimation as well as spatial outliers in the mapping process. To underline the versatility of the proposed navigation system, we evaluate it in a set of experiments on a multi-rotor system as well as on a quadrupedal walking robot. We tightly integrate our framework into the stabilization-loop of the UAV and the mapping framework of the walking robot. It is shown that the dense framework exhibits good tracking and mapping performance in terms of accuracy as well as robustness in scenarios with highly dynamic motion patterns while retaining a relatively small computational footprint. This makes it an ideal candidate for control and navigation tasks in unstructured GPS-denied environments, for a wide range of robotic platforms with power and weight constraints. The proposed framework is released as an open-source ROS package.