Added marginals function (could move to superclass?)

gtsam/slam/pose2SLAM.h

@@ -23,6 +23,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose2.h>
 
 // Use pose2SLAM namespace for specific SLAM instance
@@ -100,6 +101,12 @@ namespace pose2SLAM {
 
     /// Optimize
     Values optimize(const Values& initialEstimate) const;
+
+    /// Return a Marginals object
+    Marginals marginals(const Values& solution) const {
+    	return Marginals(*this,solution);
+    }
+
   };
 
 } // pose2SLAM