Simultaneous Robot and Feature Localization Via Angle-Based Sensing With Barycentric Coordinate Representation

This paper studies the Simultaneous Robot and Feature Localization (SRAFL) problem leveraging angle-based sensing from a monocular camera, and addresses it from a networked signal processing perspective. By modeling the robot-feature interaction as a sensing graph, in which the robot and environmental features function as nodes connected through bearing angle measurements, we reformulate the inherently nonlinear SRAFL problem into a dynamic network localization task. Based on this formulation, we propose a Barycentric Linear method, termed BL-SRAFL, which exploits barycentric coordinate representations to obtain a linear localization framework for jointly estimating the robot trajectory and the Euclidean positions of environmental features. The proposed method admits direct and efficient iterative solutions with linear computational and memory complexity, making it particularly suitable for resource-constrained applications. Rigorous theoretical analysis is provided to characterize the solvability and establish the convergence of the proposed method. For scenarios requiring enhanced accuracy, we further introduce the BL-SRAFL+BA pipeline, which integrates an optional Bundle Adjustment (BA) refinement step. Numerical simulations and real-world experiments demonstrate the robustness and effectiveness of the proposed methods.