Simultaneous Localization and Mapping (SLAM) combining visual and inertial measurements has achieved significant attention in the community of Robotics and Computer Vision. However, it is still a challenge to balance real-time requirements and accuracy. Therefore, this paper proposes a feedback mechanism for stereo Visual-Inertial SLAM (VISLAM) to provide accurate and real-time motion estimation and map reconstruction. The key idea of the feedback mechanism is that the frontend and backend in the VISLAM system can promote each other. The results of the backend optimization are fed back to the Kalman Filter (KF)-based frontend to reduce the motion estimate error caused by the well-known linearization of the KF estimator. Conversely, this more accurate motion estimate of the frontend can accelerate the backend optimization since it provides a more accurate initial state for the backend. In addition, we design a relocalization and continued SLAM framework with the feedback mechanism for the application of autonomous robot navigation or continuing SLAM. We evaluated the performance of the proposed VISLAM system through experiments on public EuRoC dataset and real-world environments. The experimental results demonstrate that our system is a promising VISLAM system compared with other state-of-the-art VISLAM systems in terms of both computing cost and accuracy. INDEX TERMS Kalman filter, nonlinear optimization, visual and inertial sensor fusion, visual-inertial simultaneous localization and mapping.

Abstract.A new method of establishing INS factor graph is proposed to deal with the serious waste of INS measurements in current integrated navigation factor graph algorithm. The INS observations pre-integrated to construct a smart factor during a time period, are used to solution simultaneously, to get the navigation states estimation. Then the estimated values can be used to construct an INS factor, adding into the INS factor graph. As a result, the new model is larger than the original, but the incremental inference technology makes sure the real-time ability of the factor graph algorithm. To analysis the performance of the proposed algorithm, INS and GNSS data are simulated, and the navigation position error of either the INS factors are added or not are compared. The results show that, the precision and stability of the factor graph algorithm are greatly improved by adding the INS factors. The proposed algorithm makes full use of the INS accuracy, and reflects the advantage of INS high stability.