diff --git a/inference/BayesNet.h b/inference/BayesNet.h index 9a8cd7afc..103a727f3 100644 --- a/inference/BayesNet.h +++ b/inference/BayesNet.h @@ -135,8 +135,8 @@ namespace gtsam { private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(conditionals_); } }; // BayesNet diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index e308907ac..31c41daaf 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -266,7 +266,7 @@ namespace gtsam { // TODO, why do we actually return a shared pointer, why does eliminate? /* ************************************************************************* */ template - template + template BayesNet BayesTree::Clique::shortcut(shared_ptr R) { // A first base case is when this clique or its parent is the root, @@ -281,13 +281,13 @@ namespace gtsam { // The parent clique has a CONDITIONAL for each frontal node in Fp // so we can obtain P(Fp|Sp) in factor graph form - FactorGraph p_Fp_Sp(*parent); + FACTORGRAPH p_Fp_Sp(*parent); // If not the base case, obtain the parent shortcut P(Sp|R) as factors - FactorGraph p_Sp_R(parent->shortcut(R)); + FACTORGRAPH p_Sp_R(parent->shortcut(R)); // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) - FactorGraph p_Cp_R = combine(p_Fp_Sp, p_Sp_R); + FACTORGRAPH p_Cp_R = combine(p_Fp_Sp, p_Sp_R); // Eliminate into a Bayes net with ordering designed to integrate out // any variables not in *our* separator. Variables to integrate out must be @@ -319,10 +319,10 @@ namespace gtsam { BOOST_FOREACH(Index key, separator) ordering.push_back(key); // eliminate to get marginal - typename FactorGraph::variableindex_type varIndex(p_Cp_R); + typename FACTORGRAPH::variableindex_type varIndex(p_Cp_R); Permutation toFront = Permutation::PullToFront(ordering, varIndex.size()); Permutation::shared_ptr toFrontInverse(toFront.inverse()); - BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, p_Cp_R) { + BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, p_Cp_R) { factor->permuteWithInverse(*toFrontInverse); } varIndex.permute(toFront); @@ -346,19 +346,18 @@ namespace gtsam { // Because the root clique could be very big. /* ************************************************************************* */ template - template - FactorGraph - BayesTree::Clique::marginal(shared_ptr R) { + template + FACTORGRAPH BayesTree::Clique::marginal(shared_ptr R) { // If we are the root, just return this root if (R.get()==this) return *R; // Combine P(F|S), P(S|R), and P(R) - BayesNet p_FSR = this->shortcut(R); + BayesNet p_FSR = this->shortcut(R); p_FSR.push_front(*this); p_FSR.push_back(*R); // Find marginal on the keys we are interested in - return Inference::Marginal(FactorGraph(p_FSR), keys()); + return Inference::Marginal(FACTORGRAPH(p_FSR), keys()); } // /* ************************************************************************* */ @@ -556,9 +555,9 @@ namespace gtsam { /* ************************************************************************* */ template - template - inline Index BayesTree::findParentClique(const Container& parents) const { - typename Container::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); + template + inline Index BayesTree::findParentClique(const CONTAINER& parents) const { + typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); assert(lowestOrderedParent != parents.end()); return *lowestOrderedParent; @@ -690,29 +689,28 @@ namespace gtsam { // First finds clique marginal then marginalizes that /* ************************************************************************* */ template - template - FactorGraph - BayesTree::marginal(Index key) const { + template + FACTORGRAPH BayesTree::marginal(Index key) const { // get clique containing key sharedClique clique = (*this)[key]; // calculate or retrieve its marginal - FactorGraph cliqueMarginal = clique->marginal(root_); + FACTORGRAPH cliqueMarginal = clique->marginal(root_); // Reorder so that only the requested key is not eliminated - typename FactorGraph::variableindex_type varIndex(cliqueMarginal); + typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal); vector keyAsVector(1); keyAsVector[0] = key; Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size())); Permutation::shared_ptr toBackInverse(toBack.inverse()); varIndex.permute(toBack); - BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, cliqueMarginal) { + BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { factor->permuteWithInverse(*toBackInverse); } // partially eliminate, remaining factor graph is requested marginal Inference::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex); - BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, cliqueMarginal) { + BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) { if(factor) factor->permuteWithInverse(toBack); } @@ -721,12 +719,11 @@ namespace gtsam { /* ************************************************************************* */ template - template - BayesNet - BayesTree::marginalBayesNet(Index key) const { + template + BayesNet BayesTree::marginalBayesNet(Index key) const { // calculate marginal as a factor graph - FactorGraph fg = this->marginal(key); + FACTORGRAPH fg = this->marginal(key); // eliminate further to Bayes net return *Inference::Eliminate(fg); @@ -808,8 +805,8 @@ namespace gtsam { /* ************************************************************************* */ template - template - void BayesTree::removeTop(const Container& keys, + template + void BayesTree::removeTop(const CONTAINER& keys, BayesNet& bn, typename BayesTree::Cliques& orphans) { // process each key of the new factor diff --git a/inference/BayesTree.h b/inference/BayesTree.h index 2409cff36..ffc81a4d4 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -107,12 +107,12 @@ namespace gtsam { /** return the conditional P(S|Root) on the separator given the root */ // TODO: create a cached version - template + template BayesNet shortcut(shared_ptr root); /** return the marginal P(C) of the clique */ - template - FactorGraph marginal(shared_ptr root); + template + FACTORGRAPH marginal(shared_ptr root); // /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ // template @@ -233,8 +233,8 @@ namespace gtsam { * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. */ - template - Index findParentClique(const Container& parents) const; + template + Index findParentClique(const CONTAINER& parents) const; /** number of cliques */ inline size_t size() const { @@ -257,11 +257,11 @@ namespace gtsam { CliqueData getCliqueData() const; /** return marginal on any variable */ - template - FactorGraph marginal(Index key) const; + template + FACTORGRAPH marginal(Index key) const; /** return marginal on any variable, as a Bayes Net */ - template + template BayesNet marginalBayesNet(Index key) const; // /** return joint on two variables */ @@ -299,8 +299,8 @@ namespace gtsam { * Given a list of keys, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ - template - void removeTop(const Container& keys, BayesNet& bn, Cliques& orphans); + template + void removeTop(const CONTAINER& keys, BayesNet& bn, Cliques& orphans); }; // BayesTree diff --git a/inference/ClusterTree-inl.h b/inference/ClusterTree-inl.h index 296f3c91a..87940fcd5 100644 --- a/inference/ClusterTree-inl.h +++ b/inference/ClusterTree-inl.h @@ -37,16 +37,16 @@ namespace gtsam { /* ************************************************************************* */ template - template + template ClusterTree::Cluster::Cluster( - const FG& fg, FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator) : + const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} /* ************************************************************************* */ template - template + template ClusterTree::Cluster::Cluster( - FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator) : + FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) : frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {} /* ************************************************************************* */ diff --git a/inference/ClusterTree.h b/inference/ClusterTree.h index 3302bc791..77f46a924 100644 --- a/inference/ClusterTree.h +++ b/inference/ClusterTree.h @@ -66,12 +66,12 @@ namespace gtsam { Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator); /* Create a node with several frontal variables */ - template - Cluster(const FG& fg, FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator); + template + Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); /* Create a node with several frontal variables */ - template - Cluster(FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator); + template + Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); // print the object void print(const std::string& indent) const; diff --git a/inference/Conditional.h b/inference/Conditional.h index 9b9c184a0..5341ebf8b 100644 --- a/inference/Conditional.h +++ b/inference/Conditional.h @@ -187,8 +187,8 @@ protected: private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(nrFrontals_); } }; diff --git a/inference/Factor.h b/inference/Factor.h index e62129ecb..803c50e44 100644 --- a/inference/Factor.h +++ b/inference/Factor.h @@ -77,7 +77,7 @@ public: FactorBase(const Conditional& c); /** Constructor from a collection of keys */ - template FactorBase(KeyIterator beginKey, KeyIterator endKey) : + template FactorBase(KEYITERATOR beginKey, KEYITERATOR endKey) : keys_(beginKey, endKey) { assertInvariants(); } /** Default constructor for I/O */ @@ -102,9 +102,9 @@ public: /** Named constructor for combining a set of factors with pre-computed set of * variables. (Old style - will be removed when scalar elimination is * removed in favor of the EliminationTree). */ - template - static typename DERIVED::shared_ptr Combine(const FactorGraphType& factorGraph, - const VariableIndex& variableIndex, const std::vector& factors, + template + static typename DERIVED::shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph, + const VariableIndex& variableIndex, const std::vector& factors, const std::vector& variables, const std::vector >& variablePositions) { return typename DERIVED::shared_ptr(new DERIVED(variables.begin(), variables.end())); } diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index 9a6fe8bff..b2e4b105d 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -103,10 +103,10 @@ namespace gtsam { } /* ************************************************************************* */ - template - FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { + template + FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) { // create new linear factor graph equal to the first one - FactorGraph fg = fg1; + FACTORGRAPH fg = fg1; // add the second factors_ in the graph fg.push_back(fg2); diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index 4a78ac64e..365503141 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -61,23 +61,23 @@ namespace gtsam { FactorGraph() {} /** convert from Bayes net */ - template - FactorGraph(const BayesNet& bayesNet); + template + FactorGraph(const BayesNet& bayesNet); /** convert from a derived type */ - template - FactorGraph(const FactorGraph& factorGraph); + template + FactorGraph(const FactorGraph& factorGraph); /** Add a factor */ - template - void push_back(const boost::shared_ptr& factor); + template + void push_back(const boost::shared_ptr& factor); /** push back many factors */ void push_back(const FactorGraph& factors); /** push back many factors with an iterator */ - template - void push_back(Iterator firstFactor, Iterator lastFactor); + template + void push_back(ITERATOR firstFactor, ITERATOR lastFactor); /** ------------------ Querying Factor Graphs ---------------------------- */ @@ -131,8 +131,8 @@ namespace gtsam { /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { + template + void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(factors_); } }; // FactorGraph @@ -143,8 +143,8 @@ namespace gtsam { * @param const &fg2 Linear factor graph * @return a new combined factor graph */ - template - FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2); + template + FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2); /** * These functions are defined here because they are templated on an @@ -155,10 +155,10 @@ namespace gtsam { /* ************************************************************************* */ template - template - FactorGraph::FactorGraph(const FactorGraph& factorGraph) { + template + FactorGraph::FactorGraph(const FactorGraph& factorGraph) { factors_.reserve(factorGraph.size()); - BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) { + BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) { if(factor) this->push_back(sharedFactor(new FACTOR(*factor))); else @@ -168,18 +168,18 @@ namespace gtsam { /* ************************************************************************* */ template - template - FactorGraph::FactorGraph(const BayesNet& bayesNet) { + template + FactorGraph::FactorGraph(const BayesNet& bayesNet) { factors_.reserve(bayesNet.size()); - BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) { + BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) { this->push_back(sharedFactor(new FACTOR(*cond))); } } /* ************************************************************************* */ template - template - inline void FactorGraph::push_back(const boost::shared_ptr& factor) { + template + inline void FactorGraph::push_back(const boost::shared_ptr& factor) { #ifndef NDEBUG factors_.push_back(boost::dynamic_pointer_cast(factor)); // add the actual factor #else @@ -189,9 +189,9 @@ namespace gtsam { /* ************************************************************************* */ template - template - void FactorGraph::push_back(Iterator firstFactor, Iterator lastFactor) { - Iterator factor = firstFactor; + template + void FactorGraph::push_back(ITERATOR firstFactor, ITERATOR lastFactor) { + ITERATOR factor = firstFactor; while(factor != lastFactor) this->push_back(*(factor++)); } diff --git a/inference/ISAM-inl.h b/inference/ISAM-inl.h index 06938db10..601bd5ea7 100644 --- a/inference/ISAM-inl.h +++ b/inference/ISAM-inl.h @@ -28,32 +28,32 @@ namespace gtsam { using namespace std; /** Create an empty Bayes Tree */ - template - ISAM::ISAM() : BayesTree() {} + template + ISAM::ISAM() : BayesTree() {} /** Create a Bayes Tree from a Bayes Net */ - template - ISAM::ISAM(const BayesNet& bayesNet) : - BayesTree(bayesNet) {} + template + ISAM::ISAM(const BayesNet& bayesNet) : + BayesTree(bayesNet) {} /* ************************************************************************* */ - template - template - void ISAM::update_internal(const FactorGraph& newFactors, Cliques& orphans) { + template + template + void ISAM::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) { // Remove the contaminated part of the Bayes tree - BayesNet bn; + BayesNet bn; removeTop(newFactors.keys(), bn, orphans); - FactorGraph factors(bn); + FACTORGRAPH factors(bn); // add the factors themselves factors.push_back(newFactors); // eliminate into a Bayes net - typename BayesNet::shared_ptr bayesNet = Inference::Eliminate(factors); + typename BayesNet::shared_ptr bayesNet = Inference::Eliminate(factors); // insert conditionals back in, straight into the topless bayesTree - typename BayesNet::const_reverse_iterator rit; + typename BayesNet::const_reverse_iterator rit; for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit ) this->insert(*rit); @@ -64,9 +64,9 @@ namespace gtsam { } - template - template - void ISAM::update(const FactorGraph& newFactors) { + template + template + void ISAM::update(const FACTORGRAPH& newFactors) { Cliques orphans; this->update_internal(newFactors, orphans); } diff --git a/inference/ISAM.h b/inference/ISAM.h index 8f8bb8261..d9dadfe5c 100644 --- a/inference/ISAM.h +++ b/inference/ISAM.h @@ -35,8 +35,8 @@ namespace gtsam { - template - class ISAM: public BayesTree { + template + class ISAM: public BayesTree { public: @@ -44,19 +44,19 @@ namespace gtsam { ISAM(); /** Create a Bayes Tree from a Bayes Net */ - ISAM(const BayesNet& bayesNet); + ISAM(const BayesNet& bayesNet); - typedef typename BayesTree::sharedClique sharedClique; + typedef typename BayesTree::sharedClique sharedClique; - typedef typename BayesTree::Cliques Cliques; + typedef typename BayesTree::Cliques Cliques; /** * iSAM. (update_internal provides access to list of orphans for drawing purposes) */ - template - void update_internal(const FactorGraph& newFactors, Cliques& orphans); - template - void update(const FactorGraph& newFactors); + template + void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans); + template + void update(const FACTORGRAPH& newFactors); }; // ISAM diff --git a/inference/IndexFactor.h b/inference/IndexFactor.h index e107b2fc2..19b7f33f8 100644 --- a/inference/IndexFactor.h +++ b/inference/IndexFactor.h @@ -63,9 +63,9 @@ public: static shared_ptr Combine(const FactorGraph& factors, const FastMap >& variableSlots); - template - static shared_ptr Combine(const FactorGraphType& factorGraph, - const VariableIndex& variableIndex, const std::vector& factors, + template + static shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph, + const VariableIndex& variableIndex, const std::vector& factors, const std::vector& variables, const std::vector >& variablePositions) { return Base::Combine(factorGraph, variableIndex, factors, variables, variablePositions); } diff --git a/inference/Permutation.h b/inference/Permutation.h index a3d8b7d5c..077eea699 100644 --- a/inference/Permutation.h +++ b/inference/Permutation.h @@ -156,22 +156,22 @@ protected: /** * Definition of Permuted class, see above comment for details. */ -template +template class Permuted { Permutation permutation_; - Container& container_; + CONTAINER& container_; public: - typedef ValueType value_type; + typedef VALUETYPE value_type; /** Construct as a permuted view on the Container. The permutation is copied * but only a reference to the container is stored. */ - Permuted(const Permutation& permutation, Container& container) : permutation_(permutation), container_(container) {} + Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {} /** Construct as a view on the Container with an identity permutation. Only * a reference to the container is stored. */ - Permuted(Container& container) : permutation_(Permutation::Identity(container.size())), container_(container) {} + Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {} /** Access the container through the permutation */ value_type operator[](size_t index) const { return container_[permutation_[index]]; } @@ -183,19 +183,19 @@ public: void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); } /** Access the underlying container */ - Container* operator->() { return &container_; } + CONTAINER* operator->() { return &container_; } /** Access the underlying container (const version) */ - const Container* operator->() const { return &container_; } + const CONTAINER* operator->() const { return &container_; } /** Size of the underlying container */ size_t size() const { return container_.size(); } /** Access to the underlying container */ - Container& container() { return container_; } + CONTAINER& container() { return container_; } /** Access to the underlying container (const version) */ - const Container& container() const { return container_; } + const CONTAINER& container() const { return container_; } /** Access the underlying permutation */ Permutation& permutation() { return permutation_; } diff --git a/inference/VariableIndex.h b/inference/VariableIndex.h index 26517fd0e..4df1936d2 100644 --- a/inference/VariableIndex.h +++ b/inference/VariableIndex.h @@ -53,7 +53,7 @@ struct _mapped_factor_type { _mapped_factor_type(size_t _factorIndex, size_t _variablePosition) : factorIndex(_factorIndex), variablePosition(_variablePosition) {} bool operator==(const _mapped_factor_type& o) const { return factorIndex == o.factorIndex && variablePosition == o.variablePosition; } }; -template +template class VariableIndex { public: @@ -64,7 +64,7 @@ public: typedef typename mapped_type::const_iterator const_factor_iterator; protected: - typedef typename VariableIndexStorage::template type_factory::type storage_type; + typedef typename VARIABLEINDEXSTORAGE::template type_factory::type storage_type; storage_type indexUnpermuted_; Permuted index_; size_t nFactors_; diff --git a/inference/graph-inl.h b/inference/graph-inl.h index 2efc5912e..3e9b06509 100644 --- a/inference/graph-inl.h +++ b/inference/graph-inl.h @@ -30,42 +30,42 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -template +template class ordering_key_visitor : public boost::default_bfs_visitor { public: - ordering_key_visitor(std::list& ordering_in) : ordering_(ordering_in) {} + ordering_key_visitor(std::list& ordering_in) : ordering_(ordering_in) {} template void discover_vertex(Vertex v, const Graph& g) const { - Key key = boost::get(boost::vertex_name, g, v); + KEY key = boost::get(boost::vertex_name, g, v); ordering_.push_front(key); } - std::list& ordering_; + std::list& ordering_; }; /* ************************************************************************* */ -template -list predecessorMap2Keys(const PredecessorMap& p_map) { +template +list predecessorMap2Keys(const PredecessorMap& p_map) { - typedef typename SGraph::Vertex SVertex; + typedef typename SGraph::Vertex SVertex; - SGraph g; + SGraph g; SVertex root; - std::map key2vertex; - boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, SVertex, Key>(p_map); + std::map key2vertex; + boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, SVertex, KEY>(p_map); // breadth first visit on the graph - std::list keys; - ordering_key_visitor vis(keys); + std::list keys; + ordering_key_visitor vis(keys); boost::breadth_first_search(g, root, boost::visitor(vis)); return keys; } /* ************************************************************************* */ -template -SDGraph toBoostGraph(const G& graph) { +template +SDGraph toBoostGraph(const G& graph) { // convert the factor graph to boost graph - SDGraph g; - typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; - map key2vertex; + SDGraph g; + typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; + map key2vertex; BoostVertex v1, v2; typename G::const_iterator itFactor; @@ -79,8 +79,8 @@ SDGraph toBoostGraph(const G& graph) { boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); if (!factor) continue; - Key key1 = factor->key1(); - Key key2 = factor->key2(); + KEY key1 = factor->key1(); + KEY key2 = factor->key2(); if (key2vertex.find(key1) == key2vertex.end()) { v1 = add_vertex(key1, g); @@ -102,14 +102,14 @@ SDGraph toBoostGraph(const G& graph) { } /* ************************************************************************* */ -template -boost::tuple > -predecessorMap2Graph(const PredecessorMap& p_map) { +template +boost::tuple > +predecessorMap2Graph(const PredecessorMap& p_map) { G g; - map key2vertex; + map key2vertex; V v1, v2, root; - Key child, parent; + KEY child, parent; bool foundRoot = false; FOREACH_PAIR(child, parent, p_map) { if (key2vertex.find(child) == key2vertex.end()) { @@ -134,47 +134,47 @@ predecessorMap2Graph(const PredecessorMap& p_map) { if (!foundRoot) throw invalid_argument("predecessorMap2Graph: invalid predecessor map!"); else - return boost::tuple >(g, root, key2vertex); + return boost::tuple >(g, root, key2vertex); } /* ************************************************************************* */ -template +template class compose_key_visitor : public boost::default_bfs_visitor { private: - boost::shared_ptr config_; + boost::shared_ptr config_; public: - compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} + compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} template void tree_edge(Edge edge, const Graph& g) const { - typename Values::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); - typename Values::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); - Pose relativePose = boost::get(boost::edge_weight, g, edge); + typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); + typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); + POSE relativePose = boost::get(boost::edge_weight, g, edge); config_->insert(key_to, (*config_)[key_from].compose(relativePose)); } }; /* ************************************************************************* */ -template -boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, - const Pose& rootPose) { +template +boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, + const POSE& rootPose) { //TODO: change edge_weight_t to edge_pose_t typedef typename boost::adjacency_list< boost::vecS, boost::vecS, boost::directedS, - boost::property, - boost::property > PoseGraph; + boost::property, + boost::property > PoseGraph; typedef typename boost::graph_traits::vertex_descriptor PoseVertex; typedef typename boost::graph_traits::edge_descriptor PoseEdge; PoseGraph g; PoseVertex root; - map key2vertex; + map key2vertex; boost::tie(g, root, key2vertex) = - predecessorMap2Graph(tree); + predecessorMap2Graph(tree); // attach the relative poses to the edges PoseEdge edge12, edge21; @@ -188,13 +188,13 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - typename Values::Key key1 = factor->key1(); - typename Values::Key key2 = factor->key2(); + typename VALUES::Key key1 = factor->key1(); + typename VALUES::Key key2 = factor->key2(); PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v2 = key2vertex.find(key2)->second; - Pose l1Xl2 = factor->measured(); + POSE l1Xl2 = factor->measured(); tie(edge12, found1) = boost::edge(v1, v2, g); tie(edge21, found2) = boost::edge(v2, v1, g); if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree"); @@ -206,10 +206,10 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap config(new Values); - typename Values::Key rootKey = boost::get(boost::vertex_name, g, root); + boost::shared_ptr config(new VALUES); + typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root); config->insert(rootKey, rootPose); - compose_key_visitor vis(config); + compose_key_visitor vis(config); boost::breadth_first_search(g, root, boost::visitor(vis)); return config; @@ -218,22 +218,22 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap -PredecessorMap findMinimumSpanningTree(const G& fg) { +template +PredecessorMap findMinimumSpanningTree(const G& fg) { - SDGraph g = gtsam::toBoostGraph(fg); + SDGraph g = gtsam::toBoostGraph(fg); // find minimum spanning tree - vector::Vertex> p_map(boost::num_vertices(g)); + vector::Vertex> p_map(boost::num_vertices(g)); prim_minimum_spanning_tree(g, &p_map[0]); // convert edge to string pairs - PredecessorMap tree; - typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; - typename vector::Vertex>::iterator vi; + PredecessorMap tree; + typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; + typename vector::Vertex>::iterator vi; for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { - Key key = boost::get(boost::vertex_name, g, *itVertex); - Key parent = boost::get(boost::vertex_name, g, *vi); + KEY key = boost::get(boost::vertex_name, g, *itVertex); + KEY parent = boost::get(boost::vertex_name, g, *vi); tree.insert(key, parent); } @@ -241,8 +241,8 @@ PredecessorMap findMinimumSpanningTree(const G& fg) { } /* ************************************************************************* */ -template -void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { +template +void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { typedef typename G::sharedFactor F ; @@ -256,12 +256,12 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { continue; } - boost::shared_ptr factor2 = boost::dynamic_pointer_cast< - Factor2>(factor); + boost::shared_ptr factor2 = boost::dynamic_pointer_cast< + FACTOR2>(factor); if (!factor2) continue; - Key key1 = factor2->key1(); - Key key2 = factor2->key2(); + KEY key1 = factor2->key1(); + KEY key2 = factor2->key2(); // if the tree contains the key if ((tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0) || diff --git a/inference/graph.h b/inference/graph.h index 880d3ffb3..043e83ee2 100644 --- a/inference/graph.h +++ b/inference/graph.h @@ -33,19 +33,19 @@ namespace gtsam { /** * SDGraph is undirected graph with variable keys and double edge weights */ - template + template class SDGraph: public boost::adjacency_list, boost::property< + boost::property, boost::property< boost::edge_weight_t, double> > { public: - typedef typename boost::graph_traits >::vertex_descriptor Vertex; + typedef typename boost::graph_traits >::vertex_descriptor Vertex; }; - template + template class SGraph : public boost::adjacency_list > { + boost::property > { public: - typedef typename boost::graph_traits >::vertex_descriptor Vertex; + typedef typename boost::graph_traits >::vertex_descriptor Vertex; }; //typedef boost::graph_traits::vertex_descriptor SVertex; @@ -53,20 +53,20 @@ namespace gtsam { /** * Map from variable key to parent key */ - template - class PredecessorMap: public std::map { + template + class PredecessorMap: public std::map { public: /** convenience insert so we can pass ints for TypedSymbol keys */ - inline void insert(const Key& key, const Key& parent) { - std::map::insert(std::make_pair(key, parent)); + inline void insert(const KEY& key, const KEY& parent) { + std::map::insert(std::make_pair(key, parent)); } }; /** * Generate a list of keys from a spanning tree represented by its predecessor map */ - template - std::list predecessorMap2Keys(const PredecessorMap& p_map); + template + std::list predecessorMap2Keys(const PredecessorMap& p_map); /** * Convert the factor graph to an SDGraph @@ -74,36 +74,36 @@ namespace gtsam { * F = Factor type * Key = Key type */ - template SDGraph toBoostGraph(const G& graph); + template SDGraph toBoostGraph(const G& graph); /** * Build takes a predecessor map, and builds a directed graph corresponding to the tree. * G = Graph type * V = Vertex type */ - template - boost::tuple > predecessorMap2Graph(const PredecessorMap& p_map); + template + boost::tuple > predecessorMap2Graph(const PredecessorMap& p_map); /** * Compose the poses by following the chain specified by the spanning tree */ - template - boost::shared_ptr - composePoses(const G& graph, const PredecessorMap& tree, const Pose& rootPose); + template + boost::shared_ptr + composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); /** * find the minimum spanning tree using boost graph library */ - template - PredecessorMap findMinimumSpanningTree(const G& g) ; + template + PredecessorMap findMinimumSpanningTree(const G& g) ; /** * Split the graph into two parts: one corresponds to the given spanning tree, * and the other corresponds to the rest of the factors */ - template - void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) ; + template + void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) ; } // namespace gtsam diff --git a/inference/inference-inl.h b/inference/inference-inl.h index 3b18d56d6..4698624b5 100644 --- a/inference/inference-inl.h +++ b/inference/inference-inl.h @@ -40,12 +40,12 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -template -inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(const FactorGraph& factorGraph) { +template +inline typename FACTORGRAPH::bayesnet_type::shared_ptr Inference::Eliminate(const FACTORGRAPH& factorGraph) { // Create a copy of the factor graph to eliminate in-place - FactorGraph eliminationGraph(factorGraph); - typename FactorGraph::variableindex_type variableIndex(eliminationGraph); + FACTORGRAPH eliminationGraph(factorGraph); + typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph); return Eliminate(eliminationGraph, variableIndex); } @@ -72,36 +72,36 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons //} /* ************************************************************************* */ -template -inline typename FactorGraph::bayesnet_type::shared_ptr -Inference::Eliminate(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex) { +template +inline typename FACTORGRAPH::bayesnet_type::shared_ptr +Inference::Eliminate(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex) { return EliminateUntil(factorGraph, variableIndex.size(), variableIndex); } /* ************************************************************************* */ -template -inline typename FactorGraph::bayesnet_type::shared_ptr -Inference::EliminateUntil(const FactorGraph& factorGraph, Index bound) { +template +inline typename FACTORGRAPH::bayesnet_type::shared_ptr +Inference::EliminateUntil(const FACTORGRAPH& factorGraph, Index bound) { // Create a copy of the factor graph to eliminate in-place - FactorGraph eliminationGraph(factorGraph); - typename FactorGraph::variableindex_type variableIndex(eliminationGraph); + FACTORGRAPH eliminationGraph(factorGraph); + typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph); return EliminateUntil(eliminationGraph, bound, variableIndex); } /* ************************************************************************* */ -template -typename FactorGraph::bayesnet_type::shared_ptr -Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex) { +template +typename FACTORGRAPH::bayesnet_type::shared_ptr +Inference::EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex) { - typename FactorGraph::bayesnet_type::shared_ptr bayesnet(new typename FactorGraph::bayesnet_type); + typename FACTORGRAPH::bayesnet_type::shared_ptr bayesnet(new typename FACTORGRAPH::bayesnet_type); // Eliminate variables one-by-one, updating the eliminated factor graph and // the variable index. for(Index var = 0; var < bound; ++var) { - typename FactorGraph::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var)); + typename FACTORGRAPH::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var)); if(conditional) // Will be NULL if the variable did not appear in the factor graph. bayesnet->push_back(conditional); } @@ -110,9 +110,9 @@ Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename Factor } /* ************************************************************************* */ -template -typename FactorGraph::bayesnet_type::sharedConditional -Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var) { +template +typename FACTORGRAPH::bayesnet_type::sharedConditional +Inference::EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var) { /* This function performs symbolic elimination of a variable, comprising * combining involved factors (analogous to "assembly" in SPQR) followed by @@ -140,14 +140,14 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable tic("EliminateOne"); // Get the factors involving the eliminated variable - typename FactorGraph::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]); - typedef typename FactorGraph::variableindex_type::mapped_factor_type mapped_factor_type; + typename FACTORGRAPH::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]); + typedef typename FACTORGRAPH::variableindex_type::mapped_factor_type mapped_factor_type; if(!varIndexEntry.empty()) { vector removedFactors(varIndexEntry.size()); transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), - boost::lambda::bind(&FactorGraph::variableindex_type::mapped_factor_type::factorIndex, boost::lambda::_1)); + boost::lambda::bind(&FACTORGRAPH::variableindex_type::mapped_factor_type::factorIndex, boost::lambda::_1)); // The new joint factor will be the last one in the factor graph size_t jointFactorIndex = factorGraph.size(); @@ -262,8 +262,8 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable // Join the factors and eliminate the variable from the joint factor tic("EliminateOne: Combine"); - typename FactorGraph::sharedFactor jointFactor( - FactorGraph::Factor::Combine( + typename FACTORGRAPH::sharedFactor jointFactor( + FACTORGRAPH::Factor::Combine( factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); toc("EliminateOne: Combine"); @@ -273,7 +273,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable factorGraph.remove(removedFactorI); } - typename FactorGraph::bayesnet_type::sharedConditional conditional; + typename FACTORGRAPH::bayesnet_type::sharedConditional conditional; tic("EliminateOne: eliminateFirst"); conditional = jointFactor->eliminateFirst(); // Eliminate the first variable in-place toc("EliminateOne: eliminateFirst"); @@ -288,38 +288,38 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable } else { // varIndexEntry.empty() toc("EliminateOne"); - return typename FactorGraph::bayesnet_type::sharedConditional(); + return typename FACTORGRAPH::bayesnet_type::sharedConditional(); } } /* ************************************************************************* */ -template -FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContainer& variables) { +template +FACTORGRAPH Inference::Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables) { // Compute a COLAMD permutation with the marginal variables constrained to the end - typename FactorGraph::variableindex_type varIndex(factorGraph); + typename FACTORGRAPH::variableindex_type varIndex(factorGraph); Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(varIndex, variables)); Permutation::shared_ptr permutationInverse(permutation->inverse()); // Copy and permute the factors varIndex.permute(*permutation); - FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size()); - BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) { - typename FactorGraph::sharedFactor permFactor(new typename FactorGraph::Factor(*factor)); + FACTORGRAPH eliminationGraph; eliminationGraph.reserve(factorGraph.size()); + BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, factorGraph) { + typename FACTORGRAPH::sharedFactor permFactor(new typename FACTORGRAPH::Factor(*factor)); permFactor->permuteWithInverse(*permutationInverse); eliminationGraph.push_back(permFactor); } // Eliminate all variables - typename FactorGraph::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex)); + typename FACTORGRAPH::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex)); // The last conditionals in the eliminated BayesNet contain the marginal for // the variables we want. Undo the permutation as we add the marginal // factors. - FactorGraph marginal; marginal.reserve(variables.size()); - typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin(); + FACTORGRAPH marginal; marginal.reserve(variables.size()); + typename FACTORGRAPH::bayesnet_type::const_reverse_iterator conditional = bn->rbegin(); for(Index j=0; jpermuteWithInverse(*permutation); marginal.push_back(factor); assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end()); @@ -330,8 +330,8 @@ FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContain } /* ************************************************************************* */ -template -Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast) { +template +Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) { size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */ @@ -344,9 +344,9 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va p[0] = 0; int count = 0; for(Index var = 0; var < variableIndex.size(); ++var) { - const typename VariableIndexType::mapped_type& column(variableIndex[var]); + const typename VARIABLEINDEXTYPE::mapped_type& column(variableIndex[var]); size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(const typename VariableIndexType::mapped_factor_type& factor_pos, column) { + BOOST_FOREACH(const typename VARIABLEINDEXTYPE::mapped_factor_type& factor_pos, column) { if(lastFactorId != numeric_limits::max()) assert(factor_pos.factorIndex > lastFactorId); A[count++] = factor_pos.factorIndex; // copy sparse column diff --git a/inference/inference.h b/inference/inference.h index bc135f998..1dc408155 100644 --- a/inference/inference.h +++ b/inference/inference.h @@ -40,8 +40,8 @@ namespace gtsam { * Eliminate a factor graph in its natural ordering, i.e. eliminating * variables in order starting from 0. */ - template - static typename FactorGraph::bayesnet_type::shared_ptr Eliminate(const FactorGraph& factorGraph); + template + static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(const FACTORGRAPH& factorGraph); /** * Eliminate a factor graph in its natural ordering, i.e. eliminating @@ -56,34 +56,34 @@ namespace gtsam { * variables in order starting from 0. Uses an existing * variable index instead of building one from scratch. */ - template - static typename FactorGraph::bayesnet_type::shared_ptr Eliminate( - FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex); + template + static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate( + FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex); /** * Partially eliminate a factor graph, up to but not including the given * variable. */ - template - static typename FactorGraph::bayesnet_type::shared_ptr - EliminateUntil(const FactorGraph& factorGraph, Index bound); + template + static typename FACTORGRAPH::bayesnet_type::shared_ptr + EliminateUntil(const FACTORGRAPH& factorGraph, Index bound); /** * Partially eliminate a factor graph, up to but not including the given * variable. Use an existing variable index instead of building one from * scratch. */ - template - static typename FactorGraph::bayesnet_type::shared_ptr - EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex); + template + static typename FACTORGRAPH::bayesnet_type::shared_ptr + EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex); /** * Eliminate a single variable, updating an existing factor graph and * variable index. */ - template - static typename FactorGraph::bayesnet_type::sharedConditional - EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var); + template + static typename FACTORGRAPH::bayesnet_type::sharedConditional + EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var); /** * Eliminate a single variable, updating an existing factor graph and @@ -101,24 +101,24 @@ namespace gtsam { * BayesTree which supports efficiently computing marginals for multiple * variables. */ - template - static FactorGraph Marginal(const FactorGraph& factorGraph, const VarContainer& variables); + template + static FACTORGRAPH Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables); /** * Compute a permutation (variable ordering) using colamd */ - template - static boost::shared_ptr PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector()); } - template - static boost::shared_ptr PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast); + template + static boost::shared_ptr PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector()); } + template + static boost::shared_ptr PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast); // /** // * Join several factors into one. This involves determining the set of // * shared variables and the correct variable positions in the new joint // * factor. // */ -// template -// static typename FactorGraph::shared_factor Combine(const FactorGraph& factorGraph, +// template +// static typename FACTORGRAPH::shared_factor Combine(const FACTORGRAPH& factorGraph, // InputIterator indicesBegin, InputIterator indicesEnd);