From c8daa4234868250f2fbfd151e342659070f0ab12 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 21 Jul 2012 23:54:55 +0000
Subject: [PATCH] Wrapped JointMarginal class and joint marginal functions of
 Marginals

---
 CMakeLists.txt                |  2 +-
 gtsam.h                       |  8 ++++++++
 gtsam/base/FastVector.h       |  5 +++++
 gtsam/nonlinear/Marginals.cpp | 14 ++++++++++++++
 gtsam/nonlinear/Marginals.h   |  7 +++++++
 5 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 13d564391..66fc43c50 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -108,7 +108,7 @@ endif()
 if(CYGWIN OR MSVC OR WIN32)
   set(Boost_USE_STATIC_LIBS 1)
 endif()
-find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time REQUIRED)
+find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time regex REQUIRED)
 set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY})
 
 # General build settings
diff --git a/gtsam.h b/gtsam.h
index 276a272a6..4a2cbb66c 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1129,6 +1129,14 @@ class Marginals {
 	void print(string s) const;
 	Matrix marginalCovariance(size_t variable) const;
 	Matrix marginalInformation(size_t variable) const;
+	gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const;
+	gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const;
+};
+
+class JointMarginal {
+	Matrix at(size_t iVariable, size_t jVariable) const;
+	void print(string s) const;
+	void print() const;
 };
 
 //*************************************************************************
diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h
index c3b8ca761..f55d67b41 100644
--- a/gtsam/base/FastVector.h
+++ b/gtsam/base/FastVector.h
@@ -88,6 +88,11 @@ public:
       Base::assign(x.begin(), x.end());
   }
 
+	/** Conversion to a standard STL container */
+	operator std::vector<VALUE>() const {
+		return std::vector<VALUE>(begin(), end());
+	}
+
 private:
   /** Serialization function */
   friend class boost::serialization::access;
diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp
index da3632570..f7f1aa00f 100644
--- a/gtsam/nonlinear/Marginals.cpp
+++ b/gtsam/nonlinear/Marginals.cpp
@@ -156,4 +156,18 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
   }
 }
 
+/* ************************************************************************* */
+void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
+	cout << s << "Joint marginal on keys ";
+	bool first = true;
+	BOOST_FOREACH(const Ordering::value_type& key_index, indices_) {
+		if(!first)
+			cout << ", ";
+		else
+			first = false;
+		cout << formatter(key_index.first);
+	}
+	cout << ".  Use 'at' or 'operator()' to query matrix blocks." << endl;
+}
+
 } /* namespace gtsam */
diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h
index f04fe310a..c792167c5 100644
--- a/gtsam/nonlinear/Marginals.h
+++ b/gtsam/nonlinear/Marginals.h
@@ -117,11 +117,18 @@ public:
   Block operator()(Key iVariable, Key jVariable) const {
     return blockView_(indices_[iVariable], indices_[jVariable]); }
 
+	/** Synonym for operator() */
+	Block at(Key iVariable, Key jVariable) const {
+		return (*this)(iVariable, jVariable); }
+
   /** Copy constructor */
   JointMarginal(const JointMarginal& other);
 
   /** Assignment operator */
   JointMarginal& operator=(const JointMarginal& rhs);
+
+	/** Print */
+	void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
 };
 
 } /* namespace gtsam */