Matlab Factor Graph Optimization