From 2bd9e890d3ba3db68858f7aeaae3e442e2f02f62 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 12 Dec 2009 04:11:42 +0000
Subject: [PATCH] eliminateAll unit test now works

---
 matlab/testGaussianFactorGraph.m | 49 ++++++++++++++++++++------------
 1 file changed, 31 insertions(+), 18 deletions(-)

diff --git a/matlab/testGaussianFactorGraph.m b/matlab/testGaussianFactorGraph.m
index 7b96a876d..7bfe9e1d6 100644
--- a/matlab/testGaussianFactorGraph.m
+++ b/matlab/testGaussianFactorGraph.m
@@ -2,7 +2,7 @@
 % equals
 fg = createGaussianFactorGraph();
 fg2 = createGaussianFactorGraph();
-CHECK('equals',fg.equals(fg,fg2));
+CHECK('equals',fg.equals(fg2,1e-9));
 
 %-----------------------------------------------------------------------
 % error
@@ -10,7 +10,6 @@ cfg = createZeroDelta();
 actual = fg.error(cfg);
 DOUBLES_EQUAL( 5.625, actual, 1e-9 );
 
-%-----------------------------------------------------------------------
 %-----------------------------------------------------------------------
 % eliminate_x1
 fg = createGaussianFactorGraph();
@@ -23,31 +22,30 @@ actual = fg.eliminateOne('x2');
 
 %-----------------------------------------------------------------------
 % eliminateAll
-sigma1=.1;
-R1 = eye(2);
+sigma1=[.1;.1];
+I = eye(2);
+R1 = I;
 d1=[-.1;-.1];
 cg1 = GaussianConditional('x1',d1, R1,sigma1);
 
-sigma2=0.149071;
-R2 = eye(2);
-A1= -eye(2);
+sigma2=[0.149071; 0.149071];
+R2 = I;
+A1= -I;
 d2=[0; .2];
 cg2 = GaussianConditional('l1',d2, R2, 'x1', A1,sigma2);
 
-sigma3=0.0894427;
-R3 = eye(2);
-A21 = [ -.2, 0.0
-    0.0, -.2];
-A22 = [-.8, 0.0
-    0.0, -.8];
+sigma3=[0.0894427; 0.0894427];
+R3 = I;
+A21 = -0.2*I;
+A22 = -0.8*I;
 d3 =[.2; -.14];
 cg3 = GaussianConditional('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
 
 expected = GaussianBayesNet;
-expected.push_back(cg1);
-expected.push_back(cg2);
 expected.push_back(cg3);
-expected.print('expected');
+expected.push_back(cg2);
+expected.push_back(cg1);
+
 % Check one ordering
 fg1 = createGaussianFactorGraph();
 ord1 = Ordering;
@@ -55,8 +53,7 @@ ord1.push_back('x2');
 ord1.push_back('l1');
 ord1.push_back('x1');
 actual1 = fg1.eliminate_(ord1);
-actual1.print('actual');
-%CHECK('eliminateAll', actual1.equals(expected));
+CHECK('eliminateAll', actual1.equals(expected,1e-5));
 
 %-----------------------------------------------------------------------
 % matrix
@@ -69,3 +66,19 @@ ord.push_back('x1');
 
 A = fg.matrix(ord);
 
+%-----------------------------------------------------------------------
+% gradientDescent
+
+% Expected solution
+fg = createGaussianFactorGraph();
+expected = fg.optimize_(ord); % destructive
+
+% Do gradient descent
+% fg2 = createGaussianFactorGraph();
+% zero = createZeroDelta();
+% actual = fg2.gradientDescent(zero);
+% CHECK(assert_equal(expected,actual,1e-2));
+
+% Do conjugate gradient descent
+% actual2 = fg2.conjugateGradientDescent(zero);
+% CHECK(assert_equal(expected,actual2,1e-2));