Abstract
This paper presents a task-oriented computational framework to enhance Visual-Inertial Navigation (VIN) in robots, addressing challenges such as limited time and energy resources. The framework strategically selects visual features using a Mean Squared Error (MSE)-based, non-submodular objective function and a simplified dynamic anticipation model. To address the NP-hardness of this problem, we introduce four polynomial-time approximation algorithms: a classic greedy method with constant-factor guarantees; a low-rank greedy variant that significantly reduces computational complexity; a randomized greedy sampler that balances efficiency and solution quality; and a linearization-based selector based on a first-order Taylor expansion for near-constant-time execution. We establish rigorous performance bounds by leveraging submodularity ratios, curvature, and element-wise curvature analyses. Extensive experiments on both standardized benchmarks and a custom control-aware platform validate our theoretical results, demonstrating that these methods achieve strong approximation guarantees while enabling real-time deployment.
Introduction: The Visual Attention Problem
For autonomous robots, processing every visual detail is computationally prohibitive. Efficient navigation requires visual attention—the ability to strategically select and track only the most informative visual features, such as corners or edges. Traditional approaches often rely on submodular metrics like "log-determinant," which are easier to optimize but may not directly reflect estimation accuracy.
This research focuses on the Mean Squared Error (MSE) objective. While MSE provides a direct measure of navigation accuracy, it is non-submodular. This means it lacks the "diminishing returns" property, making optimal feature selection an NP-hard problem. This paper introduces a framework to bridge the gap between theoretical accuracy and real-time robotic constraints.
Task-Oriented Framework and Anticipation
The proposed framework divides the navigation task into a front-end for feature extraction and a back-end for state estimation. A key innovation is the integration of a dynamic anticipation model. By predicting the robot's future trajectory, the system can prioritize features that will remain visible and informative over a longer horizon, rather than just the immediate next frame.
Efficient Approximation Algorithms
To overcome the computational hurdles of non-submodular optimization, four polynomial-time algorithms were developed:
- Simple Greedy: Iteratively selects features with the highest marginal gain. New theoretical bounds were derived to guarantee its performance despite the non-submodularity of MSE.
- Fast Low-Rank Greedy: Utilizes the Sherman-Morrison-Woodbury identity to avoid full matrix inversions, significantly speeding up the selection process.
- Randomized Greedy: Samples a subset of candidates to balance speed and quality, ideal for high-density feature environments.
- Linearization-based Selector: Approximates the MSE objective using leverage scores and first-order Taylor expansion, allowing for near-constant-time execution.
Experimental Validation
The framework was tested on the EuRoC MAV Dataset (hex-rotor UAV) and a custom QCar Platform (1/10-scale RC car). The QCar setup was particularly valuable for testing the anticipation model, as it provided real-time control inputs to predict future horizons.
Key Results
- Accuracy: MSE-based selection consistently outperformed random or grid-based methods. On the EuRoC benchmark, greedy methods achieved a Relative Translational Error (RTE) of ~0.35m, compared to ~0.44m for random selection.
- Speed: The Linearized Greedy algorithm proved exceptionally fast, taking only ~7.75 ms per frame on the QCar, compared to ~82 ms for the standard greedy approach, with negligible loss in accuracy.
- Horizon Impact: Performance improved as the prediction horizon increased, saturating at approximately 1 second (23 frames) ahead.
Conclusion and Future Directions
This work demonstrates that non-submodular MSE objectives can be effectively optimized for real-time robot navigation. While the theoretical bounds are rigorous, they can be conservative; empirical results often exceed these guarantees. Future work will explore adaptive budgeting—adjusting the number of tracked features based on scene complexity—and extending the framework to multi-modal sensors like LiDAR.
Summary updated