From 4e2aae01214a4e20e6dc260b1f15144fb032bc15 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 26 Jun 2012 18:49:30 +0000 Subject: [PATCH] formatting of comments, added parent() interface for Bayes Tree cliques --- gtsam/inference/BayesTree-inl.h | 3 +- gtsam/inference/BayesTreeCliqueBase.h | 3 + tests/testSubgraphPreconditioner.cpp | 297 +++++++++++++------------- 3 files changed, 152 insertions(+), 151 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 22dd0c62b..8e9d769a6 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -67,6 +67,7 @@ namespace gtsam { of.close(); } + /* ************************************************************************* */ template void BayesTree::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { static int num = 0; @@ -101,7 +102,7 @@ namespace gtsam { } } - + /* ************************************************************************* */ template typename BayesTree::CliqueStats BayesTree::CliqueData::getStats() const { diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 9a2f6c54e..d054fe43f 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -117,6 +117,9 @@ namespace gtsam { /** return the const reference of children */ const std::list& children() const { return children_; } + /** return a shared_ptr to the parent clique */ + derived_ptr parent() const { return parent_.lock(); } + /// @} /// @name Advanced Interface /// @{ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 382ac4cc7..417643e78 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -40,14 +40,14 @@ Key i3002 = 3002, i2002 = 2002, i1002 = 1002; Key i3001 = 3001, i2001 = 2001, i1001 = 1001; // TODO fix Ordering::equals, because the ordering *is* correct ! -/* ************************************************************************* * -TEST( SubgraphPreconditioner, planarOrdering ) -{ - // Check canonical ordering - Ordering expected, ordering = planarOrdering(3); - expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; - CHECK(assert_equal(expected,ordering)); -} +/* ************************************************************************* */ +//TEST( SubgraphPreconditioner, planarOrdering ) +//{ +// // Check canonical ordering +// Ordering expected, ordering = planarOrdering(3); +// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; +// CHECK(assert_equal(expected,ordering)); +//} /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) @@ -66,148 +66,145 @@ TEST( SubgraphPreconditioner, planarGraph ) CHECK(assert_equal(xtrue,actual)); } -/* ************************************************************************* * -TEST( SubgraphPreconditioner, splitOffPlanarTree ) -{ - // Build a planar graph - GaussianFactorGraph A; - VectorValues xtrue; - boost::tie(A, xtrue) = planarGraph(3); - - // Get the spanning tree and constraints, and check their sizes - JacobianFactorGraph T, C; - // TODO big mess: GFG and JFG mess !!! - boost::tie(T, C) = splitOffPlanarTree(3, A); - LONGS_EQUAL(9,T.size()); - LONGS_EQUAL(4,C.size()); - - // Check that the tree can be solved to give the ground xtrue - GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); - VectorValues xbar = optimize(*R1); - CHECK(assert_equal(xtrue,xbar)); -} - -/* ************************************************************************* * -TEST( SubgraphPreconditioner, system ) -{ - // Build a planar graph - JacobianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 - - // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); - - // Create zero config - VectorValues zeros = VectorValues::Zero(xbar); - - // Set up y0 as all zeros - VectorValues y0 = zeros; - - // y1 = perturbed y0 - VectorValues y1 = zeros; - y1[i2003] = Vector_(2, 1.0, -1.0); - - // Check corresponding x values - VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[i2003] = Vector_(2, 2.01, 2.99); - expected_x1[i3003] = Vector_(2, 3.01, 2.99); - CHECK(assert_equal(xtrue, system.x(y0))); - CHECK(assert_equal(expected_x1,system.x(y1))); - - // Check errors -// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO ! -// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! - DOUBLES_EQUAL(0,error(system,y0),1e-9); - DOUBLES_EQUAL(3,error(system,y1),1e-9); - - // Test gradient in x - VectorValues expected_gx0 = zeros; - VectorValues expected_gx1 = zeros; - CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[i1003] = Vector_(2, -100., 100.); - expected_gx1[i2002] = Vector_(2, -100., 100.); - expected_gx1[i2003] = Vector_(2, 200., -200.); - expected_gx1[i3002] = Vector_(2, -100., 100.); - expected_gx1[i3003] = Vector_(2, 100., -100.); - CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); - - // Test gradient in y - VectorValues expected_gy0 = zeros; - VectorValues expected_gy1 = zeros; - expected_gy1[i1003] = Vector_(2, 2., -2.); - expected_gy1[i2002] = Vector_(2, -2., 2.); - expected_gy1[i2003] = Vector_(2, 3., -3.); - expected_gy1[i3002] = Vector_(2, -1., 1.); - expected_gy1[i3003] = Vector_(2, 1., -1.); - CHECK(assert_equal(expected_gy0,gradient(system,y0))); - CHECK(assert_equal(expected_gy1,gradient(system,y1))); - - // Check it numerically for good measure - // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) - // Vector numerical_g1 = numericalGradient (error, y1, 0.001); - // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., - // 3., -3., 0., 0., -1., 1., 1., -1.); - // CHECK(assert_equal(expected_g1,numerical_g1)); -} - -/* ************************************************************************* * -TEST( SubgraphPreconditioner, conjugateGradients ) -{ - // Build a planar graph - GaussianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 - - // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); - - // Create zero config y0 and perturbed config y1 - VectorValues y0 = VectorValues::Zero(xbar); - - VectorValues y1 = y0; - y1[i2003] = Vector_(2, 1.0, -1.0); - VectorValues x1 = system.x(y1); - - // Solve for the remaining constraints using PCG - ConjugateGradientParameters parameters; -// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); -// CHECK(assert_equal(y0,actual)); - - // Compare with non preconditioned version: - VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); - CHECK(assert_equal(xtrue,actual2,1e-4)); -} +/* ************************************************************************* */ +//TEST( SubgraphPreconditioner, splitOffPlanarTree ) +//{ +// // Build a planar graph +// GaussianFactorGraph A; +// VectorValues xtrue; +// boost::tie(A, xtrue) = planarGraph(3); +// +// // Get the spanning tree and constraints, and check their sizes +// JacobianFactorGraph T, C; +// // TODO big mess: GFG and JFG mess !!! +// boost::tie(T, C) = splitOffPlanarTree(3, A); +// LONGS_EQUAL(9,T.size()); +// LONGS_EQUAL(4,C.size()); +// +// // Check that the tree can be solved to give the ground xtrue +// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); +// VectorValues xbar = optimize(*R1); +// CHECK(assert_equal(xtrue,xbar)); +//} /* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +//TEST( SubgraphPreconditioner, system ) +//{ +// // Build a planar graph +// JacobianFactorGraph Ab; +// VectorValues xtrue; +// size_t N = 3; +// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b +// +// // Get the spanning tree and corresponding ordering +// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 +// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); +// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); +// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); +// +// // Eliminate the spanning tree to build a prior +// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 +// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 +// +// // Create Subgraph-preconditioned system +// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible +// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); +// +// // Create zero config +// VectorValues zeros = VectorValues::Zero(xbar); +// +// // Set up y0 as all zeros +// VectorValues y0 = zeros; +// +// // y1 = perturbed y0 +// VectorValues y1 = zeros; +// y1[i2003] = Vector_(2, 1.0, -1.0); +// +// // Check corresponding x values +// VectorValues expected_x1 = xtrue, x1 = system.x(y1); +// expected_x1[i2003] = Vector_(2, 2.01, 2.99); +// expected_x1[i3003] = Vector_(2, 3.01, 2.99); +// CHECK(assert_equal(xtrue, system.x(y0))); +// CHECK(assert_equal(expected_x1,system.x(y1))); +// +// // Check errors +//// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO ! +//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! +// DOUBLES_EQUAL(0,error(system,y0),1e-9); +// DOUBLES_EQUAL(3,error(system,y1),1e-9); +// +// // Test gradient in x +// VectorValues expected_gx0 = zeros; +// VectorValues expected_gx1 = zeros; +// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); +// expected_gx1[i1003] = Vector_(2, -100., 100.); +// expected_gx1[i2002] = Vector_(2, -100., 100.); +// expected_gx1[i2003] = Vector_(2, 200., -200.); +// expected_gx1[i3002] = Vector_(2, -100., 100.); +// expected_gx1[i3003] = Vector_(2, 100., -100.); +// CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); +// +// // Test gradient in y +// VectorValues expected_gy0 = zeros; +// VectorValues expected_gy1 = zeros; +// expected_gy1[i1003] = Vector_(2, 2., -2.); +// expected_gy1[i2002] = Vector_(2, -2., 2.); +// expected_gy1[i2003] = Vector_(2, 3., -3.); +// expected_gy1[i3002] = Vector_(2, -1., 1.); +// expected_gy1[i3003] = Vector_(2, 1., -1.); +// CHECK(assert_equal(expected_gy0,gradient(system,y0))); +// CHECK(assert_equal(expected_gy1,gradient(system,y1))); +// +// // Check it numerically for good measure +// // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) +// // Vector numerical_g1 = numericalGradient (error, y1, 0.001); +// // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., +// // 3., -3., 0., 0., -1., 1., 1., -1.); +// // CHECK(assert_equal(expected_g1,numerical_g1)); +//} + +/* ************************************************************************* */ +//TEST( SubgraphPreconditioner, conjugateGradients ) +//{ +// // Build a planar graph +// GaussianFactorGraph Ab; +// VectorValues xtrue; +// size_t N = 3; +// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b +// +// // Get the spanning tree and corresponding ordering +// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 +// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); +// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); +// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); +// +// // Eliminate the spanning tree to build a prior +// Ordering ordering = planarOrdering(N); +// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 +// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 +// +// // Create Subgraph-preconditioned system +// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible +// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); +// +// // Create zero config y0 and perturbed config y1 +// VectorValues y0 = VectorValues::Zero(xbar); +// +// VectorValues y1 = y0; +// y1[i2003] = Vector_(2, 1.0, -1.0); +// VectorValues x1 = system.x(y1); +// +// // Solve for the remaining constraints using PCG +// ConjugateGradientParameters parameters; +//// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); +//// CHECK(assert_equal(y0,actual)); +// +// // Compare with non preconditioned version: +// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); +// CHECK(assert_equal(xtrue,actual2,1e-4)); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */