GaussianBayesNet::matrix now properly divides in sigmas

cpp/GaussianBayesNet.cpp

@@ -156,17 +156,20 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn)  {
     // find corresponding conditional
     GaussianConditional::shared_ptr cg = bn[key];
     
+    // get sigmas
+    Vector sigmas = cg->get_sigmas();
+
     // get RHS and copy to d
     const Vector& d_ = cg->get_d();
     const size_t n = d_.size();
     for (size_t i=0;i<n;i++)
-      d(I+i) = d_(i);
+      d(I+i) = d_(i)/sigmas(i);
 
     // get leading R matrix and copy to R
     const Matrix& R_ = cg->get_R();
     for (size_t i=0;i<n;i++)
       for(size_t j=0;j<n;j++)
-      	R(I+i,I+j) = R_(i,j);
+      	R(I+i,I+j) = R_(i,j)/sigmas(i);
 
     // loop over S matrices and copy them into R
     GaussianConditional::const_iterator keyS = cg->parentsBegin();
@@ -176,7 +179,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn)  {
       const size_t J = mapping[keyS->first];     // find column index
       for (size_t i=0;i<m;i++)
       	for(size_t j=0;j<n;j++)
-      		R(I+i,J+j) = S(i,j);
+      		R(I+i,J+j) = S(i,j)/sigmas(i);
     } // keyS
 
   } // keyI

cpp/testGaussianFactorGraph.cpp

@@ -618,6 +618,36 @@ TEST( GaussianFactorGraph, rhs )
 	CHECK(assert_equal(expected,actual));
 }
 
+/* ************************************************************************* */
+// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
+TEST( GaussianFactorGraph, elimination )
+{
+	// Create Gaussian Factor Graph
+	GaussianFactorGraph fg;
+	Matrix Ap = eye(1), An = eye(1) * -1;
+	Vector b = Vector_(1, 0.0);
+	double sigma = 1.0/0.5;
+	fg.add("x1", An, "x2", Ap, b, sigma);
+	fg.add("x1", Ap, b, sigma);
+	fg.add("x2", Ap, b, sigma);
+
+	// Eliminate
+	Ordering ord;
+	ord += "x1", "x2";
+	GaussianBayesNet bayesNet = fg.eliminate(ord);
+
+	// Check sigma
+	DOUBLES_EQUAL(1.0/0.612372,bayesNet["x2"]->get_sigmas()(0),1e-5);
+
+	// Check matrix
+	Matrix R;Vector d;
+	boost::tie(R,d) = matrix(bayesNet);
+	Matrix expected = Matrix_(2,2,
+			0.707107,	-0.353553,
+      0.0,	 0.612372);
+	CHECK(assert_equal(expected,R,1e-6));
+}
+
  /* ************************************************************************* */
 // Tests ported from ConstrainedGaussianFactorGraph
 /* ************************************************************************* */