SCORE: Saturated Consensus Relocalization in Semantic Line Maps

This is the arxiv version for our paper submitted to IEEE/RSJ IROS 2025. We propose a scene-agnostic and light-weight visual relocalization framework that leverages semantically labeled 3D lines as a compact map representation. In our framework, the robot localizes itself by capturing a single image, extracting 2D lines, associating them with semantically similar 3D lines in the map, and solving a robust perspective-n-line problem. To address the extremely high outlier ratios~(exceeding 99.5\%) caused by one-to-many ambiguities in semantic matching, we introduce the Saturated Consensus Maximization~(Sat-CM) formulation, which enables accurate pose estimation when the classic Consensus Maximization framework fails. We further propose a fast global solver to the formulated Sat-CM problems, leveraging rigorous interval analysis results to ensure both accuracy and computational efficiency. Additionally, we develop a pipeline for constructing semantic 3D line maps using posed depth images. To validate the effectiveness of our framework, which integrates our innovations in robust estimation and practical engineering insights, we conduct extensive experiments on the ScanNet++ dataset.

Efficient Planar Pose Estimation via UWB Measurements

State estimation is an essential part of autonomous systems. Integrating the Ultra-Wideband(UWB) technique has been shown to correct the long-term estimation drift and bypass the complexity of loop closure detection. However, few works on robotics adopt UWB as a stand-alone state estimation solution. The primary purpose of this work is to investigate planar pose estimation using only UWB range measurements and study the estimator's statistical efficiency. We prove the excellent property of a two-step scheme, which says that we can refine a consistent estimator to be asymptotically efficient by one step of Gauss-Newton iteration. Grounded on this result, we design the GN-ULS estimator and evaluate it through simulations and collected datasets. GN-ULS attains millimeter and sub-degree level accuracy on our static datasets and attains centimeter and degree level accuracy on our dynamic datasets, presenting the possibility of using only UWB for real-time state estimation.

Closed-form Error Propagation on the SE_n Group for Invariant Extended Kalman Filtering with Applications to VINS

Pose estimation is important for robotic perception, path planning, etc. Robot poses can be modeled on matrix Lie groups and are usually estimated via filter-based methods. In this paper, we establish the closed-form formula for the error propagation for the Invariant extended Kalman filter (IEKF) in the presence of random noises and apply it to vision-aided inertial navigation. We evaluate our algorithm via numerical simulations and experiments on the OPENVINS platform. Both simulations and the experiments performed on the public EuRoC MAV datasets demonstrate that our algorithm outperforms some state-of-art filter-based methods such as the quaternion-based EKF, first estimates Jacobian EKF, etc.