diff --git a/.cproject b/.cproject index d0c9e6d01..c8509cd4e 100644 --- a/.cproject +++ b/.cproject @@ -5,46 +5,47 @@ - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -298,605 +299,611 @@ - - -make - -all -true -true -true - - -make - -clean -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - - -make -clean -true -true -true - - -make --k -check -true -false -true - - -make --j2 -testSimpleCamera.run -true -true -true - - -make --f local.mk -testCal3_S2.run -true -true -true - - -make --j2 -testVSLAMFactor.run -true -true -true - - -make --j2 -testCalibratedCamera.run -true -true -true - - -make --j2 -testGaussianConditional.run -true -true -true - - -make --j2 -testPose2.run -true -true -true - - -make --j2 -testRot3.run -true -true -true - - -make --j2 -testNonlinearOptimizer.run -true -true -true - - -make --j2 -testGaussianFactor.run -true -true -true - - -make --j2 -testGaussianFactorGraph.run -true -true -true - - -make --j2 -testNonlinearFactorGraph.run -true -true -true - - -make --j2 -testPose3.run -true -true -true - - -make --j2 -testVectorMap.run -true -true -true - - -make --j2 -testPoint2.run -true -true -true - - -make --j2 -testNonlinearFactor.run -true -true -true - - -make --j2 -timeGaussianFactor.run -true -true -true - - -make --j2 -timeGaussianFactorGraph.run -true -true -true - - -make --j2 -testGaussianBayesNet.run -true -true -true - - -make - -testBayesTree.run -true -false -true - - -make -testSymbolicBayesNet.run -true -false -true - - -make - -testSymbolicFactorGraph.run -true -false -true - - -make --j2 -testVector.run -true -true -true - - -make --j2 -testMatrix.run -true -true -true - - -make --j2 -testVSLAMGraph.run -true -true -true - - -make --j2 -testNonlinearEquality.run -true -true -true - - -make --j2 -testSQP.run -true -true -true - - -make --j2 -testNonlinearConstraint.run -true -true -true - - -make --j2 -testVSLAMConfig.run -true -true -true - - -make --j2 -testOrdering.run -true -true -true - - -make --j2 -testRot2.run -true -true -true - - -make --j2 -testGaussianBayesTree.run -true -true -true - - -make --j2 -testPose3Config.run -true -true -true - - -make --j2 -testUrbanMeasurement.run -true -true -true - - -make --j2 -testUrbanOdometry.run -true -true -true - - -make --j2 -testIterative.run -true -true -true - - -make --j2 -testGaussianISAM2.run -true -true -true - - -make --j2 -testSubgraphPreconditioner.run -true -true -true - - -make --j2 -testBayesNetPreconditioner.run -true -true -true - - -make --j2 -testPose2Factor.run -true -true -true - - -make --j2 -timeRot3.run -true -true -true - - -make --j2 -testPose2SLAM.run -true -true -true - - -make --j2 -testPose2Config.run -true -true -true - - -make --j2 -testLieConfig.run -true -true -true - - -make --j2 -testPlanarSLAM.run -true -true -true - - -make - -testGraph.run -true -false -true - - -make --j2 -testPose3SLAM.run -true -true -true - - -make --j2 -testTupleConfig.run -true -true -true - - -make --j2 -testPose2Prior.run -true -true -true - - -make --j2 -testNoiseModel.run -true -true -true - - -make --j2 -testISAM.run -true -true -true - - -make --j2 -testGaussianISAM.run -true -true -true - - -make -testSimulated2D.run -true -false -true - - -make --j2 -timeMatrix.run -true -true -true - - -make --j2 -testKey.run -true -true -true - - -make --j2 -timeVectorConfig.run -true -true -true - - -make --j2 -testHomography2.run -true -true -true - - -make --j2 -testVectorBTree.run -true -true -true - - -make - -testErrors.run -true -false -true - - -make - -testDSF.run -true -true -true - - -make --j2 -testFactorGraph.run -true -true -true - - -make - -testConstraintOptimizer.run -true -true -true - - -make -testBTree.run -true -true -true - - -make - -testSimulated2DOriented.run -true -false -true - - -make -testDSFVector.run -true -true -true - - -make -testSPQRUtil.run -true -true -true - - -make -testInference.run -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -clean -true -true -true - - -make --j2 -check -true -true -true - - - - + + + make + all + true + true + true + + + make + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + check + true + true + true + + + make + + clean + true + true + true + + + make + -k + check + true + false + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -f local.mk + testCal3_S2.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + testGaussianConditional.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testGaussianFactorGraph.run + true + true + true + + + make + -j2 + testNonlinearFactorGraph.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + testVectorMap.run + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j2 + testNonlinearFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + testBayesTree.run + true + false + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testSQP.run + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testGaussianBayesTree.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testUrbanMeasurement.run + true + true + true + + + make + -j2 + testUrbanOdometry.run + true + true + true + + + make + -j2 + testIterative.run + true + true + true + + + make + -j2 + testGaussianISAM2.run + true + true + true + + + make + -j2 + testSubgraphPreconditioner.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testPlanarSLAM.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + -j2 + testTupleConfig.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testISAM.run + true + true + true + + + make + -j2 + testGaussianISAM.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + testKey.run + true + true + true + + + make + -j2 + timeVectorConfig.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testVectorBTree.run + true + true + true + + + make + testErrors.run + true + false + true + + + make + testDSF.run + true + true + true + + + make + -j2 + testFactorGraph.run + true + true + true + + + make + testConstraintOptimizer.run + true + true + true + + + make + + testBTree.run + true + true + true + + + make + testSimulated2DOriented.run + true + false + true + + + make + + testDSFVector.run + true + true + true + + + make + + testSPQRUtil.run + true + true + true + + + make + + testInference.run + true + true + true + + + make + + testJunctionTree.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + + - - + + diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index e86d69281..886c2ce89 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -67,6 +67,9 @@ namespace gtsam { /** is this the root of a Bayes tree ? */ inline bool isRoot() const { return parent_==NULL;} + /** return the const reference of children */ + const std::list& children() const { return children_; } + /** The size of subtree rooted at this clique, i.e., nr of Cliques */ size_t treeSize() const; @@ -230,9 +233,7 @@ namespace gtsam { * Altering Bayes trees */ - /** - * Remove all nodes - */ + /** Remove all nodes */ void clear(); /** diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index c0cbeb371..37ddaab59 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -165,7 +165,7 @@ std::pair, set > FactorGraph::removeSingleto singletons.insert(new_singletons.begin(), new_singletons.end()); BOOST_FOREACH(const Symbol& singleton, new_singletons) - findAndRemoveFactors(singleton); + findAndRemoveFactors > >(singleton); // exit when there are no more singletons if (new_singletons.empty()) break; @@ -317,9 +317,8 @@ list FactorGraph::factors(const Symbol& key) const { /* ************************************************************************* */ /** find all non-NULL factors for a variable, then set factors to NULL */ /* ************************************************************************* */ -template -vector > -FactorGraph::findAndRemoveFactors(const Symbol& key) { +template template +Factors FactorGraph::findAndRemoveFactors(const Symbol& key) { // find all factor indices associated with the key Indices::const_iterator it = indices_.find(key); @@ -329,7 +328,7 @@ FactorGraph::findAndRemoveFactors(const Symbol& key) { + (string)key + " not found"); const list& factorsAssociatedWithKey = it->second; - vector found; + Factors found; BOOST_FOREACH(const int& i, factorsAssociatedWithKey) { sharedFactor& fi = factors_.at(i); // throws exception ! if(fi == NULL) continue; // skip NULL factors @@ -477,7 +476,8 @@ std::pair, FactorGraph > FactorGraph::splitM template boost::shared_ptr removeAndCombineFactors(FactorGraph& factorGraph, const Symbol& key) { - vector > found = factorGraph.findAndRemoveFactors(key); + typedef vector > Factors; + Factors found = factorGraph.template findAndRemoveFactors(key); boost::shared_ptr new_factor(new Factor(found)); return new_factor; } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index c633eba33..85f64a3b6 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -123,7 +123,8 @@ namespace gtsam { * from the factor graph * @param key the key for the given node */ - std::vector findAndRemoveFactors(const Symbol& key); + template + Factors findAndRemoveFactors(const Symbol& key); /** * find the minimum spanning tree using boost graph library diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 60261b440..ab1382de8 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -16,6 +16,7 @@ #include "FactorGraph-inl.h" #include "inference-inl.h" #include "iterative.h" +#include "JunctionTree-inl.h" using namespace std; using namespace gtsam; @@ -207,7 +208,8 @@ std::pair combineFactorsAndCreateMatrix( GaussianConditional::shared_ptr GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) { // find and remove all factors connected to key - vector factors = findAndRemoveFactors(key); + typedef vector Factors; + Factors factors = findAndRemoveFactors(key); // Collect all dimensions as well as the set of separator keys set separator; @@ -285,6 +287,14 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old) return delta; } +/* ************************************************************************* */ +VectorConfig GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering) +{ + GaussianJunctionTree junctionTree(*this, ordering); + + return junctionTree.optimize(); +} + /* ************************************************************************* */ boost::shared_ptr GaussianFactorGraph::eliminate_(const Ordering& ordering) diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 26e9ad0b2..57d1cf617 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -154,6 +154,11 @@ namespace gtsam { */ VectorConfig optimize(const Ordering& ordering, bool enableJoinFactor = true); + /** + * optimize a linear factor graph with multi-frontals + */ + VectorConfig optimizeMultiFrontals(const Ordering& ordering); + /** * shared pointer versions for MATLAB */ diff --git a/cpp/JunctionTree-inl.h b/cpp/JunctionTree-inl.h index ca807476d..3cc8073b2 100644 --- a/cpp/JunctionTree-inl.h +++ b/cpp/JunctionTree-inl.h @@ -8,43 +8,113 @@ #pragma once -#include #include -#include -#include "Pose2.h" -#include "BayesTree-inl.h" + #include "SymbolicFactorGraph.h" - +#include "BayesTree-inl.h" #include "JunctionTree.h" -#define DEBUG(i) \ - if (verboseLevel>i) cout - namespace gtsam { using namespace std; + /* ************************************************************************* */ + template + bool JunctionTree::Clique::equals(const JunctionTree::Clique& other) const { + if (!frontal_.equals(other.frontal_)) + return false; + + if (!separator_.equals(other.separator_)) + return false; + + if (children_.size() != other.children_.size()) + return false; + + typename vector::const_iterator it1 = children_.begin(); + typename vector::const_iterator it2 = other.children_.begin(); + for(; it1!=children_.end(); it1++, it2++) + if (!(*it1)->equals(**it2)) return false; + + return true; + } + /* ************************************************************************* */ /** - * Linear JunctionTree + * JunctionTree */ - template - void JunctionTree::SubFG::printTree(const string& indent) const { + template + void JunctionTree::Clique::print(const string& indent) const { + // FG::print(indent); + cout << "kai1" << endl; + cout << indent; + BOOST_FOREACH(const Symbol& key, frontal_) + cout << (string)key << " "; + cout << ":"; + BOOST_FOREACH(const Symbol& key, separator_) + cout << (string)key << " "; + cout << endl; + } + + /* ************************************************************************* */ + template + void JunctionTree::Clique::printTree(const string& indent) const { print(indent); BOOST_FOREACH(const shared_ptr& child, children_) child->printTree(indent+" "); } /* ************************************************************************* */ - template + template + JunctionTree::JunctionTree(FG& fg, const Ordering& ordering) { + // Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree + SymbolicFactorGraph sfg(fg); + SymbolicBayesNet sbn = sfg.eliminate(ordering); + BayesTree sbt(sbn); + sbt.print("sbt"); + + // distribtue factors + root_ = distributeFactors(fg, sbt.root()); + } + + /* ************************************************************************* */ + template + typename JunctionTree::sharedClique JunctionTree::distributeFactors(FG& fg, + const BayesTree::sharedClique bayesClique) { + // create a new clique in the junction tree + sharedClique clique(new Clique()); + clique->frontal_ = bayesClique->ordering(); + clique->separator_.insert(bayesClique->separator_.begin(), bayesClique->separator_.end()); + + // recursively call the children + BOOST_FOREACH(const BayesTree::sharedClique bayesChild, bayesClique->children()) { + sharedClique child = distributeFactors(fg, bayesChild); + clique->children_.push_back(child); + child->parent_ = clique; + } + + // collect the factors + typedef vector Factors; + BOOST_FOREACH(const Symbol& frontal, clique->frontal_) { + Factors factors = fg.template findAndRemoveFactors(frontal); + BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors) { + clique->push_back(factor_); + } + } + + return clique; + } + + /* ************************************************************************* */ + template template pair::sharedClique> - JunctionTree::eliminateOneClique(sharedSubFG current, BayesTree& bayesTree) { + JunctionTree::eliminateOneClique(sharedClique current, BayesTree& bayesTree) { + typedef typename BayesTree::sharedClique sharedBtreeClique; FG fg; // factor graph will be assembled from local factors and marginalized children - list children; + list children; fg.push_back(*current); // add the local factor graph - BOOST_FOREACH(sharedSubFG& child, current->children_) { + BOOST_FOREACH(sharedClique& child, current->children_) { // receive the factors from the child and its clique point - FG fgChild; sharedClique cliqueChild; + FG fgChild; sharedBtreeClique cliqueChild; boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree); if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!"); @@ -58,57 +128,54 @@ namespace gtsam { BayesNet bn = fg.eliminateFrontals(current->frontal_); // create a new clique corresponding the combined factors - sharedClique new_clique = bayesTree.insert(bn, children); + sharedBtreeClique new_clique = bayesTree.insert(bn, children); return make_pair(fg, new_clique); } /* ************************************************************************* */ - template - BayesTree JunctionTree::eliminate() { + template template + BayesTree JunctionTree::eliminate() { BayesTree bayesTree; - eliminateOneClique(rootFG_, bayesTree); + eliminateOneClique(root_, bayesTree); return bayesTree; } /* ************************************************************************* */ - template - void JunctionTree::iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current) { - if (!current.get()) current = rootFG_; -// iterateBFS(visitor, current); - } - - /* ************************************************************************* */ - template - void JunctionTree::iterSubGraphsBFS(VisitorSubFG visitor) { -// iterateDFS(visitor, rootFG_); + template + bool JunctionTree::equals(const JunctionTree& other, double tol) const { + if (!root_ || !other.root_) return false; + return root_->equals(*other.root_); } /* ************************************************************************* */ /** - * Linear JunctionTree + * GaussianJunctionTree */ - template - void LinearJunctionTree::btreeBackSubstitue(typename BayesTree::sharedClique current, VectorConfig& config) { + template + void GaussianJunctionTree::btreeBackSubstitue(typename BayesTree::sharedClique current, VectorConfig& config) { // solve the bayes net in the current node - typename BayesNet::const_reverse_iterator it = current->rbegin(); + typename BayesNet::const_reverse_iterator it = current->rbegin(); for (; it!=current->rend(); it++) { Vector x = (*it)->solve(config); // Solve for that variable config.insert((*it)->key(),x); // store result in partial solution } // solve the bayes nets in the child nodes - BOOST_FOREACH(sharedClique child, current->children_) { + typedef typename BayesTree::sharedClique sharedBayesClique; + BOOST_FOREACH(sharedBayesClique child, current->children_) { btreeBackSubstitue(child, config); } } /* ************************************************************************* */ - template - VectorConfig LinearJunctionTree::optimize() { + template + VectorConfig GaussianJunctionTree::optimize() { // eliminate from leaves to the root - BayesTree bayesTree = JunctionTree::eliminate(); + typedef JunctionTree Base; + BayesTree bayesTree = this->eliminate(); + // back-substitution VectorConfig result; btreeBackSubstitue(bayesTree.root(), result); return result; diff --git a/cpp/JunctionTree.h b/cpp/JunctionTree.h index 9d12b5700..b788ec059 100644 --- a/cpp/JunctionTree.h +++ b/cpp/JunctionTree.h @@ -18,128 +18,114 @@ namespace gtsam { /* ************************************************************************* */ - template - class JunctionTree/*: public BayesTree*/ { + template + class JunctionTree : public Testable > { public: - typedef typename BayesTree::sharedClique sharedClique; - - // the threshold for the sizes of submaps. Smaller ones will be absorbed into the separator - static const int const_minNodesPerMap_default = 10; - static const int const_minNodesPerMap_ultra = 1; - - // when to stop partitioning - static const int const_numNodeStopPartition_default = 50; - static const int const_numNodeStopPartition_ultra = 3; // so that A,B,C all have one variable - - // the class for subgraphs that also include the pointers to the parents and two children - class SubFG : public FG { - public: - typedef typename boost::shared_ptr shared_ptr; + class Clique : public FG { + private: + typedef typename boost::shared_ptr shared_ptr; shared_ptr parent_; // the parent subgraph node + std::vector children_; // the child cliques Ordering frontal_; // the frontal varaibles - Unordered separator_; // the separator variables + Unordered separator_; // the separator variables - friend class JunctionTree; + friend class JunctionTree; public: - std::vector children_; // the child cliques // empty constructor - SubFG() {} + Clique() {} // constructor with all the information - SubFG(const FG& fgLocal, const Ordering& frontal, const Unordered& separator, + Clique(const FG& fgLocal, const Ordering& frontal, const Unordered& separator, const shared_ptr& parent) : frontal_(frontal), separator_(separator), FG(fgLocal), parent_(parent) {} // constructor for an empty graph - SubFG(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent) + Clique(const Ordering& frontal, const Unordered& separator, const shared_ptr& parent) : frontal_(frontal), separator_(separator), parent_(parent) {} + // return the members const Ordering& frontal() const { return frontal_;} const Unordered& separator() const { return separator_;} - std::vector& children() { return children_; } // TODO:: add const + const std::vector& children() { return children_; } // add a child node void addChild(const shared_ptr& child) { children_.push_back(child); } + // print the object + void print(const std::string& indent) const; void printTree(const std::string& indent) const; + + // check equality + bool equals(const Clique& other) const; }; // typedef for shared pointers to cliques - typedef typename SubFG::shared_ptr sharedSubFG; - typedef boost::function VisitorSubFG; + typedef typename Clique::shared_ptr sharedClique; protected: // Root clique - sharedSubFG rootFG_; + sharedClique root_; private: + // distribute the factors along the Bayes tree + sharedClique distributeFactors(FG& fg, const BayesTree::sharedClique clique); // utility function called by eliminateBottomUp - std::pair eliminateOneClique(sharedSubFG fg_, BayesTree& bayesTree); + template + std::pair::sharedClique> eliminateOneClique( + sharedClique fg_, BayesTree& bayesTree); public: + // constructor + JunctionTree() {} - JunctionTree() : verboseLevel(0) {} + // constructor given a factor graph and the elimination ordering + JunctionTree(FG& fg, const Ordering& ordering); - // return the root graph - sharedSubFG rootFG() const { return rootFG_; } + // return the root clique + sharedClique root() const { return root_; } // eliminate the factors in the subgraphs + template BayesTree eliminate(); // print the object void print(const std::string& str) const { - if (rootFG_.get()) rootFG_->printTree(""); + cout << str << endl; + if (root_.get()) root_->printTree(""); } - // iterate over all the subgraphs from root to leaves in the DFS order, recursive - void iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current = sharedSubFG()); + /** check equality */ + bool equals(const JunctionTree& other, double tol = 1e-9) const; - // iterate over all the subgraphs from root to leaves in the BFS order, non-recursive - void iterSubGraphsBFS(VisitorSubFG visitor); - - // the output level - int verboseLevel; }; // JunctionTree /* ************************************************************************* */ /** - * Linear JunctionTree which can do optimization + * GaussianJunctionTree that does the optimization */ - template - class LinearJunctionTree: public JunctionTree { + template + class GaussianJunctionTree: public JunctionTree { public: - typedef JunctionTree Base; - typedef typename BayesTree::sharedClique sharedClique; - typedef typename JunctionTree::sharedSubFG sharedSubFG; + typedef JunctionTree Base; + typedef typename JunctionTree::sharedClique sharedClique; protected: // back-substitute in topological sort order (parents first) - void btreeBackSubstitue(typename BayesTree::sharedClique current, VectorConfig& config); + void btreeBackSubstitue(typename BayesTree::sharedClique current, VectorConfig& config); public : - LinearJunctionTree() : Base() {} + GaussianJunctionTree() : Base() {} // constructor - LinearJunctionTree(const FG& fg, const Ordering& ordering, int numNodeStopPartition = Base::const_numNodeStopPartition_default, - int minNodesPerMap = Base::const_minNodesPerMap_default) : - Base(fg, ordering, numNodeStopPartition, minNodesPerMap) {} + GaussianJunctionTree(FG& fg, const Ordering& ordering) : Base(fg, ordering) {} // optimize the linear graph VectorConfig optimize(); }; // Linear JunctionTree - class SymbolicConditional; - class SymbolicFactorGraph; - - /** - * recursive partitioning - */ - typedef JunctionTree SymbolicTSAM; - typedef JunctionTree GaussianTSAM; - } // namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 2ebec0fe7..b54ae39d2 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -75,13 +75,15 @@ headers += BayesTree.h BayesTree-inl.h headers += ISAM.h ISAM-inl.h GaussianISAM.h headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h sources += GaussianISAM.cpp GaussianISAM2.cpp -check_PROGRAMS += testGraph testFactorGraph testInference testOrdering +check_PROGRAMS += testGraph testFactorGraph testJunctionTree testInference testOrdering check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2 testGraph_SOURCES = testGraph.cpp testGraph_LDADD = libgtsam.la testFactorGraph_SOURCES = testFactorgraph.cpp -testInference_SOURCES = testInference.cpp testFactorGraph_LDADD = libgtsam.la +testJunctionTree_SOURCES = testJunctionTree.cpp +testJunctionTree_LDADD = libgtsam.la +testInference_SOURCES = testInference.cpp testInference_LDADD = libgtsam.la testBayesTree_SOURCES = testBayesTree.cpp testBayesTree_LDADD = libgtsam.la diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index a649ae68f..6121eb919 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -693,7 +693,7 @@ void householder(Matrix &A) { __CLPK_integer info; dgeqrf_(&m, &n, a, &m, tau, &work_optimal_size, &lwork, &info); - lwork = (int)work_optimal_size; + lwork = (__CLPK_integer)work_optimal_size; double work[lwork]; dgeqrf_(&m, &n, a, &m, tau, work, &lwork, &info); int k0 = 0; diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 9f10f5188..8a8086869 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -475,38 +475,6 @@ TEST( GaussianFactorGraph, optimize ) CHECK(assert_equal(expected,actual)); } -/* ************************************************************************* * - Bayes tree for smoother with "nested dissection" ordering: - C1 x5 x6 x4 - C2 x3 x2 : x4 - C3 x1 : x2 - C4 x7 : x6 - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, optimizeMultiFrontal ) -{ - // create a graph - GaussianFactorGraph fg = createSmoother(7); - - // create an ordering - Ordering ordering; - ordering += "x1","x3","x5","x7","x2","x6","x4"; - - // Symbolic factorization - // GaussianFactorGraph -> SymbolicFactorGraph -> SymbolicBayesNet -> SymbolicBayesTree - SymbolicFactorGraph sfg(fg); - SymbolicBayesNet sbn = sfg.eliminate(ordering); - BayesTree sbt(sbn); - -// // optimize the graph -// VectorConfig actual = fg.optimizeMultiFrontal(sbt); -// -// // verify -// VectorConfig expected = createCorrectDelta(); -// -// CHECK(assert_equal(expected,actual)); -} - /* ************************************************************************* */ TEST( GaussianFactorGraph, combine) { @@ -584,7 +552,8 @@ TEST( GaussianFactorGraph, findAndRemoveFactors ) GaussianFactor::shared_ptr f2 = fg[2]; // call the function - vector factors = fg.findAndRemoveFactors("x1"); + vector factors = fg.findAndRemoveFactors + >("x1"); // Check the factors CHECK(f0==factors[0]); @@ -607,7 +576,8 @@ TEST( GaussianFactorGraph, findAndRemoveFactors_twice ) GaussianFactor::shared_ptr f2 = fg[2]; // call the function - vector factors = fg.findAndRemoveFactors("x1"); + vector factors = fg.findAndRemoveFactors + >("x1"); // Check the factors CHECK(f0==factors[0]); diff --git a/cpp/testJunctionTree.cpp b/cpp/testJunctionTree.cpp new file mode 100644 index 000000000..5c67af9d0 --- /dev/null +++ b/cpp/testJunctionTree.cpp @@ -0,0 +1,86 @@ +/* + * testJunctionTree.cpp + * + * Created on: Jul 8, 2010 + * Author: nikai + * Description: + */ + +#include +#include + +#include +#include // for operator += +#include // for operator += +using namespace boost::assign; + +#define GTSAM_MAGIC_KEY + +#include "smallExample.h" +#include "JunctionTree-inl.h" + +using namespace std; +using namespace gtsam; +using namespace example; + +/* ************************************************************************* * + Bayes tree for smoother with "nested dissection" ordering: + C1 x5 x6 x4 + C2 x3 x2 : x4 + C3 x1 : x2 + C4 x7 : x6 +/* ************************************************************************* */ +TEST( GaussianFactorGraph, constructor ) +{ + // create a graph + GaussianFactorGraph fg = createSmoother(7); + + // create an ordering + Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; + + GaussianJunctionTree junctionTree(fg, ordering); + Ordering frontal1; frontal1 += "x5", "x6", "x4"; + Ordering frontal2; frontal2 += "x3", "x2"; + Ordering frontal3; frontal3 += "x1"; + Ordering frontal4; frontal4 += "x7"; + Unordered sep1; + Unordered sep2; sep2 += "x4"; + Unordered sep3; sep3 += "x2"; + Unordered sep4; sep4 += "x6"; + CHECK(assert_equal(frontal1, junctionTree.root()->frontal())); + CHECK(assert_equal(sep1, junctionTree.root()->separator())); + LONGS_EQUAL(5, junctionTree.root()->size()); + CHECK(assert_equal(frontal2, junctionTree.root()->children()[0]->frontal())); + CHECK(assert_equal(sep2, junctionTree.root()->children()[0]->separator())); + LONGS_EQUAL(4, junctionTree.root()->children()[0]->size()); + CHECK(assert_equal(frontal3, junctionTree.root()->children()[0]->children()[0]->frontal())); + CHECK(assert_equal(sep3, junctionTree.root()->children()[0]->children()[0]->separator())); + LONGS_EQUAL(2, junctionTree.root()->children()[0]->children()[0]->size()); + CHECK(assert_equal(frontal4, junctionTree.root()->children()[1]->frontal())); + CHECK(assert_equal(sep4, junctionTree.root()->children()[1]->separator())); + LONGS_EQUAL(2, junctionTree.root()->children()[1]->size()); +} + + +/* ************************************************************************* * +TEST( GaussianFactorGraph, optimizeMultiFrontal ) +{ + // create a graph + GaussianFactorGraph fg = createSmoother(7); + + // create an ordering + Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; + + // optimize the graph + LinearJunctionTree junctionTree(fg, ordering); + VectorConfig actual = junctionTree.optimize(); + + // verify + VectorConfig expected = createCorrectDelta(); + + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index fb0fb438a..57775c07e 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -46,7 +46,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors ) SymbolicFactorGraph actual(factorGraph); SymbolicFactor::shared_ptr f1 = actual[0]; SymbolicFactor::shared_ptr f3 = actual[2]; - actual.findAndRemoveFactors("x2"); + actual.findAndRemoveFactors("x2"); // construct expected graph after find_factors_and_remove SymbolicFactorGraph expected;