From 7a28e6d5cb5b3c523f2d5f0e70dbe76e280085fb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 20 May 2012 22:02:11 +0000
Subject: [PATCH] marginals

---
 examples/LocalizationExample.cpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp
index fdf522f73..6b9a48339 100644
--- a/examples/LocalizationExample.cpp
+++ b/examples/LocalizationExample.cpp
@@ -18,6 +18,13 @@
 // pull in the 2D PoseSLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/pose2SLAM.h>
 
+// include this for marginals
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+
+#include <iomanip>
+#include <iostream>
+
 using namespace std;
 using namespace gtsam;
 
@@ -58,6 +65,14 @@ int main(int argc, char** argv) {
 	pose2SLAM::Values result = graph.optimize(initialEstimate);
 	result.print("\nFinal result:\n  ");
 
+	// use an explicit Optimizer object so we can query the marginals
+	LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+	Marginals marginals(graph, result);
+	cout.precision(2);
+  cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
+  cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl;
+  cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl;
+
 	return 0;
 }