Building Graph

But not yet optimizing

Error using gtsam_wrapper
Exception from gtsam:
Attempting to access a key from an ordering that does not contain that key:O6


Error in gtsam.LevenbergMarquardtOptimizer (line 32)
        [ my_ptr, base_ptr ] = gtsam_wrapper(1195, varargin{1}, varargin{2});

Error in graph_to_gtsam (line 80)
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);