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 % equals
fg = createGaussianFactorGraph(); fg = createGaussianFactorGraph();
fg2 = createGaussianFactorGraph(); fg2 = createGaussianFactorGraph();
CHECK('equals',fg.equals(fg,fg2)); CHECK('equals',fg.equals(fg2,1e-9));
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% error % error
@ -10,7 +10,6 @@ cfg = createZeroDelta();
actual = fg.error(cfg); actual = fg.error(cfg);
DOUBLES_EQUAL( 5.625, actual, 1e-9 ); DOUBLES_EQUAL( 5.625, actual, 1e-9 );
%-----------------------------------------------------------------------
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% eliminate_x1 % eliminate_x1
fg = createGaussianFactorGraph(); fg = createGaussianFactorGraph();
@ -23,31 +22,30 @@ actual = fg.eliminateOne('x2');
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% eliminateAll % eliminateAll
sigma1=.1; sigma1=[.1;.1];
R1 = eye(2); I = eye(2);
R1 = I;
d1=[-.1;-.1]; d1=[-.1;-.1];
cg1 = GaussianConditional('x1',d1, R1,sigma1); cg1 = GaussianConditional('x1',d1, R1,sigma1);
sigma2=0.149071; sigma2=[0.149071; 0.149071];
R2 = eye(2); R2 = I;
A1= -eye(2); A1= -I;
d2=[0; .2]; d2=[0; .2];
cg2 = GaussianConditional('l1',d2, R2, 'x1', A1,sigma2); cg2 = GaussianConditional('l1',d2, R2, 'x1', A1,sigma2);
sigma3=0.0894427; sigma3=[0.0894427; 0.0894427];
R3 = eye(2); R3 = I;
A21 = [ -.2, 0.0 A21 = -0.2*I;
0.0, -.2]; A22 = -0.8*I;
A22 = [-.8, 0.0
0.0, -.8];
d3 =[.2; -.14]; d3 =[.2; -.14];
cg3 = GaussianConditional('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3); cg3 = GaussianConditional('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
expected = GaussianBayesNet; expected = GaussianBayesNet;
expected.push_back(cg1);
expected.push_back(cg2);
expected.push_back(cg3); expected.push_back(cg3);
expected.print('expected'); expected.push_back(cg2);
expected.push_back(cg1);
% Check one ordering % Check one ordering
fg1 = createGaussianFactorGraph(); fg1 = createGaussianFactorGraph();
ord1 = Ordering; ord1 = Ordering;
@ -55,8 +53,7 @@ ord1.push_back('x2');
ord1.push_back('l1'); ord1.push_back('l1');
ord1.push_back('x1'); ord1.push_back('x1');
actual1 = fg1.eliminate_(ord1); actual1 = fg1.eliminate_(ord1);
actual1.print('actual'); CHECK('eliminateAll', actual1.equals(expected,1e-5));
%CHECK('eliminateAll', actual1.equals(expected));
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% matrix % matrix
@ -69,3 +66,19 @@ ord.push_back('x1');
A = fg.matrix(ord); 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));