Manifold Geometry with Fast Automatic Derivatives and Coordinate Frame Semantics Checking in C++

Computer vision and robotics problems often require representation and estimation of poses on the SE(3) manifold. Developers of algorithms that must run in real time face several time-consuming programming tasks, including deriving and computing analytic derivatives and avoiding mathematical errors when handling poses in multiple coordinate frames. To support rapid and error-free development, we present wave_geometry, a C++ manifold geometry library with two key contributions: expression template-based automatic differentiation and compile-time enforcement of coordinate frame semantics. We contrast the library with existing open source packages and show that it can evaluate Jacobians in forward and reverse mode with little to no runtime overhead compared to hand-coded derivatives. The library is available at https://github.com/wavelab/wave_geometry.

I. Introduction

Representation and estimation of poses is central to a broad range of computer vision and robotics problems. Most approaches rely on some form of optimization over possible poses, which requires operations over the differentiable manifold of SE(3) [1], and frequently leads to long chains of transformations between coordinates frames. This results in a persistent need for the computation of Jacobians for complex expressions of manifold elements, a process that is both time-consuming and error-prone.