From ddbea256af1f5e1dc627c699df6a1e0289893de7 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 27 Jun 2012 20:03:13 +0000 Subject: [PATCH] Merge branch 'master' into new_wrap_local --- gtsam/discrete/DiscreteConditional.h | 6 +- gtsam/discrete/DiscreteFactor.h | 6 +- gtsam/discrete/DiscreteFactorGraph.cpp | 13 ++ gtsam/discrete/DiscreteFactorGraph.h | 11 +- gtsam/inference/BayesNet-inl.h | 23 +- gtsam/inference/BayesNet.h | 8 +- gtsam/inference/BayesTree-inl.h | 12 + gtsam/inference/BayesTree.h | 1 + gtsam/inference/BayesTreeCliqueBase.h | 3 + gtsam/inference/ClusterTree-inl.h | 17 +- gtsam/inference/ClusterTree.h | 10 +- gtsam/inference/EliminationTree-inl.h | 9 +- gtsam/inference/EliminationTree.h | 7 +- gtsam/inference/Factor-inl.h | 5 +- gtsam/inference/Factor.h | 5 +- gtsam/inference/FactorGraph-inl.h | 5 +- gtsam/inference/FactorGraph.h | 3 +- gtsam/linear/GaussianDensity.cpp | 4 +- gtsam/linear/GaussianDensity.h | 3 +- gtsam/linear/VectorValues.cpp | 4 +- gtsam/linear/VectorValues.h | 10 +- gtsam/nonlinear/NonlinearISAM.cpp | 8 +- tests/testSubgraphPreconditioner.cpp | 297 ++++++++++++------------- 23 files changed, 266 insertions(+), 204 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index c95cc36d6..6de9989f2 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -63,9 +63,11 @@ namespace gtsam { /// @{ /** GTSAM-style print */ - void print(const std::string& s = "Discrete Conditional: ") const { + void print(const std::string& s = "Discrete Conditional: ", + const boost::function& formatter + = &(boost::lexical_cast)) const { std::cout << s << std::endl; - IndexConditional::print(s); + IndexConditional::print(s, formatter); Potentials::print(s); } diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 0bfdb3068..deac7efce 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -82,8 +82,10 @@ namespace gtsam { /// @{ // print - virtual void print(const std::string& s = "DiscreteFactor\n") const { - IndexFactor::print(s); + virtual void print(const std::string& s = "DiscreteFactor\n", + const boost::function& formatter + = &(boost::lexical_cast)) const { + IndexFactor::print(s,formatter); } /// @} diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4726c752f..f3c89721b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -63,6 +63,18 @@ namespace gtsam { return product; } + /* ************************************************************************* */ + void DiscreteFactorGraph::print(const std::string& s, + const IndexFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + } + } + /* ************************************************************************* */ std::pair // EliminateDiscrete(const FactorGraph& factors, size_t num) { @@ -90,6 +102,7 @@ namespace gtsam { return std::make_pair(cond, sum); } + /* ************************************************************************* */ } // namespace diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index d96a3049a..44b814507 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -77,15 +77,8 @@ public: double operator()(const DiscreteFactor::Values & values) const; /// print - void print(const std::string& s = "DiscreteFactorGraph") const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); - } - } + void print(const std::string& s = "DiscreteFactorGraph", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; }; // DiscreteFactorGraph diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 57cef4634..89361e8fc 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -26,6 +26,7 @@ #include // for += using boost::assign::operator+=; +#include #include #include @@ -33,12 +34,30 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesNet::print(const std::string& s) const { + void BayesNet::print(const std::string& s, + const boost::function& formatter) const { std::cout << s; BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) - conditional->print(); + conditional->print("Conditional", formatter); } + /* ************************************************************************* */ + template + void BayesNet::printStats(const std::string& s) const { + + const size_t n = conditionals_.size(); + size_t max_size = 0; + size_t total = 0; + BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) { + max_size = std::max(max_size, conditional->size()); + total += conditional->size(); + } + std::cout << s + << "maximum conditional size = " << max_size << std::endl + << "average conditional size = " << total / n << std::endl + << "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl; + } + /* ************************************************************************* */ template bool BayesNet::equals(const BayesNet& cbn, double tol) const { diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index f6721816c..32abd9ed6 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,7 @@ public: typedef typename boost::shared_ptr > shared_ptr; /** We store shared pointers to Conditional densities */ + typedef typename CONDITIONAL::KeyType KeyType; typedef typename boost::shared_ptr sharedConditional; typedef typename boost::shared_ptr const_sharedConditional; typedef typename std::list Conditionals; @@ -88,7 +90,11 @@ public: /// @{ /** print */ - void print(const std::string& s = "") const; + void print(const std::string& s = "", + const boost::function& formatter = &(boost::lexical_cast)) const; + + /** print statistics */ + void printStats(const std::string& s = "") const; /** check equality */ bool equals(const BayesNet& other, double tol = 1e-9) const; diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 22dd0c62b..501a8aabd 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; @@ -102,6 +103,17 @@ namespace gtsam { } + /* ************************************************************************* */ + template + void BayesTree::CliqueStats::print(const std::string& s) const { + std::cout << s + <<"avg Conditional Size: " << avgConditionalSize << std::endl + << "max Conditional Size: " << maxConditionalSize << std::endl + << "avg Separator Size: " << avgSeparatorSize << std::endl + << "max Separator Size: " << maxSeparatorSize << std::endl; + } + + /* ************************************************************************* */ template typename BayesTree::CliqueStats BayesTree::CliqueData::getStats() const { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 9c38d6496..6413e4b0d 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -81,6 +81,7 @@ namespace gtsam { std::size_t maxConditionalSize; double avgSeparatorSize; std::size_t maxSeparatorSize; + void print(const std::string& s = "") const ; }; /** store all the sizes */ 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/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h index 548a55cf4..b3fe204e7 100644 --- a/gtsam/inference/ClusterTree-inl.h +++ b/gtsam/inference/ClusterTree-inl.h @@ -71,10 +71,11 @@ namespace gtsam { /* ************************************************************************* */ template - void ClusterTree::Cluster::print(const std::string& indent) const { + void ClusterTree::Cluster::print(const std::string& indent, + const boost::function& formatter) const { std::cout << indent; BOOST_FOREACH(const Index key, frontal) - std::cout << key << " "; + std::cout << formatter(key) << " "; std::cout << ": "; BOOST_FOREACH(const Index key, separator) std::cout << key << " "; @@ -83,19 +84,21 @@ namespace gtsam { /* ************************************************************************* */ template - void ClusterTree::Cluster::printTree(const std::string& indent) const { - print(indent); + void ClusterTree::Cluster::printTree(const std::string& indent, + const boost::function& formatter) const { + print(indent, formatter); BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent + " "); + child->printTree(indent + " ", formatter); } /* ************************************************************************* * * ClusterTree * ************************************************************************* */ template - void ClusterTree::print(const std::string& str) const { + void ClusterTree::print(const std::string& str, + const boost::function& formatter) const { std::cout << str << std::endl; - if (root_) root_->printTree(""); + if (root_) root_->printTree("", formatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 92919b936..f253fe4d6 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -25,6 +25,7 @@ #include #include +#include #include @@ -38,6 +39,9 @@ namespace gtsam { */ template class ClusterTree { + public: + // Access to factor types + typedef typename FG::KeyType KeyType; protected: @@ -74,10 +78,10 @@ namespace gtsam { Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); /// print - void print(const std::string& indent) const; + void print(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; /// print the enire tree - void printTree(const std::string& indent) const; + void printTree(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; /// check equality bool equals(const Cluster& other) const; @@ -123,7 +127,7 @@ namespace gtsam { /// @{ /// print the object - void print(const std::string& str="") const; + void print(const std::string& str="", const boost::function& formatter = &(boost::lexical_cast)) const; /** check equality */ bool equals(const ClusterTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index c4cb9d4cf..81c98c926 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -166,12 +166,13 @@ EliminationTree::Create(const FactorGraph& factorGraph) { /* ************************************************************************* */ template -void EliminationTree::print(const std::string& name) const { - std::cout << name << " (" << key_ << ")" << std::endl; +void EliminationTree::print(const std::string& name, + const boost::function& formatter) const { + std::cout << name << " (" << formatter(key_) << ")" << std::endl; BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->print(name + " "); } + factor->print(name + " ", formatter); } BOOST_FOREACH(const shared_ptr& child, subTrees_) { - child->print(name + " "); } + child->print(name + " ", formatter); } } /* ************************************************************************* */ diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 6ab7dcf49..8e57a7208 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -54,6 +54,8 @@ public: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef gtsam::BayesNet BayesNet; ///< The BayesNet corresponding to FACTOR + typedef FACTOR Factor; + typedef typename FACTOR::KeyType KeyType; /** Typedef for an eliminate subroutine */ typedef typename FactorGraph::Eliminate Eliminate; @@ -67,7 +69,7 @@ private: typedef FastList SubTrees; typedef std::vector Conditionals; - Index key_; ///< index associated with root + Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor? Factors factors_; ///< factors associated with root SubTrees subTrees_; ///< sub-trees @@ -141,7 +143,8 @@ public: /// @{ /** Print the tree to cout */ - void print(const std::string& name = "EliminationTree: ") const; + void print(const std::string& name = "EliminationTree: ", + const boost::function& formatter = &(boost::lexical_cast)) const; /** Test whether the tree is equal to another */ bool equals(const EliminationTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index d9fb1e0de..ad9f43701 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -56,9 +56,10 @@ namespace gtsam { /* ************************************************************************* */ template - void Factor::print(const std::string& s) const { + void Factor::print(const std::string& s, + const boost::function& formatter) const { std::cout << s << " "; - BOOST_FOREACH(KEY key, keys_) std::cout << " " << key; + BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); std::cout << std::endl; } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 6ed9c809b..0e80fc8ea 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include namespace gtsam { @@ -190,7 +192,8 @@ public: /// @{ /// print - void print(const std::string& s = "Factor") const; + void print(const std::string& s = "Factor", + const boost::function& formatter = &(boost::lexical_cast)) const; /// check equality bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 90212f39f..c787655af 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -48,13 +48,14 @@ namespace gtsam { /* ************************************************************************* */ template - void FactorGraph::print(const std::string& s) const { + void FactorGraph::print(const std::string& s, + const boost::function& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); } } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 89662ffc4..905afc55c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -138,7 +138,8 @@ template class BayesTree; /// @{ /** print out graph */ - void print(const std::string& s = "FactorGraph") const; + void print(const std::string& s = "FactorGraph", + const boost::function& formatter = &(boost::lexical_cast)) const; /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 97e42d9ba..c8fc4c21a 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,11 +23,11 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - void GaussianDensity::print(const string &s) const + void GaussianDensity::print(const string &s, const IndexFormatter& formatter) const { cout << s << ": density on "; for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) - cout << (boost::format("[%1%]")%(*it)).str() << " "; + cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; gtsam::print(Matrix(get_R()),"R"); gtsam::print(Vector(get_d()),"d"); diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 2c13e847c..61db582f4 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -54,7 +54,8 @@ namespace gtsam { } /// print - void print(const std::string& = "GaussianDensity") const; + void print(const std::string& = "GaussianDensity", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 5868d8452..1413d1517 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -77,10 +77,10 @@ void VectorValues::insert(Index j, const Vector& value) { } /* ************************************************************************* */ -void VectorValues::print(const std::string& str) const { +void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; for (Index var = 0; var < size(); ++var) - std::cout << " " << var << ": \n" << operator[](var) << "\n"; + std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n"; std::cout.flush(); } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index f3d82511e..89c59a6ad 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -159,15 +160,16 @@ namespace gtsam { iterator begin() { chk(); return maps_.begin(); } ///< Iterator over variables const_iterator begin() const { chk(); return maps_.begin(); } ///< Iterator over variables - iterator end() { chk(); return maps_.end(); } ///< Iterator over variables + iterator end() { chk(); return maps_.end(); } ///< Iterator over variables const_iterator end() const { chk(); return maps_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables + reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ") const; + void print(const std::string& str = "VectorValues: ", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 6b5ea31bc..b2135fedd 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -118,13 +118,7 @@ void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) con /* ************************************************************************* */ void NonlinearISAM::printStats() const { - gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData(); - gtsam::GaussianISAM::CliqueStats stats = data.getStats(); - cout << "\navg Conditional Size: " << stats.avgConditionalSize; - cout << "\nmax Conditional Size: " << stats.maxConditionalSize; - cout << "\navg Separator Size: " << stats.avgSeparatorSize; - cout << "\nmax Separator Size: " << stats.maxSeparatorSize; - cout << endl; + isam_.getCliqueData().getStats().print(); } /* ************************************************************************* */ 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); } /* ************************************************************************* */