12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

eliminateAll unit test now works

release/4.3a0
Frank Dellaert 2009-12-12 04:11:42 +00:00
parent 58ed9ec367
commit 2bd9e890d3
1 changed files with 31 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
matlab/testGaussianFactorGraph.m
View File

@ -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));