From 1ae6bb40303b35e18a56f09aebf2518cb9a7fde6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 12 Nov 2009 06:09:03 +0000 Subject: [PATCH] Added push_fron convenience method to add ConditionalGaussians into a Bayes net with much less clutter. Modernized some very old tests in the process. --- cpp/GaussianBayesNet.cpp | 14 +++++ cpp/GaussianBayesNet.h | 14 +++++ cpp/testBayesTree.cpp | 23 ++++---- cpp/testLinearFactorGraph.cpp | 99 ++++++----------------------------- 4 files changed, 53 insertions(+), 97 deletions(-) diff --git a/cpp/GaussianBayesNet.cpp b/cpp/GaussianBayesNet.cpp index fad6751c2..fd1e24d43 100644 --- a/cpp/GaussianBayesNet.cpp +++ b/cpp/GaussianBayesNet.cpp @@ -43,6 +43,20 @@ GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigm return bn; } +/* ************************************************************************* */ +void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, + const string& name1, Matrix S, Vector sigmas) { + ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, sigmas)); + bn.push_front(cg); +} + +/* ************************************************************************* */ +void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, + const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) { + ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, name2, T, sigmas)); + bn.push_front(cg); +} + /* ************************************************************************* */ VectorConfig optimize(const GaussianBayesNet& bn) { diff --git a/cpp/GaussianBayesNet.h b/cpp/GaussianBayesNet.h index 1a1226e40..e013fe64c 100644 --- a/cpp/GaussianBayesNet.h +++ b/cpp/GaussianBayesNet.h @@ -25,6 +25,20 @@ namespace gtsam { /** Create a simple Gaussian on a single multivariate variable */ GaussianBayesNet simpleGaussian(const std::string& key, const Vector& mu, double sigma=1.0); + /** + * Add a conditional node with one parent + * |Rx+Sy-d| + */ + void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R, + const std::string& name1, Matrix S, Vector sigmas); + + /** + * Add a conditional node with two parents + * |Rx+Sy+Tz-d| + */ + void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R, + const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas); + /** * optimize, i.e. return x = inv(R)*d */ diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index ff70d5c89..d5c6bc3b7 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -121,8 +121,8 @@ TEST( BayesTree, linear_smoother_shortcuts ) // Check the conditional P(C3|Root) Vector sigma3 = repeat(2, 0.61808); Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); - ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x5", zero(2), eye(2), "x6", A56, sigma3)); - GaussianBayesNet expected3; expected3.push_back(cg3); + GaussianBayesNet expected3; + push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3); Gaussian::sharedClique C3 = bayesTree["x4"]; GaussianBayesNet actual3 = C3->shortcut(R); CHECK(assert_equal(expected3,actual3,1e-4)); @@ -130,8 +130,8 @@ TEST( BayesTree, linear_smoother_shortcuts ) // Check the conditional P(C4|Root) Vector sigma4 = repeat(2, 0.661968); Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); - ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x6", A46, sigma4)); - GaussianBayesNet expected4; expected4.push_back(cg4); + GaussianBayesNet expected4; + push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4); Gaussian::sharedClique C4 = bayesTree["x3"]; GaussianBayesNet actual4 = C4->shortcut(R); CHECK(assert_equal(expected4,actual4,1e-4)); @@ -249,8 +249,7 @@ TEST( BayesTree, balanced_smoother_clique_marginals ) GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2); Vector sigma = repeat(2, 0.707107); Matrix A12 = (-0.5)*eye(2); - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x1", zero(2), eye(2), "x2", A12, sigma)); - expected.push_front(cg); + push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma); Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; FactorGraph marginal = C3->marginal(R); GaussianBayesNet actual = eliminate(marginal,C3->keys()); @@ -275,15 +274,13 @@ TEST( BayesTree, balanced_smoother_joint ) // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7); - ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1", zero(2), eye(2), "x7", A, sigma)); - expected1.push_front(cg1); + push_front(expected1,"x1", zero(2), eye(2), "x7", A, sigma); GaussianBayesNet actual1 = bayesTree.jointBayesNet("x1","x7"); CHECK(assert_equal(expected1,actual1,1e-4)); // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1); - ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("x7", zero(2), eye(2), "x1", A, sigma)); - expected2.push_front(cg2); + push_front(expected2,"x7", zero(2), eye(2), "x1", A, sigma); GaussianBayesNet actual2 = bayesTree.jointBayesNet("x7","x1"); CHECK(assert_equal(expected2,actual2,1e-4)); @@ -291,8 +288,7 @@ TEST( BayesTree, balanced_smoother_joint ) GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4); Vector sigma14 = repeat(2, 0.784465); Matrix A14 = (-0.0769231)*eye(2); - ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x1", zero(2), eye(2), "x4", A14, sigma14)); - expected3.push_front(cg3); + push_front(expected3,"x1", zero(2), eye(2), "x4", A14, sigma14); GaussianBayesNet actual3 = bayesTree.jointBayesNet("x1","x4"); CHECK(assert_equal(expected3,actual3,1e-4)); @@ -300,8 +296,7 @@ TEST( BayesTree, balanced_smoother_joint ) GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1); Vector sigma41 = repeat(2, 0.668096); Matrix A41 = (-0.055794)*eye(2); - ConditionalGaussian::shared_ptr cg4(new ConditionalGaussian("x4", zero(2), eye(2), "x1", A41, sigma41)); - expected4.push_front(cg4); + push_front(expected4,"x4", zero(2), eye(2), "x1", A41, sigma41); GaussianBayesNet actual4 = bayesTree.jointBayesNet("x4","x1"); CHECK(assert_equal(expected4,actual4,1e-4)); } diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index cd1edab48..dbe68d811 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -193,21 +193,8 @@ TEST( LinearFactorGraph, eliminateOne_x1 ) ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x1"); // create expected Conditional Gaussian - Matrix R11 = Matrix_(2,2, - 1.0, 0.0, - 0.0, 1.0 - ); - Matrix S12 = Matrix_(2,2, - -0.111111, 0.00, - +0.00,-0.111111 - ); - Matrix S13 = Matrix_(2,2, - -0.444444, 0.00, - +0.00,-0.444444 - ); - Vector d(2); d(0) = -0.133333; d(1) = -0.0222222; - Vector sigma(2); sigma(0) = 1./15; sigma(1) = 1./15; - + Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15); ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,sigma); CHECK(assert_equal(expected,*actual,tol)); @@ -221,21 +208,8 @@ TEST( LinearFactorGraph, eliminateOne_x2 ) ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x2"); // create expected Conditional Gaussian - Matrix R11 = Matrix_(2,2, - 1.0, 0.0, - 0.0, 1.0 - ); - Matrix S12 = Matrix_(2,2, - -0.2, 0.0, - +0.0,-0.2 - ); - Matrix S13 = Matrix_(2,2, - -0.8, 0.0, - +0.0,-0.8 - ); - Vector d(2); d(0) = 0.2; d(1) = -0.14; - Vector sigma(2); sigma(0) = 0.0894427; sigma(1) = 0.0894427; - + Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427); ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,sigma); CHECK(assert_equal(expected,*actual,tol)); @@ -248,21 +222,8 @@ TEST( LinearFactorGraph, eliminateOne_l1 ) ConditionalGaussian::shared_ptr actual = fg.eliminateOne("l1"); // create expected Conditional Gaussian - Matrix R11 = Matrix_(2,2, - 1.0, 0.0, - 0.0, 1.0 - ); - Matrix S12 = Matrix_(2,2, - -0.5, 0.0, - +0.0,-0.5 - ); - Matrix S13 = Matrix_(2,2, - -0.5, 0.0, - +0.0,-0.5 - ); - Vector d(2); d(0) = -0.1; d(1) = 0.25; - Vector sigma(2); sigma(0) = 0.141421; sigma(1) = 0.141421; - + Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421); ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,sigma); CHECK(assert_equal(expected,*actual,tol)); @@ -272,50 +233,22 @@ TEST( LinearFactorGraph, eliminateOne_l1 ) TEST( LinearFactorGraph, eliminateAll ) { // create expected Chordal bayes Net - double data1[] = { 1.0, 0.0, - 0.0, 1.0}; - Matrix R1 = Matrix_(2,2, data1); - Vector d1(2); d1(0) = -0.1; d1(1) = -0.1; - Vector sigma1(2); sigma1(0) = 0.1; sigma1(1) = 0.1; + Matrix I = eye(2); - ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian("x1",d1, R1, sigma1)); + Vector d1 = Vector_(2, -0.1,-0.1); + GaussianBayesNet expected = simpleGaussian("x1",d1,0.1); - double data21[] = { 1.0, 0.0, - 0.0, 1.0}; - Matrix R2 = Matrix_(2,2, data21); - double data22[] = { -1.0, 0.0, - 0.0, -1.0}; - Matrix A1 = Matrix_(2,2, data22); - Vector d2(2); d2(0) = 0.0; d2(1) = 0.2; - Vector sigma2(2); sigma2(0) = 0.149071; sigma2(1) = 0.149071; + Vector d2 = Vector_(2, 0.0, 0.2), sigma2 = repeat(2,0.149071); + push_front(expected,"l1",d2, I,"x1", (-1)*I,sigma2); - ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian("l1",d2, R2,"x1", A1,sigma2)); - - double data31[] = { 1.0, 0.0, - 0.0, 1.0}; - Matrix R3 = Matrix_(2,2, data31); - double data32[] = { -0.2, 0.0, - 0.0, -0.2}; - Matrix A21 = Matrix_(2,2, data32); - double data33[] = { -0.8, 0.0, - 0.0, -0.8}; - Matrix A22 = Matrix_(2,2, data33); - - Vector d3(2); d3(0) = 0.2; d3(1) = -0.14; - Vector sigma3(2); sigma3(0) = 0.0894427; sigma3(1) = 0.0894427; - - ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3,"l1", A21, "x1", A22, sigma3)); - - GaussianBayesNet expected; - expected.push_back(cg3); - expected.push_back(cg2); - expected.push_back(cg1); + Vector d3 = Vector_(2, 0.2, -0.14), sigma3 = repeat(2,0.0894427); + push_front(expected,"x2",d3, I,"l1", (-0.2)*I, "x1", (-0.8)*I, sigma3); // Check one ordering LinearFactorGraph fg1 = createLinearFactorGraph(); - Ordering ord1; - ord1 += "x2","l1","x1"; - GaussianBayesNet actual = fg1.eliminate(ord1); + Ordering ordering; + ordering += "x2","l1","x1"; + GaussianBayesNet actual = fg1.eliminate(ordering); CHECK(assert_equal(expected,actual,tol)); }