AXLE: Computationally-efficient trajectory smoothing using factor graph chains

Abstract

Factor graph chains– the special case of a factor graph in which there are no potentials connecting non-adjacent nodes– arise naturally in many robotics problems. Importantly, they are often part of an inner loop in trajectory optimization and estimation problems, and so applications can be very sensitive to the performance of a solver.Of course, it is well-known that factor graph chains have an O(N) solution, but an actual solution is often left as "an exercise to the reader"… with the inevitable consequence that few (if any) efficient solutions are readily available.In this paper, we carefully derive the solution while keeping track of the specific block structure that arises, we work through a number of practical implementation challenges, and we highlight additional optimizations that are not at first apparent. An easy-to-use and self-contained solver is provided in C, which outperforms the AprilSAM general-purpose sparse matrix factorization library by a factor of 7.3x even without specialized block operations.The name AXLE reflects the names of the key matrices involved (the approach here solves the linear problem AX = E by factoring A as LLT), while also reflecting its key application in kino-dynamic trajectory estimation of vehicles with axles.