Robust vision-aided navigation using Sliding-Window Factor graphs

This paper proposes a navigation algorithm that provides a low-latency solution while estimating the full nonlinear navigation state. Our approach uses Sliding-Window Factor Graphs, which extend existing incremental smoothing methods to operate on the subset of measurements and states that exist inside a sliding time window. We split the estimation into a fast short-term smoother, a slower but fully global smoother, and a shared map of 3D landmarks. A novel three-stage visual feature model is presented that takes advantage of both smoothers to optimize the 3D landmark map, while minimizing the computation required for processing tracked features in the short-term smoother. This three-stage model is formulated based on the maturity of the estimation of the 3D location of the underlying landmark in the map. Long-range associations are used as global measurements from matured landmarks in the short-term smoother and loop closure constraints in the long-term smoother. Experimental results demonstrate our approach provides highly-accurate solutions on large-scale real data sets using multiple sensors in GPS-denied settings.

Authors

• Han-Pang Chiu
• Stephen Williams
• Frank Dellaert
• Supun Samarasekera
• Rakesh Kumar 0001

Keywords

• Three-dimensional displays
• Navigation
• Smoothing methods
• Estimation
• Sensors
• Solid modeling
• Current measurement
• Factor Graph
• Smoothing
• Visual Features
• Global Measures
• 3D Position
• Feature Tracking
• Loop Closure
• Three-stage Model
• Sliding Time Window
• 3D Landmarks
• State Variables
• Mapping Algorithm
• Past Conditions
• Spatial Association
• Maximum A Posteriori
• Local Coordinate System
• Simultaneous Localization And Mapping
• Top Of The Tree
• Bayberry
• Large-scale Scenarios
• Binary Factor
• World Coordinate System
• 3D Error
• Smoothing Algorithm
• Nonlinear Least Squares Problem
• Nonlinear Factors
• Incremental Update
• Binary Constraints