Feature-Driven Field-of-View Overlap Assurance with Control Barrier Functions for Cooperative Visual Localization

Transcript

1. Feature-Driven Field-of-View Overlap Assurance with Control Barrier Functions for Cooperative Visual Localization Tomoki Arita and Toru Namerikawa Department of System Design Engineering, Keio University SICE Fes 2025 Chiang Mai, 9th - 12th Sep. 2025
2. Motivation equinor.com Collaborative visual inertial sensor systems (CoVINS) are essential, especially under limited sensor resources e.g. sensing tasks such as mapping are conducted using a group of tiny agents equipped only with monocular cameras [Zhang et al., ImageProcessing2022] A single monocular camera agent struggles to perform real-time mapping ⇒ Cooperation among multiple agents (e.g. collaborative active perception) is necessary to achieve effective mapping bitcraze.io [Giernacki et al., MMAR2017] 2 T. Zhang, L. Zhang, Y. Chen and Y. Zhou, "CVIDS: A Collaborative Localization and Dense Mapping Framework for Multi-Agent Based Visual-Inertial SLAM," in IEEE Transactions on Image Processing.
3. CoVINS: Collaborative Visual Inertial System Agent 1 states Agent 2 states IMU Measurements Visual Measurements Features 3
4. Agent 1 states Agent 2 states IMU Measurements Visual Measurements Features Reconstructable 4 CoVINS: Collaborative Visual Inertial System
5. Reconstructable 5 CoVINS: Collaborative Visual Inertial System ➢ Formulated Field-of-View Overlap Control Problem for CoVINS ➢ Solve problem by CBF with Probabilistic Safe Sets ➢ Validated the Distributed Algorithm through simulations
6. Modeling Field-of-View Kinematics in SE(3) state velocity ：Feature point set observable by agent i 6
7. Control Barrier Functions(CBFs) In dynamics of If below constraint is satisfied, Safety Set is Forward Invariant. A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, 2017. 7
8. Probabilistic CBFs Probability agent i, j can observe 𝒎 shared feature point at least Cross section of FoV Agent i Agent j Probability agent i, j can observe 𝑞𝑙 8 m shared point Theorem 1 When the safety constraint is satisfied, at least m common feature points are always observable. That is, holds at all times.
9. Probabilistic CBFs Exact probability Not differentiable Approximated probability differentiable Approximated probability for localization utility differentiable Cross section of FoV Probable 9
10. Probabilistic CBFs Standard QP with CBF Gradient in Riemannian manifold 10
11. Distributed CBFs by PDMM [Heusdens et al., Signal Processing2024] Centralized CBFs Agent 1 Agent 2 Solve QP Consensus Distributed CBFs Cannot compute by each agent ! 11 R. Heusdens and G. Zhang, “Distributed Optimisation With Linear Equality and Inequality Constraints Using PDMM,” IEEE Transactions on Signal and Information Processing over Networks, Vol. 10, pp. 294-306, 2024.
12. Experimental Result Target Tracking by CBF 12 Safety constraint becomes active (constraint margin = 0; blue line) when the target is about to exit the Field of View (FoV) Feature Points Tracking by PCBF A sufficient number of feature points remain within the FoV at all times (colored points)
13. CoFoV by Centralized CBF Experimental Result 13 A subset of feature points (green) remains visible from both agents simultaneously CoFoV by Distributed CBF The sum of safety terms for 2 agents consistently satisfies the constraint (>0; green line)
14. Summary Future work • Formulated Field-of-View Overlap Control for cooperative multi-agent SLAM on SE(3) • Enabled controllability of the VINS problem using probabilistic safe sets in Control Barrier Functions (CBF) • Validated the distributed control approach through simulations • Development of scalable active perception methods for more than two agents • Validation of the SLAM integrated system 14
15. Reference [1] T. Zhang, L. Zhang, Y. Chen and Y. Zhou, "CVIDS: A Collaborative Localization and Dense Mapping Framework for Multi-Agent Based Visual-Inertial SLAM," in IEEE Transactions on Image Processing, vol. 31, pp. 6562-6576, 2022. [2] W. Giernacki, M. Skwierczyński, W. Witwicki, P. Wroński, and P. Kozierski, “Crazyflie 2.0 quadrotor as a platform for research and education in robotics and control engineering,” in 2017 22nd International Conference on Methods and Models in Automation and Robotics(MMAR), pp. 37–42, IEEE, 2017. [3] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, Vol. 62, No. 8, pp. 3861-3876, 2017. [4] R. Heusdens and G. Zhang, “Distributed OptimisationWith Linear Equality and Inequality Constraints Using PDMM,” IEEE Transactions on Signal and Information Processing over Networks, Vol. 10, pp. 294-306, 2024. 15