Added print, moved instance variables

gtsam/nonlinear/Marginals.cpp

@@ -43,6 +43,14 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
     bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
 }
 
+/* ************************************************************************* */
+void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const {
+	ordering_.print(str+"Ordering: ", keyFormatter);
+	graph_.print(str+"Graph: ");
+	values_.print(str+"Solution: ", keyFormatter);
+	bayesTree_.print(str+"Bayes Tree: ");
+}
+
 /* ************************************************************************* */
 Matrix Marginals::marginalCovariance(Key variable) const {
   return marginalInformation(variable).inverse();

gtsam/nonlinear/Marginals.h

@@ -40,6 +40,16 @@ public:
     QR
   };
 
+protected:
+
+  GaussianFactorGraph graph_;
+  Ordering ordering_;
+  Values values_;
+  Factorization factorization_;
+  GaussianBayesTree bayesTree_;
+
+public:
+
   /** Construct a marginals class.
    * @param graph The factor graph defining the full joint density on all variables.
    * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
@@ -47,6 +57,9 @@ public:
    */
   Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
 
+  /** print */
+  void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
   /** Compute the marginal covariance of a single variable */
   Matrix marginalCovariance(Key variable) const;
 
@@ -60,14 +73,6 @@ public:
 
   /** Compute the joint marginal information of several variables */
   JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
-
-protected:
-
-  GaussianFactorGraph graph_;
-  Values values_;
-  Ordering ordering_;
-  Factorization factorization_;
-  GaussianBayesTree bayesTree_;
 };
 
 /**
@@ -76,7 +81,17 @@ protected:
 class JointMarginal {
 
 protected:
-  typedef SymmetricBlockView<Matrix> BlockView;
+
+	typedef SymmetricBlockView<Matrix> BlockView;
+
+  Matrix fullMatrix_;
+  BlockView blockView_;
+  Ordering indices_;
+
+  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
+    fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
+
+  friend class Marginals;
 
 public:
   /** A block view of the joint marginal - this stores a reference to the
@@ -101,16 +116,6 @@ public:
    */
   Block operator()(Key iVariable, Key jVariable) const {
     return blockView_(indices_[iVariable], indices_[jVariable]); }
-
-protected:
-  Matrix fullMatrix_;
-  BlockView blockView_;
-  Ordering indices_;
-
-  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
-    fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
-
-  friend class Marginals;
 };
 
 } /* namespace gtsam */