Model-aided monocular visual-inertial state estimation and dense mapping

Robust state estimation and real-time dense mapping are two core capabilities for autonomous navigation of mobile robots. Global Navigation Satellite System (GNSS) and visual odometry/SLAM are popular methods for state estimation. However, when working between tall buildings or in indoor environments, GNSS fails due to limited sky view or obstruction from buildings. Visual odometry/SLAM are prone to long-term drifting in the absence of reliable loop closure detection. A state estimation method with global-consistent guarantee is desirable for navigation applications. As for real-time mapping, SLAM methods usually get a sparse map that is not good enough for obstacle avoidance and path-planning, and high-quality dense mapping is often computationally too demanding for mobile devices. Realizing the availability of city-scale 3D models, in this work, we improve our previous work on model-based global localization, and propose a model-aided monocular visual-inertial state estimation and dense mapping solution. We first develop a global-consistent state estimator by fusing visual-inertial odometry with the model-based localization results. Utilizing depth prior from the model, we perform motion stereo with semi-global disparity smoothing. Our dense mapping pipeline is capable of online detection of obstacles that are originally not included in the offline 3D model. Our method runs onboard an embedded computer in real-time. We validate both the state estimation and mapping accuracy in real-world experiments.