The Importance of Coordinate Frames in Dynamic SLAM

TL;DR

A front-end agnostic framework using GTSAM that can be used to evaluate various Dynamic SLAM formulations is introduced, introducing a front-end agnostic framework using GTSAM that can be used to evaluate various Dynamic SLAM formulations.

Abstract

Most Simultaneous localisation and mapping (SLAM) systems have traditionally assumed a static world, which does not align with real-world scenarios. To enable robots to safely navigate and plan in dynamic environments, it is essential to employ representations capable of handling moving objects. Dynamic SLAM is an emerging field in SLAM research as it improves the overall system accuracy while providing additional estimation of object motions. State-of-the-art literature informs two main formulations for Dynamic SLAM, representing dynamic object points in either the world or object coordinate frame. While expressing object points in a local reference frame may seem intuitive, it may not necessarily lead to the most accurate and robust solutions. This paper conducts and presents a thorough analysis of various Dynamic SLAM formulations, identifying the best approach to address the problem. To this end, we introduce a front-end agnostic framework using GTSAM that can be used to evaluate various Dynamic SLAM formulations.

Figures (4)

• Figure 1: Object-centric vs world-centric. A comparison of two Dynamic SLAM formulations viewing the same scene from time-step $k-1$ to $k$. Three reference frames are included, namely world $\{W\}$, object $\{L\}$ and camera $\{X\}$. (a) The object-centric formulation expresses dynamic points $\prescript{L}{}{\mathbf{m}}_{}$ in the local object frame $\{L\}$ defined by an estimate for object pose $\prescript{W}{}{\mathbf{L}}_{}$ at each time-step. (b) The world-centric representation describes the rigid body motion $\prescript{W}{k-1}{\mathbf{H}}_{k}$ directly with dynamic points $\prescript{W}{}{\mathbf{m}}_{}$.
• Figure 2: World-centric formulation factor graph. Example factor graph including three static points $\prescript{W}{}{\mathbf{m}}_{}^{i:i+2}$ and one dynamic point $\prescript{W}{}{\mathbf{m}}_{}^{i+3}$ on object $j$ seen at three consecutive time-steps $k-2:k$. Point measurement factors are shown as white squares, odometry factors as orange squares and world-centric motion factors as magenta squares. The motion smoothing factor is shown in blue and the prior factor is in black.
• Figure 3: Object-centric formulation factor graph. Example factor graphs showing three static points $\prescript{W}{}{\mathbf{m}}_{}^{i:i+2}$ and a tracked point $\prescript{L^j}{}{\mathbf{m}}_{}^{i+3}$ on a dynamic object $\prescript{}{}{\mathbf{L}}_{}^j$, seen at consecutive time-steps $k-2:k$. Dynamic point measurement factors are shown as gray squares, object-centric motion factors are magenta squares and green squares represent object kinematic factors. The motion smoothing factor is blue and priors factors are black. (b) and (c) show variations of the graph in (a).
• Figure 4: Chi-squared errors $\chi_{n}$ for world and object-centric formulations per-step during optimisation for sequences 04 and 05. The Error Change is computed as $\chi_{n-1}^2 - \chi_{n}^2$ where $n$ represents the step. The linear system is solved at every step but only re-linearised every iteration, which is marked in plots (a) and (c).