righ-hand-side

cpp/GaussianBayesNet.cpp

@@ -187,6 +187,22 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn)  {
   return make_pair(R,d);
 }
 
+/* ************************************************************************* */
+VectorConfig rhs(const GaussianBayesNet& bn) {
+	VectorConfig result;
+  BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
+  	const Symbol& key = cg->key();
+  	// get sigmas
+    Vector sigmas = cg->get_sigmas();
+
+    // get RHS and copy to d
+    const Vector& d = cg->get_d();
+    result.insert(key,ediv_(d,sigmas)); // TODO ediv_? I think not
+  }
+
+  return result;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

cpp/GaussianBayesNet.h

@@ -74,4 +74,10 @@ namespace gtsam {
 	 */
 	std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);
 
+  /**
+   * Return RHS d as a VectorConfig
+   * Such that backSubstitute(bn,d) = optimize(bn)
+   */
+  VectorConfig rhs(const GaussianBayesNet&);
+
 } /// namespace gtsam

cpp/testGaussianBayesNet.cpp

@@ -136,6 +136,43 @@ TEST( GaussianBayesNet, backSubstitute )
   CHECK(assert_equal(x,y));
 }
 
+/* ************************************************************************* */
+TEST( GaussianBayesNet, rhs )
+{
+	// y=R*x, x=inv(R)*y
+	// 2 = 1 1  -1
+	// 3     1   3
+  GaussianBayesNet cbn = createSmallGaussianBayesNet();
+	VectorConfig expected = gtsam::optimize(cbn);
+	VectorConfig d = rhs(cbn);
+	VectorConfig actual = backSubstitute(cbn, d);
+	CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST( GaussianBayesNet, rhs_with_sigmas )
+{
+	Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
+	Matrix R22 = Matrix_(1, 1, 1.0);
+	Vector d1(1), d2(1);
+	d1(0) = 9;
+	d2(0) = 5;
+	Vector tau(1);
+	tau(0) = 0.25;
+
+	// define nodes and specify in reverse topological sort (i.e. parents last)
+	GaussianConditional::shared_ptr Px_y(new GaussianConditional("x", d1, R11,
+			"y", S12, tau)), Py(new GaussianConditional("y", d2, R22, tau));
+	GaussianBayesNet cbn;
+	cbn.push_back(Px_y);
+	cbn.push_back(Py);
+
+	VectorConfig expected = gtsam::optimize(cbn);
+	VectorConfig d = rhs(cbn);
+	VectorConfig actual = backSubstitute(cbn, d);
+	CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST( GaussianBayesNet, backSubstituteTranspose )
 {