RGB-D Inertial Odometry for Indoor Robot via Keyframe-based Nonlinear Optimization

In recent years, Visual Inertial Odometry(VIO) has been an attractive topic in robotics area. VIO aim to estimate robots trajectory via analyzing measurements obtained from visual sensors and inertial sensors. There have been several outstanding results in these topics, however, these approaches were suffering from the longtime initialization and lack the robustness. In this paper, we present a novel tightly-coupled method which promotes accuracy and robustness in pose estimation with fusing image and depth information from the RGB-D camera and the measurement from the inertial sensor. Instead of using a filter-based method, we put visual and inertial data into a nonlinear optimization framework. In our work, a robust optimization-based method is used to obtain high accuracy estimator initialization. In order to maintain the balance of accuracy with computational complexity, we use a sliding-window optimizer to optimize the keyframes pose graph. We also propose an implement of 3D reconstruction. We test our algorithm on the Automated Guided Vehicle (AGV) platform, and we open source our implementations for PCs.