added method to get covariance to optimizer

examples/Pose2SLAMExample_advanced.cpp

@@ -24,6 +24,9 @@
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/nonlinear/NonlinearOptimization-inl.h>
 
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/Matrix.h>
+
 using namespace std;
 using namespace gtsam;
 using namespace boost;
@@ -52,7 +55,7 @@ int main(int argc, char** argv) {
 	graph->addConstraint(x2, x3, odom_measurement, odom_model);
 	graph->print("full graph");
 
-  /* 3. Create the data structure to hold the initial estinmate to the solution
+  /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
 	shared_ptr<Values> initial(new Values);
 	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
@@ -72,6 +75,13 @@ int main(int argc, char** argv) {
 
 	Values result = *optimizer_result.values();
 	result.print("final result");
+
+	Vector mean; Matrix covariance;
+	boost::tie( mean, covariance)  = optimizer_result.marginalStandard(x1);
+
+	print(mean, "Mean");
+	print(covariance, "Covariance");
+
 	return 0;
 }
 

linear/GaussianMultifrontalSolver.cpp

@@ -55,4 +55,11 @@ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const {
   return Base::marginal(j);
 }
 
+/* ************************************************************************* */
+std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalStandard(Index j) const {
+	GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
+	Matrix R = conditional->get_R();
+	return make_pair(conditional->get_d(), inverse(trans(R)*R));
+}
+
 }

linear/GaussianMultifrontalSolver.h

@@ -101,7 +101,7 @@ public:
    * returns a GaussianConditional, this function back-substitutes the R factor
    * to obtain the mean, then computes \Sigma = (R^T * R)^-1.
    */
-//  std::pair<Vector, Matrix> marginalStandard(Index j) const;
+   std::pair<Vector, Matrix> marginalStandard(Index j) const;
 
 };
 

linear/GaussianSequentialSolver.cpp

@@ -72,6 +72,13 @@ GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const {
   return Base::marginal(j);
 }
 
+std::pair<Vector, Matrix> GaussianSequentialSolver::marginalStandard(Index j) const {
+	GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
+	Matrix R = conditional->get_R();
+	return make_pair(conditional->get_d(), inverse(trans(R)*R));
+}
+
+
 /* ************************************************************************* */
 GaussianFactorGraph::shared_ptr GaussianSequentialSolver::joint(const std::vector<Index>& js) const {
   return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::joint(js)));

linear/GaussianSequentialSolver.h

@@ -22,6 +22,7 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/GaussianConditional.h>
 
 #include <utility>
 #include <vector>
@@ -107,7 +108,7 @@ public:
    * returns a GaussianConditional, this function back-substitutes the R factor
    * to obtain the mean, then computes \Sigma = (R^T * R)^-1.
    */
-//  std::pair<Vector, Matrix> marginalStandard(Index j) const;
+  std::pair<Vector, Matrix> marginalStandard(Index j) const;
 
   /**
    * Compute the marginal joint over a set of variables, by integrating out

nonlinear/NonlinearOptimizer.h

@@ -158,6 +158,13 @@ namespace gtsam {
 			return values_;
 		}
 
+		/**
+		 * Return mean and covariance on a single variable
+		 */
+		std::pair<Vector, Matrix> marginalStandard(Symbol j) const {
+			return solver_->marginalStandard((*ordering_)[j]);
+		}
+
 		/**
 		 *  linearize and optimize
 		 *  This returns an VectorValues, i.e., vectors in tangent space of T