Consensus-based Distributed 3D Pose Estimation with Noisy Relative Measurements

Abstract

In this paper we study consensus-based distributed estimation algorithms for estimating the global translation and rotation of each agent in a multi-agent system. We consider the case in which agents measure the noisy relative pose of their neighbors and communicate their estimates to agree upon the global poses in an arbitrary reference frame. The main contribution of this paper is a formal analysis that provides necessary and sufficient conditions to guarantee stability (in a Lyapunov sense) of the estimation system's equilibria. We prove that consensus-based algorithms will diverge, even with arbitrarily small inconsistencies on the relative pose, unless the measurements satisfy minimum consistency conditions. We determine these consistency conditions for translation-only, rotation-only, and combined 3D pose estimation using the axisangle rotation representation over undirected graphs. We then propose an initialization method based on these conditions that guarantees consistency and stability of the estimator's equilibria. Additionally, we show that existing distributed estimation methods in literature exploit these conditions to guarantee convergence of their algorithms. Lastly, we perform simulations that show convergence when consistency conditions hold and divergence when they do not.

Towards Collaborative Simultaneous Localization and Mapping: a Survey of the Current Research Landscape

“…We observe that more exciting new directions are still being discovered, considering that recent approaches such as (Tian et al, 2021) have been shown to outperform, both in accuracy and convergence rate, the well established Distributed Gauss-Seidel pose graph optimization method (Choudhary et al, 2017a) reused in many state-of-the-art C-SLAM systems such as (Cieslewski et al, 2018;Lajoie et al, 2020;Wang et al, 2019). Those promising approaches also include the majorization-minimization technique from (Fan and Murphey, 2020) and the consensus-based 3D pose estimation technique inspired by distributed formation control from (Cristofalo et al, 2019;Cristofalo et al, 2020).…”

GeoD: Consensus-based Geodesic Distributed Pose Graph Optimization

“…It is worth noting that the estimation scheme we propose is similar to other distributed consensus-based algorithms. In particular, it is inspired by previous 3D distributed pose estimation work [8] and by 3D formation control [27]. Note that there are three major differences.…”

“…Note that there are three major differences. First, with respect to [8], we use the rotation matrix parameterization of rotations instead of the angle-axis. This allows us to converge to a solution that is closer to the global minimum as we demonstrate in Section VI-A1.…”

“…This work builds upon an earlier conference version [8], which used the angle-axis rotation parameterization. The current paper presents a fundamentally new algorithm using rotation matrices, together with an entirely new mathematical analysis.…”

“…We also present new performance comparisons with two state-of-the-art pose graph optimization algorithms, and give results from new hardware experiments. We briefly compare GeoD with the earlier iteration from [8] in Section VI and show that GeoD is advantageous in terms of both convergence speed as well as solution accuracy by over a factor of 5×.…”