From 61d3d30f3b3d46bd446ae0274dce638ef2c55611 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Fri, 11 Feb 2011 22:23:42 +0000
Subject: [PATCH] Added a marginalCovariance function directly to NonlinearISAM

---
 gtsam/nonlinear/NonlinearISAM-inl.h | 8 ++++++++
 gtsam/nonlinear/NonlinearISAM.h     | 3 +++
 2 files changed, 11 insertions(+)

diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h
index 7a26b0076..4f9dafacc 100644
--- a/gtsam/nonlinear/NonlinearISAM-inl.h
+++ b/gtsam/nonlinear/NonlinearISAM-inl.h
@@ -91,3 +91,11 @@ Values NonlinearISAM<Values>::estimate() {
   else
     return linPoint_;
 }
+
+/* ************************************************************************* */
+template<class Values>
+Matrix NonlinearISAM<Values>::marginalCovariance(const Symbol& key) {
+	Matrix covariance; Vector mean;
+	boost::tie(mean, covariance) = isam_.marginal(ordering_[key]);
+	return covariance;
+}
diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h
index 8c6dc6bbd..2b7a8a8e4 100644
--- a/gtsam/nonlinear/NonlinearISAM.h
+++ b/gtsam/nonlinear/NonlinearISAM.h
@@ -77,6 +77,9 @@ public:
 	/** replace the current ordering */
 	void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
 
+	/** find the marginal covariance for a single variable */
+	Matrix marginalCovariance(const Symbol& key);
+
 	// access
 
 	/** access the underlying bayes tree */