Support for Landmarks as Vertices in g2o format

[varunagrawal]

Feature

Currently, GTSAM saves only camera poses as vertices in the g2o format using writeG2o and readG2o. We would like GTSAM to also save landmark points as vertices using the same machinery so we can save and retrieve 3D landmark points.

The format for landmarks as per the g2o repo is

VERTEX_TRACKXYZ i x y z

This specifies a 3D point x_i = (x, y, z)^T which is located at (x, y, z)

Motivation

This feature would allow us to read arbitrary g2o files and save/load both poses and landmarks.

Pitch

Read and save both poses and points.

Alternatives

N/A

Additional context

https://github.com/RainerKuemmerle/g2o/wiki/File-format-slam-3d