Wrapped JointMarginal class and joint marginal functions of Marginals

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

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;
 };
 
 //*************************************************************************

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;

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 */

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 */