From ff3edc6823a4ab8954ef660e993caf5fe123f552 Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Tue, 15 May 2012 00:15:11 +0000
Subject: [PATCH] Updated examples to use the new Marginals interface

---
 examples/PlanarSLAMSelfContained_advanced.cpp | 17 +++++++----------
 examples/Pose2SLAMExample_advanced.cpp        | 14 ++++++++------
 2 files changed, 15 insertions(+), 16 deletions(-)

diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp
index a7a45b726..bd5c5295a 100644
--- a/examples/PlanarSLAMSelfContained_advanced.cpp
+++ b/examples/PlanarSLAMSelfContained_advanced.cpp
@@ -36,8 +36,7 @@
 // implementations for structures - needed if self-contained, and these should be included last
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/linear/GaussianSequentialSolver.h>
-#include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/nonlinear/Marginals.h>
 
 using namespace std;
 using namespace gtsam;
@@ -125,15 +124,13 @@ int main(int argc, char** argv) {
 	Values resultMultifrontal = optimizer.optimize();
 	resultMultifrontal.print("final result (solved with a multifrontal solver)");
 
-	const Ordering& ordering = *optimizer.params().ordering;
-	GaussianMultifrontalSolver linearSolver(*graph.linearize(resultMultifrontal, ordering));
-
   // Print marginals covariances for all variables
-  print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance");
-  print(linearSolver.marginalCovariance(ordering[x2]), "x2 covariance");
-  print(linearSolver.marginalCovariance(ordering[x3]), "x3 covariance");
-  print(linearSolver.marginalCovariance(ordering[l1]), "l1 covariance");
-  print(linearSolver.marginalCovariance(ordering[l2]), "l2 covariance");
+	Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY);
+  print(marginals.marginalCovariance(x1), "x1 covariance");
+  print(marginals.marginalCovariance(x2), "x2 covariance");
+  print(marginals.marginalCovariance(x3), "x3 covariance");
+  print(marginals.marginalCovariance(l1), "l1 covariance");
+  print(marginals.marginalCovariance(l2), "l2 covariance");
 
 	return 0;
 }
diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp
index 282287827..29d8c58cf 100644
--- a/examples/Pose2SLAMExample_advanced.cpp
+++ b/examples/Pose2SLAMExample_advanced.cpp
@@ -23,6 +23,7 @@
 // pull in the Pose2 SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
 
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
@@ -74,12 +75,13 @@ int main(int argc, char** argv) {
 	pose2SLAM::Values result = optimizer.optimize();
 	result.print("final result");
 
-//	/* Get covariances */
-//	Matrix covariance1  = optimizer_result.marginalCovariance(PoseKey(1));
-//	Matrix covariance2  = optimizer_result.marginalCovariance(PoseKey(2));
-//
-//	print(covariance1, "Covariance1");
-//	print(covariance2, "Covariance2");
+	/* Get covariances */
+	Marginals marginals(graph, result, Marginals::CHOLESKY);
+	Matrix covariance1  = marginals.marginalCovariance(PoseKey(1));
+	Matrix covariance2  = marginals.marginalCovariance(PoseKey(2));
+
+	print(covariance1, "Covariance1");
+	print(covariance2, "Covariance2");
 
 	return 0;
 }