On the Manifold: Representing Geometry in C++ for State Estimation