Estimating Rigid Transformation With Correlated Observations

The problem of finding the rotation and the translation between two sets of corresponded points is known as the rigid transformation estimation problem. It plays a crucial role in many robotic applications such as “simultaneous localization and mapping” (SLAM), surface reconstruction, and inertial sensor calibration. The most widely used solution to this problem is based on performing the singular value decomposition (SVD) over a derived data matrix. A drawback of the SVD method is that it is a least-squares method and thus may fail to take into account the anisotropic and/or correlated noises, which often present in practical applications. A natural variation is to add a matrix weight to the least-squares problem to balance the estimation errors in different measurement directions. However, it becomes difficult to write down a closed form solution in this setup. In this paper, an efficient algorithm is presented to estimate the rigid transformation with correlated observations. The effectiveness of the proposed method is experimentally demonstrated on two robotic applications, namely the point set registration and the inertial sensor localization.