Robust Rigid Body Pose Estimation for Sparse Point Sets Under Heteroscedastic Noise

Abstract

Rigid body pose estimation from sparse point correspondences is important in robotics but remains challenging when measurements are affected by heteroscedastic noise and occasional outliers. This issue is particularly evident in marker-based tracking of fast-moving platforms, where localization uncertainty may vary substantially across markers due to motion blur, occlusion, and sensing artifacts. To address this setting, we develop a robust registration framework tailored to sparse and structured point sets by modeling residuals with a heavy-tailed Student's t-distribution. Using its Gaussian scale mixture representation, the estimation problem is formulated as a maximum-likelihood objective with latent correspondence reliabilities, and solved via an iteratively reweighted least-squares procedure with a graduated non-convexity schedule. The resulting method adaptively down-weights unreliable measurements while retaining the efficiency of weighted rigid alignment. Simulation results across multiple noise models and outlier ratios show that the proposed approach achieves consistently low rotation and translation errors in sparse regimes. Real-world experiments on high-speed quadrotor tracking further demonstrate improved geometric consistency and reduced trajectory jerk relative to standard least-squares estimation.