From 5b7d7b3793850d9757a59ca0bc2610bc6b77d79f Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 8 Aug 2013 20:45:44 +0000 Subject: [PATCH] Closer to building and tests running --- gtsam_unstable/linear/bayesTreeOperations.h | 2 +- .../linear/tests/testBayesTreeOperations.cpp | 4 +- .../linear/tests/testConditioning.cpp | 304 +++++++++--------- 3 files changed, 157 insertions(+), 153 deletions(-) diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h index ecfb9a711..1658c8973 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -76,7 +76,7 @@ GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool s else result.push_back(splitFactor(conditional)); } - BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, root->children()) + BOOST_FOREACH(typename BAYESTREE::sharedClique child, root->children) result.push_back(liquefy(child, splitConditionals)); return result; } diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp index 332b91ce7..b0d36c515 100644 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp @@ -193,10 +193,10 @@ TEST( testBayesTreeOperations, liquefy ) { // Create smoother with 7 nodes Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph smoother = createSmoother(7, ordering).first; + GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree - GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // bayesTree.print("Full tree"); SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6))); diff --git a/gtsam_unstable/linear/tests/testConditioning.cpp b/gtsam_unstable/linear/tests/testConditioning.cpp index 69646fbdc..da8f56117 100644 --- a/gtsam_unstable/linear/tests/testConditioning.cpp +++ b/gtsam_unstable/linear/tests/testConditioning.cpp @@ -12,6 +12,8 @@ #include #include +#include +#include #include @@ -64,7 +66,8 @@ TEST( testConditioning, directed_elimination_singlefrontal ) { Index root_key = 0, removed_key = 1, remaining_parent = 2; Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0); - Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0); + Vector d0 = d.segment(0,1), d1 = d.segment(1,1); + SharedDiagonal sigmas = noiseModel::Unit::Create(1); GaussianConditional::shared_ptr initConditional(new GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas)); @@ -95,155 +98,156 @@ TEST( testConditioning, directed_elimination_singlefrontal ) { EXPECT(!actRemoveFrontal); } -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_multifrontal ) { - // Use top two rows from the previous example - Index root_key = 0, removed_key = 1, remaining_parent = 2; - Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1), - Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0); - Vector d1 = d.segment(0,2), sigmas1 = Vector_(1, 1.0), sigmas2 = Vector_(2, 1.0, 1.0); - - - std::list > terms; - terms += make_pair(root_key, Matrix(R11.col(0))); - terms += make_pair(removed_key, Matrix(R11.col(1))); - terms += make_pair(remaining_parent, S); - GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2)); - - VectorValues solution; - solution.insert(0, x.segment(0,1)); - solution.insert(1, x.segment(1,1)); - solution.insert(2, x.segment(2,1)); - - std::set saved_indices; - saved_indices += root_key, remaining_parent; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - GaussianConditional::shared_ptr expSummarized(new - GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); - - CHECK(actSummarized); - EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); -} - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_multifrontal_multidim ) { - // use larger example, three frontal variables, dim = 2 each, two parents (one removed) - // Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4; - // Remove 1, 3 - Matrix Rinit = Matrix_(6, 11, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4, - 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5, - 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6); - - vector init_dims; init_dims += 2, 2, 2, 2, 2, 1; - GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); - Vector sigmas = ones(6); - vector init_keys; init_keys += 0, 1, 2, 3, 4; - GaussianConditional::shared_ptr initConditional(new - GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas)); - - // Construct a solution vector - VectorValues solution; - solution.insert(0, zero(2)); - solution.insert(1, zero(2)); - solution.insert(2, zero(2)); - solution.insert(3, Vector_(2, 1.0, 2.0)); - solution.insert(4, Vector_(2, 3.0, 4.0)); - - initConditional->solveInPlace(solution); - - std::set saved_indices; - saved_indices += 0, 2, 4; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - CHECK(actSummarized); - - Matrix Rexp = Matrix_(4, 7, - 1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2, - 0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5, - 0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6); - - // Update rhs - Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 6, 2, 2) * solution.at(3); - Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3); - - vector exp_dims; exp_dims += 2, 2, 2, 1; - GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); - Vector exp_sigmas = ones(4); - vector exp_keys; exp_keys += 0, 2, 4; - GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas); - - EXPECT(assert_equal(expSummarized, *actSummarized, tol)); -} - -/* ************************************************************************* */ -TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { - // Example from LinearAugmentedSystem - // 4 variables, last two in ordering kept - should be able to do this with no computation. - - vector init_dims; init_dims += 3, 3, 2, 2, 1; - - //Full initial conditional: density on [3] [4] [5] [6] - Matrix Rinit = Matrix_(10, 11, - 8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0, - 0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0, - 0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0, - 0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0, - 0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0); - Vector dinit = Vector_(10, - -0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134); - Rinit.rightCols(1) = dinit; - Vector sigmas = ones(10); - - GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end()); - vector init_keys; init_keys += 3, 4, 5, 6; - GaussianConditional::shared_ptr initConditional(new - GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas)); - - // Calculate a solution - VectorValues solution; - solution.insert(0, zero(3)); - solution.insert(1, zero(3)); - solution.insert(2, zero(3)); - solution.insert(3, zero(3)); - solution.insert(4, zero(3)); - solution.insert(5, zero(2)); - solution.insert(6, zero(2)); - - initConditional->solveInPlace(solution); - - // Perform summarization - std::set saved_indices; - saved_indices += 5, 6; - - GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); - CHECK(actSummarized); - - // Create expected value on [5], [6] - Matrix Rexp = Matrix_(4, 5, - 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011, - 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465, - 0.0, 0.0, 2.04823446, -0.302033014, 0.0439795, - 0.0, 0.0, 0.0, 2.24068986, -0.0222134); - Vector expsigmas = ones(4); - - vector exp_dims; exp_dims += 2, 2, 1; - GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end()); - vector exp_keys; exp_keys += 5, 6; - GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas); - - EXPECT(assert_equal(expConditional, *actSummarized, tol)); -} +///* ************************************************************************* */ +//TEST( testConditioning, directed_elimination_multifrontal ) { +// // Use top two rows from the previous example +// Index root_key = 0, removed_key = 1, remaining_parent = 2; +// Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1), +// Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0); +// Vector d1 = d.segment(0,2); +// SharedDiagonal sigmas1 = noiseModel::Unit::Create(1), sigmas2 = noiseModel::Unit::Create(2); +// +// +// std::list > terms; +// terms += make_pair(root_key, Matrix(R11.col(0))); +// terms += make_pair(removed_key, Matrix(R11.col(1))); +// terms += make_pair(remaining_parent, S); +// GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2)); +// +// VectorValues solution; +// solution.insert(0, x.segment(0,1)); +// solution.insert(1, x.segment(1,1)); +// solution.insert(2, x.segment(2,1)); +// +// std::set saved_indices; +// saved_indices += root_key, remaining_parent; +// +// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); +// GaussianConditional::shared_ptr expSummarized(new +// GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1)); +// +// CHECK(actSummarized); +// EXPECT(assert_equal(*expSummarized, *actSummarized, tol)); +//} +// +///* ************************************************************************* */ +//TEST( testConditioning, directed_elimination_multifrontal_multidim ) { +// // use larger example, three frontal variables, dim = 2 each, two parents (one removed) +// // Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4; +// // Remove 1, 3 +// Matrix Rinit = Matrix_(6, 11, +// 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1, +// 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2, +// 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3, +// 0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4, +// 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5, +// 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6); +// +// vector init_dims; init_dims += 2, 2, 2, 2, 2, 1; +// VerticalBlockMatrix init_matrices(init_dims, Rinit); +// SharedDiagonal sigmas = noiseModel::Unit::Create(6); +// vector init_keys; init_keys += 0, 1, 2, 3, 4; +// GaussianConditional::shared_ptr initConditional(new +// GaussianConditional(init_keys, 3, init_matrices, sigmas)); +// +// // Construct a solution vector +// VectorValues solution; +// solution.insert(0, zero(2)); +// solution.insert(1, zero(2)); +// solution.insert(2, zero(2)); +// solution.insert(3, Vector_(2, 1.0, 2.0)); +// solution.insert(4, Vector_(2, 3.0, 4.0)); +// +// solution = initConditional->solve(solution); +// +// std::set saved_indices; +// saved_indices += 0, 2, 4; +// +// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); +// CHECK(actSummarized); +// +// Matrix Rexp = Matrix_(4, 7, +// 1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1, +// 0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2, +// 0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5, +// 0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6); +// +// // Update rhs +// Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 6, 2, 2) * solution.at(3); +// Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3); +// +// vector exp_dims; exp_dims += 2, 2, 2, 1; +// VerticalBlockMatrix exp_matrices(exp_dims, Rexp); +// SharedDiagonal exp_sigmas = noiseModel::Unit::Create(4); +// vector exp_keys; exp_keys += 0, 2, 4; +// GaussianConditional expSummarized(exp_keys, 2, exp_matrices, exp_sigmas); +// +// EXPECT(assert_equal(expSummarized, *actSummarized, tol)); +//} +// +///* ************************************************************************* */ +//TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) { +// // Example from LinearAugmentedSystem +// // 4 variables, last two in ordering kept - should be able to do this with no computation. +// +// vector init_dims; init_dims += 3, 3, 2, 2, 1; +// +// //Full initial conditional: density on [3] [4] [5] [6] +// Matrix Rinit = Matrix_(10, 11, +// 8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0, +// 0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0, +// 0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0, +// 0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0, +// 0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0, +// 0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0, +// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0, +// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0, +// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0, +// 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0); +// Vector dinit = Vector_(10, +// -0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134); +// Rinit.rightCols(1) = dinit; +// SharedDiagonal sigmas = noiseModel::Unit::Create(10); +// +// VerticalBlockMatrix init_matrices(init_dims, Rinit); +// vector init_keys; init_keys += 3, 4, 5, 6; +// GaussianConditional::shared_ptr initConditional(new +// GaussianConditional(init_keys, 4, init_matrices, sigmas)); +// +// // Calculate a solution +// VectorValues solution; +// solution.insert(0, zero(3)); +// solution.insert(1, zero(3)); +// solution.insert(2, zero(3)); +// solution.insert(3, zero(3)); +// solution.insert(4, zero(3)); +// solution.insert(5, zero(2)); +// solution.insert(6, zero(2)); +// +// solution = initConditional->solve(solution); +// +// // Perform summarization +// std::set saved_indices; +// saved_indices += 5, 6; +// +// GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution); +// CHECK(actSummarized); +// +// // Create expected value on [5], [6] +// Matrix Rexp = Matrix_(4, 5, +// 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011, +// 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465, +// 0.0, 0.0, 2.04823446, -0.302033014, 0.0439795, +// 0.0, 0.0, 0.0, 2.24068986, -0.0222134); +// SharedDiagonal expsigmas = noiseModel::Unit::Create(4); +// +// vector exp_dims; exp_dims += 2, 2, 1; +// VerticalBlockMatrix exp_matrices(exp_dims, Rexp); +// vector exp_keys; exp_keys += 5, 6; +// GaussianConditional expConditional(exp_keys, 2, exp_matrices, expsigmas); +// +// EXPECT(assert_equal(expConditional, *actSummarized, tol)); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }