rhs method

cpp/GaussianFactorGraph.cpp

@@ -182,6 +182,14 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
 	return result;
 }
 
+/* ************************************************************************* */
+Errors GaussianFactorGraph::rhs() const {
+	Errors e;
+	BOOST_FOREACH(sharedFactor factor,factors_)
+		e.push_back(ediv(factor->get_b(),factor->get_sigmas()));
+	return e;
+}
+
 /* ************************************************************************* */  
 pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const {
 

cpp/GaussianFactorGraph.h

@@ -156,6 +156,11 @@ namespace gtsam {
      */
     GaussianFactorGraph add_priors(double sigma) const;
 
+    /**
+     * Return RHS (b./sigmas) as Errors class
+     */
+    Errors rhs() const;
+
     /**
      * Return (dense) matrix associated with factor graph
      * @param ordering of variables needed for matrix column order

cpp/testGaussianFactorGraph.cpp

@@ -606,6 +606,18 @@ TEST( GaussianFactorGraph, transposeMultiplication )
 	CHECK(assert_equal(expected,actual));
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, rhs )
+{
+	GaussianFactorGraph Ab = createGaussianFactorGraph();
+	Errors expected, actual = Ab.rhs();
+  expected.push_back(Vector_(2,-1.0,-1.0));
+  expected.push_back(Vector_(2, 2.0,-1.0));
+  expected.push_back(Vector_(2, 0.0, 1.0));
+  expected.push_back(Vector_(2,-1.0, 1.5));
+	CHECK(assert_equal(expected,actual));
+}
+
 /* ************************************************************************* */
 // Tests ported from ConstrainedGaussianFactorGraph
 /* ************************************************************************* */