Fast and accurate PoseSLAM by combining relative and global state spaces

We revisit the question of state space in the context of performing loop closure. Although a relative state space has been previously discounted, we show that such a state space is actually extremely powerful, able to achieve recognizable results after just one iteration. The power behind the technique (called POReSS) is the coupling between parameters that causes the orientation of one node to affect the position and orientation of other nodes. At the same time, the approach is fast because, like the more popular incremental state space, the Jacobian never needs to be explicitly computed. Furthermore, we show that while POReSS is able to quickly compute a solution near the global optimum, it is not precise enough to perform the fine adjustments necessary to reach the global minimum. As a result, we augment POReSS with a fast variant of Gauss-Seidel (called Graph-Seidel) on a global state space to allow the solution to settle closer to the global minimum. We show that this combination of POReSS and Graph-Seidel converges more quickly and scales to very large graphs better than other techniques while at the same time computing a competitive residual.