diff --git a/gtsam/inference/ConditionalUnordered.h b/gtsam/inference/ConditionalUnordered.h new file mode 100644 index 000000000..1a7e28012 --- /dev/null +++ b/gtsam/inference/ConditionalUnordered.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Conditional.h + * @brief Base class for conditional densities + * @author Frank Dellaert + */ + +// \callgraph +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /** + * Base class for conditional densities, templated on KEY type. This class + * provides storage for the keys involved in a conditional, and iterators and + * access to the frontal and separator keys. + * + * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer + * to the associated factor type and shared_ptr type of the derived class. See + * IndexConditional and GaussianConditional for examples. + * \nosubgrouping + */ + template + class ConditionalUnordered { + + protected: + + /** The first nrFrontal variables are frontal and the rest are parents. */ + size_t nrFrontals_; + + /** Iterator over keys */ + typedef typename FACTOR::iterator iterator; + + /** Const iterator over keys */ + typedef typename FACTOR::const_iterator const_iterator; + + public: + + typedef ConditionalUnordered This; + + /** View of the frontal keys (call frontals()) */ + typedef boost::iterator_range Frontals; + + /** View of the separator keys (call parents()) */ + typedef boost::iterator_range Parents; + + /// @name Standard Constructors + /// @{ + + /** Empty Constructor to make serialization possible */ + ConditionalUnordered() : nrFrontals_(0) {} + + /** Constructor */ + ConditionalUnordered(size_t nrFrontals) : nrFrontals_(nrFrontals) {} + + /// @} + /// @name Testable + /// @{ + + /** print with optional formatter */ + void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** check equality */ + bool equals(const This& c, double tol = 1e-9) const { return nrFrontals_ == c.nrFrontals_; } + + /// @} + /// @name Standard Interface + /// @{ + + /** return the number of frontals */ + size_t nrFrontals() const { return nrFrontals_; } + + /** return the number of parents */ + size_t nrParents() const { return asDerived.size() - nrFrontals_; } + + /** Convenience function to get the first frontal key */ + Key firstFrontalKey() const { + if(nrFrontals_ > 0) + return asDerived().front(); + else + throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys"); + } + + /** return a view of the frontal keys */ + Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); } + + /** return a view of the parent keys */ + Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); } + + /** Iterator pointing to first frontal key. */ + const_iterator beginFrontals() const { return asDerived().begin(); } + + /** Iterator pointing past the last frontal key. */ + const_iterator endFrontals() const { return asDerived().begin() + nrFrontals_; } + + /** Iterator pointing to the first parent key. */ + const_iterator beginParents() const { return endFrontals(); } + + /** Iterator pointing past the last parent key. */ + const_iterator endParents() const { return asDerived().end(); } + + /// @} + /// @name Advanced Interface + /// @{ + + /** Mutable iterators and accessors */ + iterator beginFrontals() { + return FactorType::begin(); + } ///(*this); } + + // Cast to derived type (const) + const FACTOR& asDerived() const { return static_cast(*this); } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(nrFrontals_); + } + + /// @} + + }; + + /* ************************************************************************* */ + template + void ConditionalUnordered::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << " P("; + BOOST_FOREACH(Key key, frontals()) + std::cout << " " << formatter(key); + if (nrParents() > 0) + std::cout << " |"; + BOOST_FOREACH(Key parent, parents()) + std::cout << " " << formatter(parent); + std::cout << ")" << std::endl; + } + +} // gtsam diff --git a/gtsam/inference/EliminationTreeUnordered-inl.h b/gtsam/inference/EliminationTreeUnordered-inl.h new file mode 100644 index 000000000..e58476329 --- /dev/null +++ b/gtsam/inference/EliminationTreeUnordered-inl.h @@ -0,0 +1,222 @@ +/* ---------------------------------------------------------------------------- + +* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* Atlanta, Georgia 30332-0415 +* All Rights Reserved +* Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +* See LICENSE for the license information + +* -------------------------------------------------------------------------- */ + +/** +* @file EliminationTree-inl.h +* @author Frank Dellaert +* @author Richard Roberts +* @date Oct 13, 2010 +*/ +#pragma once + +#include +#include + +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + EliminationTreeUnordered::EliminationTreeUnordered(const FactorGraphType& graph, + const VariableIndexUnordered& structure, const std::vector& order) + { + gttic(ET_Create1); + + // Number of factors and variables - NOTE in the case of partial elimination, n here may + // be fewer variables than are actually present in the graph. + const size_t m = graph.size(); + const size_t n = order.size(); + + static const size_t none = std::numeric_limits::max(); + + // Allocate result parent vector and vector of last factor columns + std::vector nodes(n); + std::vector parents(n, none); + std::vector prevCol(m, none); + std::vector factorUsed(m, false); + + try { + // for column j \in 1 to n do + for (size_t j = 0; j < n; j++) + { + // Retrieve the factors involving this variable and create the current node + const VariableIndex::Factors& factors = structure[order[j]]; + nodes[j] = boost::make_shared >(order[j]); + + // for row i \in Struct[A*j] do + BOOST_FOREACH(const size_t i, factors) { + // If we already hit a variable in this factor, make the subtree containing the previous + // variable in this factor a child of the current node. This means that the variables + // eliminated earlier in the factor depend on the later variables in the factor. If we + // haven't yet hit a variable in this factor, we add the factor to the current node. + // TODO: Store root shortcuts instead of parents. + if (prevCol[i] != none) { + size_t k = prevCol[i]; + // Find root r of the current tree that contains k. Use raw pointers in computing the + // parents to avoid changing the reference counts while traversing up the tree. + size_t r = k; + while (parents[r] != none) + r = parents[r]; + // If the root of the subtree involving this node is actually the current node, + // TODO: what does this mean? forest? + if (r != j) { + // Now that we found the root, hook up parent and child pointers in the nodes. + parents[r] = j; + nodes[j]->subTrees_.push_back(nodes[r]); + } + } else { + // Add the current factor to the current node since we are at the first variable in this + // factor. + nodes[j]->factors_.push_back(graph[i]); + factorUsed[i] = true; + } + prevCol[i] = j; + } + } + } catch(std::invalid_argument& e) { + // If this is thrown from structure[order[j]] above, it means that it was requested to + // eliminate a variable not present in the graph, so throw a more informative error message. + throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph"); + } catch(...) { + throw; + } + + // Find roots + assert(parents.back() == none); // We expect the last-eliminated node to be a root no matter what + for(size_t j = 0; j < n; ++j) + if(parents[j] == none) + roots_.push_back(nodes[j]); + + // Gather remaining factors + for(size_t i = 0; i < m; ++i) + if(!factorUsed[i]) + remainingFactors_.push_back(graph[i]); + } + + /* ************************************************************************* */ + template + EliminationTreeUnordered::EliminationTreeUnordered( + const FactorGraphType& factorGraph, const std::vector& order) + { + gttic(ET_Create2); + // Build variable index first + const VariableIndexUnordered variableIndex(factorGraph); + This temp(factorGraph, variableIndex, order); + roots_.swap(temp.roots_); // Swap in the tree, and temp will be deleted + remainingFactors_.swap(temp.remainingFactors_); + } + + /* ************************************************************************* */ + namespace { + template + struct EliminationNode { + bool expanded; + Key key; + std::vector > factors; + EliminationNode* parent; + template EliminationNode( + Key _key, size_t nFactorsToReserve, ITERATOR firstFactor, ITERATOR lastFactor, EliminationNode* _parent) : + expanded(false), key(_key), parent(_parent) { + factors.reserve(nFactorsToReserve); + factors.insert(factors.end(), firstFactor, lastFactor); + } + }; + } + + /* ************************************************************************* */ + template + std::pair, boost::shared_ptr > + EliminationTreeUnordered::eliminate(Eliminate function) + { + // Stack for eliminating nodes. We use this stack instead of recursive function calls to + // avoid call stack overflow due to very long trees that arise from chain-like graphs. We use + // an std::vector for storage here since we do not want frequent reallocations and do not care + // about the vector growing to be very large once and not being deallocated until this + // function exits, because in the worst case we only store one pointer in this stack for each + // variable in the system. + typedef EliminationNode EliminationNode; + std::stack > eliminationStack; + + // Create empty Bayes net and factor graph to hold result + boost::shared_ptr bayesNet = boost::make_shared(); + // Initialize remaining factors with the factors remaining from creation of the + // EliminationTree - these are the factors that were not included in the partial elimination + // at all. + boost::shared_ptr remainingFactors = + boost::make_shared(remainingFactors_); + + // Add roots to the stack + BOOST_FOREACH(const sharedNode& root, roots_) { + eliminationStack.push( + EliminationNode(root->key, root->factors.size() + root->subTrees.size(), + root->factors.begin(), root->factors.end(), 0)); } + + // Until the stack is empty + while(!eliminationStack.empty()) { + // Process the next node. If it has children, add its children to the stack and skip it - + // we'll come back and eliminate it later after the children have been processed. If it has + // no children, we can eliminate it immediately and remove it from the stack. + EliminationNode& node = nodeStack.top(); + if(node.expanded) { + // Remove from stack + nodeStack.pop(); + // Do a dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + function(node.factors, node.key); + // Add conditional to BayesNet and remaining factor to parent + bayesNet->push_back(eliminationResult.first); + // TODO: Don't add null factor? + if(node.parent) + node.parent->factors.push_back(eliminationResult.second); + else + remainingFactors->push_back(eliminationResult.second); + } else { + // Expand children and mark as expanded + node.expanded = true; + BOOST_FOREACH(const sharedNode& child, node.subTrees) { + nodeStack.push( + EliminationNode(child->key, child->factors.size() + child->subTrees.size(), + child->factors.begin(), child->factors.end(), 0)); } + } + } + + // Return results + return std::make_pair(bayesNet, remainingFactors); + } + + /* ************************************************************************* */ + template + void EliminationTreeUnordered::print(const std::string& name, + const IndexFormatter& formatter) const { + std::cout << name << " (" << formatter(key_) << ")" << std::endl; + BOOST_FOREACH(const sharedFactor& factor, factors_) { + factor->print(name + " ", formatter); } + BOOST_FOREACH(const shared_ptr& child, subTrees_) { + child->print(name + " ", formatter); } + } + + /* ************************************************************************* */ + template + bool EliminationTreeUnordered::equals(const This& expected, double tol) const { + if(this->key_ == expected.key_ && this->factors_ == expected.factors_ + && this->subTrees_.size() == expected.subTrees_.size()) { + typename SubTrees::const_iterator this_subtree = this->subTrees_.begin(); + typename SubTrees::const_iterator expected_subtree = expected.subTrees_.begin(); + while(this_subtree != this->subTrees_.end()) + if( ! (*(this_subtree++))->equals(**(expected_subtree++), tol)) + return false; + return true; + } else + return false; + } + +} diff --git a/gtsam/inference/EliminationTreeUnordered.cpp b/gtsam/inference/EliminationTreeUnordered.cpp new file mode 100644 index 000000000..93c31b23e --- /dev/null +++ b/gtsam/inference/EliminationTreeUnordered.cpp @@ -0,0 +1,27 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EliminationTree.cpp + * @author Frank Dellaert + * @author Richard Roberts + * @date Oct 13, 2010 + */ + +#include + +namespace gtsam { + namespace internal { + + } + +} + diff --git a/gtsam/inference/EliminationTreeUnordered.h b/gtsam/inference/EliminationTreeUnordered.h new file mode 100644 index 000000000..90401f5ce --- /dev/null +++ b/gtsam/inference/EliminationTreeUnordered.h @@ -0,0 +1,150 @@ +/* ---------------------------------------------------------------------------- + +* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* Atlanta, Georgia 30332-0415 +* All Rights Reserved +* Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +* See LICENSE for the license information + +* -------------------------------------------------------------------------- */ + +/** +* @file EliminationTree.h +* @author Frank Dellaert +* @author Richard Roberts +* @date Oct 13, 2010 +*/ +#pragma once + +#include +#include + +#include +#include +#include + +class EliminationTreeTester; // for unit tests, see testEliminationTree + +namespace gtsam { + + class VariableIndexUnordered; + + /** + * An elimination tree is a data structure used intermediately during + * elimination. In future versions it will be used to save work between + * multiple eliminations. + * + * When a variable is eliminated, a new factor is created by combining that + * variable's neighboring factors. The new combined factor involves the combined + * factors' involved variables. When the lowest-ordered one of those variables + * is eliminated, it consumes that combined factor. In the elimination tree, + * that lowest-ordered variable is the parent of the variable that was eliminated to + * produce the combined factor. This yields a tree in general, and not a chain + * because of the implicit sparse structure of the resulting Bayes net. + * + * This structure is examined even more closely in a JunctionTree, which + * additionally identifies cliques in the chordal Bayes net. + * \nosubgrouping + */ + template + class EliminationTreeUnordered { + + public: + + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef EliminationTreeUnordered This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR + typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals + typedef typename GRAPH::Eliminate Eliminate; ///< Typedef for an eliminate subroutine + + class Node { + public: + typedef boost::shared_ptr shared_ptr; + typedef FastList Factors; + typedef FastList SubTrees; + + Key key; ///< key associated with root + Factors factors; ///< factors associated with root + SubTrees subTrees; ///< sub-trees + }; + + typedef Node::shared_ptr sharedNode; ///< Shared pointer to Node + + private: + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + + FastList roots_; + FactorGraphType remainingFactors_; + + public: + + /// @name Standard Constructors + /// @{ + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + EliminationTreeUnordered(const FactorGraphType& factorGraph, + const VariableIndexUnordered& structure, const std::vector& order); + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector& order); + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + EliminationTreeUnordered(const This& other); + + /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + This& operator=(const This& other); + + /// @} + /// @name Standard Interface + /// @{ + + /** Eliminate the factors to a Bayes net and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes net and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(Eliminate function) const; + + /// @} + /// @name Testable + /// @{ + + /** Print the tree to cout */ + void print(const std::string& name = "EliminationTree: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + /// @} + + private: + + /// Allow access to constructor and add methods for testing purposes + friend class ::EliminationTreeTester; + + }; + +} + +#include diff --git a/gtsam/inference/FactorGraphUnordered-inl.h b/gtsam/inference/FactorGraphUnordered-inl.h new file mode 100644 index 000000000..4d45ecde8 --- /dev/null +++ b/gtsam/inference/FactorGraphUnordered-inl.h @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FactorGraph-inl.h + * @brief Factor Graph Base Class + * @author Carlos Nieto + * @author Frank Dellaert + * @author Alireza Fathi + * @author Michael Kaess + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void FactorGraphUnordered::print(const std::string& s, + const KeyFormatter& 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); + } + } + + /* ************************************************************************* */ + template + bool FactorGraphUnordered::equals(const This& fg, double tol) const { + /** check whether the two factor graphs have the same number of factors_ */ + if (factors_.size() != fg.size()) return false; + + /** check whether the factors_ are the same */ + for (size_t i = 0; i < factors_.size(); i++) { + // TODO: Doesn't this force order of factor insertion? + sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; + if (f1 == NULL && f2 == NULL) continue; + if (f1 == NULL || f2 == NULL) return false; + if (!f1->equals(*f2, tol)) return false; + } + return true; + } + + /* ************************************************************************* */ + template + size_t FactorGraphUnordered::nrFactors() const { + size_t size_ = 0; + BOOST_FOREACH(const sharedFactor& factor, factors_) + if (factor) size_++; + return size_; + } + + /* ************************************************************************* */ + template + FastSet FactorGraphUnordered::keys() const { + FastSet allKeys; + BOOST_FOREACH(const sharedFactor& factor, factors_) + if (factor) + allKeys.insert(factor->begin(), factor->end()); + return allKeys; + } + + /* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h new file mode 100644 index 000000000..e7d2aa2fa --- /dev/null +++ b/gtsam/inference/FactorGraphUnordered.h @@ -0,0 +1,266 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FactorGraph.h + * @brief Factor Graph Base Class + * @author Carlos Nieto + * @author Christian Potthast + * @author Michael Kaess + */ + +// \callgraph + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + + /** + * A factor graph is a bipartite graph with factor nodes connected to variable nodes. + * In this class, however, only factor nodes are kept around. + * \nosubgrouping + */ + template + class FactorGraphUnordered { + + public: + + typedef FACTOR FactorType; ///< factor type + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + + typedef FactorGraphUnordered This; ///< Typedef for this class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer for this class + typedef typename std::vector::iterator iterator; + typedef typename std::vector::const_iterator const_iterator; + + /** typedef for elimination result */ + typedef std::pair EliminationResult; + + /** typedef for an eliminate subroutine */ + typedef boost::function Eliminate; + + protected: + + /** concept check, makes sure FACTOR defines print and equals */ + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + + /** Collection of factors */ + std::vector factors_; + + public: + + /// @name Standard Constructor + /// @{ + + /** Default constructor */ + FactorGraphUnordered() {} + + /// @} + /// @name Advanced Constructors + /// @{ + + // TODO: are these needed? + + ///** + // * @brief Constructor from a Bayes net + // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor + // * @return a factor graph with all the conditionals, as factors + // */ + //template + //FactorGraph(const BayesNet& bayesNet); + + ///** convert from Bayes tree */ + //template + //FactorGraph(const BayesTree& bayesTree); + + ///** convert from a derived type */ + //template + //FactorGraph(const FactorGraph& factors) { + // factors_.assign(factors.begin(), factors.end()); + //} + + /// @} + /// @name Adding Factors + /// @{ + + /** + * Reserve space for the specified number of factors if you know in + * advance how many there will be (works like std::vector::reserve). + */ + void reserve(size_t size) { factors_.reserve(size); } + + // TODO: are these needed? + + /** Add a factor */ + template + void push_back(const boost::shared_ptr& factor) { + factors_.push_back(boost::shared_ptr(factor)); + } + + /** push back many factors */ + void push_back(const This& factors) { + factors_.insert(end(), factors.begin(), factors.end()); + } + + /** push back many factors with an iterator */ + template + void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { + factors_.insert(end(), firstFactor, lastFactor); + } + + /** + * @brief Add a vector of derived factors + * @param factors to add + */ + //template + //void push_back(const std::vector >& factors) { + // factors_.insert(end(), factors.begin(), factors.end()); + //} + + /// @} + /// @name Testable + /// @{ + + /** print out graph */ + void print(const std::string& s = "FactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** Check equality */ + bool equals(const This& fg, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** return the number of factors (including any null factors set by remove() ). */ + size_t size() const { return factors_.size(); } + + /** Check if the graph is empty (null factors set by remove() will cause this to return false). */ + bool empty() const { return factors_.empty(); } + + /** Get a specific factor by index (this checks array bounds and may throw an exception, as + * opposed to operator[] which does not). + */ + const sharedFactor at(size_t i) const { return factors_.at(i); } + + /** Get a specific factor by index (this checks array bounds and may throw an exception, as + * opposed to operator[] which does not). + */ + sharedFactor& at(size_t i) { return factors_.at(i); } + + /** Get a specific factor by index (this does not check array bounds, as opposed to at() which + * does). + */ + const sharedFactor operator[](size_t i) const { return at(i); } + + /** Get a specific factor by index (this does not check array bounds, as opposed to at() which + * does). + */ + sharedFactor& operator[](size_t i) { return at(i); } + + /** Iterator to beginning of factors. */ + const_iterator begin() const { return factors_.begin();} + + /** Iterator to end of factors. */ + const_iterator end() const { return factors_.end(); } + + /** Get the first factor */ + sharedFactor front() const { return factors_.front(); } + + /** Get the last factor */ + sharedFactor back() const { return factors_.back(); } + + ///** Eliminate the first \c n frontal variables, returning the resulting + // * conditional and remaining factor graph - this is very inefficient for + // * eliminating all variables, to do that use EliminationTree or + // * JunctionTree. + // */ + //std::pair > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const; + // + ///** Factor the factor graph into a conditional and a remaining factor graph. Given the factor + // * graph \f$ f(X) \f$, and \c variables to factorize out \f$ V \f$, this function factorizes + // * into \f$ f(X) = f(V;Y)f(Y) \f$, where \f$ Y := X \backslash V \f$ are the remaining + // * variables. If \f$ f(X) = p(X) \f$ is a probability density or likelihood, the factorization + // * produces a conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + // * + // * For efficiency, this function treats the variables to eliminate + // * \c variables as fully-connected, so produces a dense (fully-connected) + // * conditional on all of the variables in \c variables, instead of a sparse BayesNet. If the + // * variables are not fully-connected, it is more efficient to sequentially factorize multiple + // * times. + // */ + //std::pair > eliminate( + // const std::vector& variables, const Eliminate& eliminateFcn, + // boost::optional variableIndex = boost::none) const; + + ///** Eliminate a single variable, by calling FactorGraph::eliminate. */ + //std::pair > eliminateOne( + // KeyType variable, const Eliminate& eliminateFcn, + // boost::optional variableIndex = boost::none) const { + // std::vector variables(1, variable); + // return eliminate(variables, eliminateFcn, variableIndex); + //} + + /// @} + /// @name Modifying Factor Graphs (imperative, discouraged) + /// @{ + + /** non-const STL-style begin() */ + iterator begin() { return factors_.begin();} + + /** non-const STL-style end() */ + iterator end() { return factors_.end(); } + + /** Directly resize the number of factors in the graph. If the new size is less than the + * original, factors at the end will be removed. If the new size is larger than the original, + * null factors will be appended. + */ + void resize(size_t size) { factors_.resize(size); } + + /** delete factor without re-arranging indexes by inserting a NULL pointer */ + void remove(size_t i) { factors_[i].reset();} + + /** replace a factor by index */ + void replace(size_t index, sharedFactor factor) { at(index) = factor; } + + /// @} + /// @name Advanced Interface + /// @{ + + /** return the number of non-null factors */ + size_t nrFactors() const; + + /** Potentially very slow function to return all keys involved */ + FastSet keys() const; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(factors_); + } + + /// @} + + }; // FactorGraph + +} // namespace gtsam + +#include diff --git a/gtsam/inference/FactorUnordered.cpp b/gtsam/inference/FactorUnordered.cpp new file mode 100644 index 000000000..d6b92bf51 --- /dev/null +++ b/gtsam/inference/FactorUnordered.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Factor.cpp + * @brief The base class for all factors + * @author Kai Ni + * @author Frank Dellaert + * @author Richard Roberts + */ + +// \callgraph + +#include + +namespace gtsam { + + /* ************************************************************************* */ + void FactorUnordered::print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const + { + return this->printKeys(s, formatter); + } + + /* ************************************************************************* */ + void FactorUnordered::printKeys(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << " "; + BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); + std::cout << std::endl; + } + +} \ No newline at end of file diff --git a/gtsam/inference/FactorUnordered.h b/gtsam/inference/FactorUnordered.h new file mode 100644 index 000000000..f263e72d6 --- /dev/null +++ b/gtsam/inference/FactorUnordered.h @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Factor.h + * @brief The base class for all factors + * @author Kai Ni + * @author Frank Dellaert + * @author Richard Roberts + */ + +// \callgraph + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + +/** + * This is the base class for all factor types. It is templated on a KEY type, + * which will be the type used to label variables. Key types currently in use + * in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and + * Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph), + * and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph). + * though currently only IndexFactor and IndexConditional derive from this + * class, using Index keys. This class does not store any data other than its + * keys. Derived classes store data such as matrices and probability tables. + * + * Note that derived classes *must* redefine the ConditionalType and shared_ptr + * typedefs to refer to the associated conditional and shared_ptr types of the + * derived class. See IndexFactor, JacobianFactor, etc. for examples. + * + * This class is \b not virtual for performance reasons - derived symbolic classes, + * IndexFactor and IndexConditional, need to be created and destroyed quickly + * during symbolic elimination. GaussianFactor and NonlinearFactor are virtual. + * \nosubgrouping + */ +class FactorUnordered { + +public: + + typedef FactorUnordered This; ///< This class + + /// A shared_ptr to this class, derived classes must redefine this. + typedef boost::shared_ptr shared_ptr; + + /// Iterator over keys + typedef std::vector::iterator iterator; + + /// Const iterator over keys + typedef std::vector::const_iterator const_iterator; + +protected: + + /// The keys involved in this factor + std::vector keys_; + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor for I/O */ + FactorUnordered() {} + + /** Construct unary factor */ + FactorUnordered(Key key) : keys_(1) { keys_[0] = key; } + + /** Construct binary factor */ + FactorUnordered(Key key1, Key key2) : keys_(2) { keys_[0] = key1; keys_[1] = key2; } + + /** Construct ternary factor */ + FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; } + + /** Construct 4-way factor */ + FactorUnordered(Key key1, Key key2, Key key3, Key key4) : keys_(4) { + keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; } + + /** Construct 5-way factor */ + FactorUnordered(Key key1, Key key2, Key key3, Key key4, Key key5) : keys_(5) { + keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; } + + /** Construct 6-way factor */ + FactorUnordered(Key key1, Key key2, Key key3, Key key4, Key key5, Key key6) : keys_(6) { + keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; } + + /// @} + + + /// @name Advanced Constructors + /// @{ + + /** Construct n-way factor from container of keys. */ + template + static FactorUnordered FromKeys(const CONTAINER& keys) { return FromIterator(keys.begin(), keys.end()); } + + /** Construct n-way factor from iterator over keys. */ + template static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) { + FactorUnordered result; result.keys_.assign(first, last); } + + /// @} + + + /// @name Standard Interface + /// @{ + + /// First key + Key front() const { return keys_.front(); } + + /// Last key + Key back() const { return keys_.back(); } + + /// find + const_iterator find(Key key) const { return std::find(begin(), end(), key); } + + /// Access the factor's involved variable keys + const std::vector& keys() const { return keys_; } + + /** Iterator at beginning of involved variable keys */ + const_iterator begin() const { return keys_.begin(); } + + /** Iterator at end of involved variable keys */ + const_iterator end() const { return keys_.end(); } + + /** + * @return the number of variables involved in this factor + */ + size_t size() const { return keys_.size(); } + + /// @} + + + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// print only keys + void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// check equality + bool equals(const This& other, double tol = 1e-9) const; + + /// @} + + + /// @name Advanced Interface + /// @{ + + /** @return keys involved in this factor */ + std::vector& keys() { return keys_; } + + /** Iterator at beginning of involved variable keys */ + iterator begin() { return keys_.begin(); } + + /** Iterator at end of involved variable keys */ + iterator end() { return keys_.end(); } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(keys_); + } + + /// @} + +}; + +} diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/inference/Key.cpp similarity index 100% rename from gtsam/nonlinear/Key.cpp rename to gtsam/inference/Key.cpp diff --git a/gtsam/nonlinear/Key.h b/gtsam/inference/Key.h similarity index 100% rename from gtsam/nonlinear/Key.h rename to gtsam/inference/Key.h diff --git a/gtsam/inference/OrderingUnordered.cpp b/gtsam/inference/OrderingUnordered.cpp new file mode 100644 index 000000000..4ab3a2b75 --- /dev/null +++ b/gtsam/inference/OrderingUnordered.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Ordering.cpp + * @author Richard Roberts + * @date Sep 2, 2010 + */ + +#include "Ordering.h" + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +Ordering::Ordering(const std::list & L) { + int i = 0; + BOOST_FOREACH( Key s, L ) + insert(s, i++) ; +} + +/* ************************************************************************* */ +Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) { + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; +} + +/* ************************************************************************* */ +Ordering& Ordering::operator=(const Ordering& rhs) { + order_ = rhs.order_; + orderingIndex_.resize(rhs.size()); + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; + return *this; +} + +/* ************************************************************************* */ +void Ordering::permuteInPlace(const Permutation& permutation) { + gttic(Ordering_permuteInPlace); + // Allocate new index and permute in map iterators + OrderingIndex newIndex(permutation.size()); + for(size_t j = 0; j < newIndex.size(); ++j) { + newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator + newIndex[j]->second = j; // Change the stored index to its permuted value + } + // Swap the new index into this Ordering class + orderingIndex_.swap(newIndex); +} + +/* ************************************************************************* */ +void Ordering::permuteInPlace(const Permutation& selector, const Permutation& permutation) { + if(selector.size() != permutation.size()) + throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes."); + // Create new index the size of the permuted entries + OrderingIndex newIndex(selector.size()); + // Permute the affected entries into the new index + for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot) + newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]]; + // Put the affected entries back in the new order and fix the indices + for(size_t slot = 0; slot < selector.size(); ++slot) { + orderingIndex_[selector[slot]] = newIndex[slot]; + orderingIndex_[selector[slot]]->second = selector[slot]; + } +} + +/* ************************************************************************* */ +void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const { + cout << str; + // Print ordering in index order + // Print the ordering with varsPerLine ordering entries printed on each line, + // for compactness. + static const size_t varsPerLine = 10; + bool endedOnNewline = false; + BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) { + if(index_key->second % varsPerLine != 0) + cout << ", "; + cout << index_key->second<< ":" << keyFormatter(index_key->first); + if(index_key->second % varsPerLine == varsPerLine - 1) { + cout << "\n"; + endedOnNewline = true; + } else { + endedOnNewline = false; + } + } + if(!endedOnNewline) + cout << "\n"; + cout.flush(); +} + +/* ************************************************************************* */ +bool Ordering::equals(const Ordering& rhs, double tol) const { + return order_ == rhs.order_; +} + +/* ************************************************************************* */ +Ordering::value_type Ordering::pop_back() { + iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry + value_type removed = *lastItem; // Save the key-index pair to return + order_.erase(lastItem); // Erase the entry from the map + orderingIndex_.pop_back(); // Erase the entry from the index + return removed; // Return the removed item +} + +/* ************************************************************************* */ +void Unordered::print(const string& s) const { + cout << s << " (" << size() << "):"; + BOOST_FOREACH(Index key, *this) + cout << " " << key; + cout << endl; +} + +/* ************************************************************************* */ +bool Unordered::equals(const Unordered &other, double tol) const { + return *this == other; +} + +} diff --git a/gtsam/inference/OrderingUnordered.h b/gtsam/inference/OrderingUnordered.h new file mode 100644 index 000000000..ad32443d6 --- /dev/null +++ b/gtsam/inference/OrderingUnordered.h @@ -0,0 +1,271 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Ordering.h + * @author Richard Roberts + * @date Sep 2, 2010 + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +/** + * An ordering is a map from symbols (non-typed keys) to integer indices + * \nosubgrouping + */ +class GTSAM_EXPORT Ordering { +protected: + typedef FastMap Map; + typedef std::vector OrderingIndex; + Map order_; + OrderingIndex orderingIndex_; + +public: + + typedef boost::shared_ptr shared_ptr; + + typedef std::pair value_type; + typedef Map::iterator iterator; + typedef Map::const_iterator const_iterator; + + /// @name Standard Constructors + /// @{ + + /// Default constructor for empty ordering + Ordering() {} + + /// Copy constructor + Ordering(const Ordering& other); + + /// Construct from list, assigns order indices sequentially to list items. + Ordering(const std::list & L); + + /// Assignment operator + Ordering& operator=(const Ordering& rhs); + + /// @} + /// @name Standard Interface + /// @{ + + /** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */ + Index size() const { return orderingIndex_.size(); } + + const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ + const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ + + Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const + Key key(Index index) const { + if(index >= orderingIndex_.size()) + throw std::out_of_range("Attempting to access out-of-range index in Ordering"); + else + return orderingIndex_[index]->first; } + + /** Assigns the ordering index of the requested \c key into \c index if the symbol + * is present in the ordering, otherwise does not modify \c index. The + * return value indicates whether the symbol is in fact present in the + * ordering. + * @param key The key whose index you request + * @param [out] index Reference into which to write the index of the requested key, if the key is present. + * @return true if the key is present and \c index was modified, false otherwise. + */ + bool tryAt(Key key, Index& index) const { + const_iterator i = order_.find(key); + if(i != order_.end()) { + index = i->second; + return true; + } else + return false; } + + /// Access the index for the requested key, throws std::out_of_range if the + /// key is not present in the ordering (note that this differs from the + /// behavior of std::map) + Index& operator[](Key key) { + iterator i=order_.find(key); + if(i == order_.end()) throw std::out_of_range( + std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); + else return i->second; } + + /// Access the index for the requested key, throws std::out_of_range if the + /// key is not present in the ordering (note that this differs from the + /// behavior of std::map) + Index operator[](Key key) const { + const_iterator i=order_.find(key); + if(i == order_.end()) throw std::out_of_range( + std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); + else return i->second; } + + /** Returns an iterator pointing to the symbol/index pair with the requested, + * or the end iterator if it does not exist. + * + * @return An iterator pointing to the symbol/index pair with the requested, + * or the end iterator if it does not exist. + */ + iterator find(Key key) { return order_.find(key); } + + /** Returns an iterator pointing to the symbol/index pair with the requested, + * or the end iterator if it does not exist. + * + * @return An iterator pointing to the symbol/index pair with the requested, + * or the end iterator if it does not exist. + */ + const_iterator find(Key key) const { return order_.find(key); } + + /** Insert a key-index pair, but will fail if the key is already present */ + iterator insert(const value_type& key_order) { + std::pair it_ok(order_.insert(key_order)); + if(it_ok.second) { + if(key_order.second >= orderingIndex_.size()) + orderingIndex_.resize(key_order.second + 1); + orderingIndex_[key_order.second] = it_ok.first; + return it_ok.first; + } else + throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); } + + /// Test if the key exists in the ordering. + bool exists(Key key) const { return order_.count(key) > 0; } + + /** Insert a key-index pair, but will fail if the key is already present */ + iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); } + + /// Adds a new key to the ordering with an index of one greater than the current highest index. + Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; } + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Iterator in order of sorted symbols, not in elimination/index order! + */ + iterator begin() { return order_.begin(); } + + /** + * Iterator in order of sorted symbols, not in elimination/index order! + */ + iterator end() { return order_.end(); } + + /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair + * from the ordering (this version is \f$ O(n) \f$, use it when you do not + * know the last-ordered key). + * + * If you already know the last-ordered symbol, call popback(Key) + * that accepts this symbol as an argument. + * + * @return The symbol and index that were removed. + */ + value_type pop_back(); + + /** + * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are + * very useful for unit tests. This functionality is courtesy of + * boost::assign. + */ + inline boost::assign::list_inserter, Key> + operator+=(Key key) { + return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); } + + /** + * Reorder the variables with a permutation. This is typically used + * internally, permuting an initial key-sorted ordering into a fill-reducing + * ordering. + */ + void permuteInPlace(const Permutation& permutation); + + void permuteInPlace(const Permutation& selector, const Permutation& permutation); + + /// Synonym for operator[](Key) + Index& at(Key key) { return operator[](key); } + + /// @} + /// @name Testable + /// @{ + + /** print (from Testable) for testing and debugging */ + void print(const std::string& str = "Ordering:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** equals (from Testable) for testing and debugging */ + bool equals(const Ordering& rhs, double tol = 0.0) const; + + /// @} + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void save(ARCHIVE & ar, const unsigned int version) const + { + ar & BOOST_SERIALIZATION_NVP(order_); + size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators + ar & BOOST_SERIALIZATION_NVP(size_); + } + template + void load(ARCHIVE & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(order_); + size_t size_; + ar & BOOST_SERIALIZATION_NVP(size_); + orderingIndex_.resize(size_); + for(iterator item = order_.begin(); item != order_.end(); ++item) + orderingIndex_[item->second] = item; // Assign the iterators + } + BOOST_SERIALIZATION_SPLIT_MEMBER() +}; // \class Ordering + +/** + * @class Unordered + * @brief a set of unordered indices + */ +class Unordered: public std::set { +public: + /** Default constructor creates empty ordering */ + Unordered() { } + + /** Create from a single symbol */ + Unordered(Index key) { insert(key); } + + /** Copy constructor */ + Unordered(const std::set& keys_in) : std::set(keys_in) {} + + /** whether a key exists */ + bool exists(const Index& key) { return find(key) != end(); } + + // Testable + GTSAM_EXPORT void print(const std::string& s = "Unordered") const; + GTSAM_EXPORT bool equals(const Unordered &t, double tol=0) const; +}; + +// Create an index formatter that looks up the Key in an inverse ordering, then +// formats the key using the provided key formatter, used in saveGraph. +class GTSAM_EXPORT OrderingIndexFormatter { +private: + Ordering ordering_; + KeyFormatter keyFormatter_; +public: + OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : + ordering_(ordering), keyFormatter_(keyFormatter) {} + std::string operator()(Index index) { + return keyFormatter_(ordering_.key(index)); } +}; + +} // \namespace gtsam + diff --git a/gtsam/inference/SymbolicConditionalUnordered.cpp b/gtsam/inference/SymbolicConditionalUnordered.cpp new file mode 100644 index 000000000..8a9d6ba39 --- /dev/null +++ b/gtsam/inference/SymbolicConditionalUnordered.cpp @@ -0,0 +1,24 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicConditional.cpp + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#include + +namespace gtsam { + + using namespace std; + +} diff --git a/gtsam/inference/SymbolicConditionalUnordered.h b/gtsam/inference/SymbolicConditionalUnordered.h new file mode 100644 index 000000000..e2d4809e7 --- /dev/null +++ b/gtsam/inference/SymbolicConditionalUnordered.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicConditional.h + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * SymbolicConditionalUnordered is a conditional with keys but no probability + * data, produced by symbolic elimination of SymbolicFactorUnordered. + * + * It is also a SymbolicFactorUnordered, and thus derives from it. It + * derives also from ConditionalUnordered, which is a generic interface + * class for conditionals. + * \nosubgrouping + */ + class SymbolicConditionalUnordered : public SymbolicFactorUnordered, public ConditionalUnordered { + + public: + + typedef SymbolicConditionalUnordered This; /// Typedef to this class + typedef SymbolicFactorUnordered BaseFactor; /// Typedef to the factor base class + typedef ConditionalUnordered BaseConditional; /// Typedef to the conditional base class + typedef boost::shared_ptr shared_ptr; /// Boost shared_ptr to this class + typedef BaseFactor::iterator iterator; /// iterator to keys + typedef BaseFactor::const_iterator const_iterator; /// const_iterator to keys + + /// @name Standard Constructors + /// @{ + + /** Empty Constructor to make serialization possible */ + SymbolicConditionalUnordered() {} + + /** No parents */ + SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(0) {} + + /** Single parent */ + SymbolicConditionalUnordered(Index j, Index parent) : BaseFactor(j, parent), BaseConditional(1) {} + + /** Two parents */ + SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(2) {} + + /** Three parents */ + SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {} + + /** Named constructor from an arbitrary number of keys and frontals */ + template + static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) : + { + SymbolicConditionalUnordered result; + result.keys_.assign(firstKey, lastKey); + result.nrFrontals_ = nrFrontals; + return result; + } + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; + +} diff --git a/gtsam/inference/SymbolicEliminationTreeUnordered.h b/gtsam/inference/SymbolicEliminationTreeUnordered.h new file mode 100644 index 000000000..3ad76b426 --- /dev/null +++ b/gtsam/inference/SymbolicEliminationTreeUnordered.h @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicEliminationTreeUnordered.h + * @date Mar 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include + +namespace gtsam { + + class SymbolicEliminationTreeUnordered : public EliminationTreeUnordered { + public: + typedef EliminationTreeUnordered Base; ///< Base class + typedef SymbolicEliminationTreeUnordered This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, + const VariableIndexUnordered& structure, const std::vector& order) : + Base(factorGraph, structure, order) {} + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + SymbolicEliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector& order) : + Base(factorGraph, order) {} + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + SymbolicEliminationTreeUnordered(const This& other) : Base(other) {} + + /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + This& operator=(const This& other) { (void) Base::operator=(other); return *this; } + + }; + +} diff --git a/gtsam/inference/SymbolicFactorGraphUnordered.cpp b/gtsam/inference/SymbolicFactorGraphUnordered.cpp new file mode 100644 index 000000000..827ee6196 --- /dev/null +++ b/gtsam/inference/SymbolicFactorGraphUnordered.cpp @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicFactorGraph.cpp + * @date Oct 29, 2009 + * @author Frank Dellaert + */ + +#include + +#include + +namespace gtsam { + + using namespace std; + + /* ************************************************************************* */ + void SymbolicFactorGraphUnordered::push_factor(Key key) { + push_back(boost::make_shared(key)); + } + + /* ************************************************************************* */ + void SymbolicFactorGraphUnordered::push_factor(Key key1, Key key2) { + push_back(boost::make_shared(key1,key2)); + } + + /* ************************************************************************* */ + void SymbolicFactorGraphUnordered::push_factor(Key key1, Key key2, Key key3) { + push_back(boost::make_shared(key1,key2,key3)); + } + + /* ************************************************************************* */ + void SymbolicFactorGraphUnordered::push_factor(Key key1, Key key2, Key key3, Key key4) { + push_back(boost::make_shared(key1,key2,key3,key4)); + } + +// /* ************************************************************************* */ +// std::pair +// SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const +// { +// return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); +// } +// +// /* ************************************************************************* */ +// std::pair +// SymbolicFactorGraph::eliminate(const std::vector& variables) const +// { +// return FactorGraph::eliminate(variables, EliminateSymbolic); +// } +// +// /* ************************************************************************* */ +// std::pair +// SymbolicFactorGraph::eliminateOne(Index variable) const +// { +// return FactorGraph::eliminateOne(variable, EliminateSymbolic); +// } +// +// /* ************************************************************************* */ +// IndexFactor::shared_ptr CombineSymbolic( +// const FactorGraph& factors, const FastMap >& variableSlots) { +// IndexFactor::shared_ptr combined(Combine (factors, variableSlots)); +//// combined->assertInvariants(); +// return combined; +// } +// +// /* ************************************************************************* */ +// pair // +// EliminateSymbolic(const FactorGraph& factors, size_t nrFrontals) { +// +// FastSet keys; +// BOOST_FOREACH(const IndexFactor::shared_ptr& factor, factors) +// BOOST_FOREACH(Index var, *factor) +// keys.insert(var); +// +// if (keys.size() < nrFrontals) throw invalid_argument( +// "EliminateSymbolic requested to eliminate more variables than exist in graph."); +// +// vector newKeys(keys.begin(), keys.end()); +// return make_pair(boost::make_shared(newKeys, nrFrontals), +// boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); +// } + + /* ************************************************************************* */ +} diff --git a/gtsam/inference/SymbolicFactorGraphUnordered.h b/gtsam/inference/SymbolicFactorGraphUnordered.h new file mode 100644 index 000000000..fde935a81 --- /dev/null +++ b/gtsam/inference/SymbolicFactorGraphUnordered.h @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicFactorGraph.h + * @date Oct 29, 2009 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** Symbolic Factor Graph + * \nosubgrouping + */ + class SymbolicFactorGraphUnordered: public FactorGraphUnordered { + + public: + + /// @name Standard Constructors + /// @{ + + /** Construct empty factor graph */ + SymbolicFactorGraphUnordered() {} + + ///** Eliminate the first \c n frontal variables, returning the resulting + // * conditional and remaining factor graph - this is very inefficient for + // * eliminating all variables, to do that use EliminationTree or + // * JunctionTree. Note that this version simply calls + // * FactorGraph::eliminateFrontals with EliminateSymbolic + // * as the eliminate function argument. + // */ + //GTSAM_EXPORT std::pair eliminateFrontals(size_t nFrontals) const; + // + ///** Factor the factor graph into a conditional and a remaining factor graph. + // * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + // * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + // * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + // * a probability density or likelihood, the factorization produces a + // * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + // * + // * For efficiency, this function treats the variables to eliminate + // * \c variables as fully-connected, so produces a dense (fully-connected) + // * conditional on all of the variables in \c variables, instead of a sparse + // * BayesNet. If the variables are not fully-connected, it is more efficient + // * to sequentially factorize multiple times. + // * Note that this version simply calls + // * FactorGraph::eliminate with EliminateSymbolic as the eliminate + // * function argument. + // */ + //GTSAM_EXPORT std::pair eliminate(const std::vector& variables) const; + + ///** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */ + //GTSAM_EXPORT std::pair eliminateOne(Index variable) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// @} + /// @name Advanced Interface + /// @{ + + /** Push back unary factor */ + GTSAM_EXPORT void push_factor(Key key); + + /** Push back binary factor */ + GTSAM_EXPORT void push_factor(Key key1, Key key2); + + /** Push back ternary factor */ + GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3); + + /** Push back 4-way factor */ + GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4); + }; + + /** Create a combined joint factor (new style for EliminationTree). */ + GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph& factors, + const FastMap >& variableSlots); + + /** + * CombineAndEliminate provides symbolic elimination. + * Combine and eliminate can also be called separately, but for this and + * derived classes calling them separately generally does extra work. + */ + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateSymbolic(const FactorGraph&, size_t nrFrontals = 1); + + /// @} + +} // namespace gtsam diff --git a/gtsam/inference/SymbolicFactorUnordered.cpp b/gtsam/inference/SymbolicFactorUnordered.cpp new file mode 100644 index 000000000..5912b389c --- /dev/null +++ b/gtsam/inference/SymbolicFactorUnordered.cpp @@ -0,0 +1,23 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicFactor.cpp + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#include + +using namespace std; + +namespace gtsam { +} // gtsam diff --git a/gtsam/inference/SymbolicFactorUnordered.h b/gtsam/inference/SymbolicFactorUnordered.h new file mode 100644 index 000000000..a347f75e2 --- /dev/null +++ b/gtsam/inference/SymbolicFactorUnordered.h @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicFactor.h + * @author Richard Roberts + * @date Oct 17, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + + // Forward declaration of SymbolicConditional + class SymbolicConditionalUnordered; + + /** + * SymbolicFactorUnordered serves two purposes. It is the base class for all linear + * factors (GaussianFactor, JacobianFactor, HessianFactor), and also functions + * as a symbolic factor, used to do symbolic elimination by JunctionTree. + * + * It derives from Factor with a key type of Key, an unsigned integer. + * \nosubgrouping + */ + class SymbolicFactorUnordered: public FactorUnordered { + + public: + + typedef SymbolicFactorUnordered This; + typedef FactorUnordered Base; + typedef SymbolicConditionalUnordered ConditionalType; + + /** Overriding the shared_ptr typedef */ + typedef boost::shared_ptr shared_ptr; + + /// @name Standard Interface + /// @{ + + /** Virtual destructor */ + virtual ~SymbolicFactorUnordered() {} + + /** Copy constructor */ + SymbolicFactorUnordered(const This& f) : Base(f) {} + + /** Default constructor for I/O */ + SymbolicFactorUnordered() {} + + /** Construct unary factor */ + SymbolicFactorUnordered(Key j) : Base(j) {} + + /** Construct binary factor */ + SymbolicFactorUnordered(Key j1, Key j2) : Base(j1, j2) {} + + /** Construct ternary factor */ + SymbolicFactorUnordered(Key j1, Key j2, Key j3) : Base(j1, j2, j3) {} + + /** Construct 4-way factor */ + SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4) : Base(j1, j2, j3, j4) {} + + /** Construct 5-way factor */ + SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5) : Base(j1, j2, j3, j4, j5) {} + + /** Construct 6-way factor */ + SymbolicFactorUnordered(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : Base(j1, j2, j3, j4, j5, j6) {} + + /// @} + + /// @name Advanced Constructors + /// @{ + + /** Constructor from a collection of keys */ + template SymbolicFactorUnordered(KeyIterator beginKey, KeyIterator endKey) : Base(beginKey, endKey) {} + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + + }; // IndexFactor + +} diff --git a/gtsam/inference/VariableIndexUnordered-inl.h b/gtsam/inference/VariableIndexUnordered-inl.h new file mode 100644 index 000000000..5e06d6686 --- /dev/null +++ b/gtsam/inference/VariableIndexUnordered-inl.h @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VariableIndex-inl.h + * @author Richard Roberts + * @date March 26, 2013 + */ + +#pragma once + +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +void VariableIndexUnordered::augment(const FG& factors) +{ + gttic(VariableIndex_augment); + + // Save original number of factors for keeping track of indices + const size_t originalNFactors = nFactors_; + + // Augment index for each factor + for(size_t i = 0; i < factors.size(); ++i) { + if(factors[i]) { + const size_t globalI = originalNFactors + i; + BOOST_FOREACH(const Key key, factors[i]) { + index_[key].push_back(globalI); + ++ nEntries_; + } + } + ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent + } +} + +/* ************************************************************************* */ +template +void VariableIndexUnordered::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) +{ + gttic(VariableIndex_remove); + + // NOTE: We intentionally do not decrement nFactors_ because the factor + // indices need to remain consistent. Removing factors from a factor graph + // does not shift the indices of other factors. Also, we keep nFactors_ + // one greater than the highest-numbered factor referenced in a VariableIndex. + ITERATOR factorIndex = firstFactor; + size_t i = 0; + for( ; factorIndex != lastFactor; ++factorIndex, ++i) { + if(i >= factors.size()) + throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); + if(factors[i]) { + BOOST_FOREACH(Key j, factors[i]) { + Factors& factorEntries = internalAt(j); + Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[i]); + if(entry == factorEntries.end()) + throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); + factorEntries.erase(entry); + -- nEntries_; + } + } + } +} + +/* ************************************************************************* */ +template +void VariableIndexUnordered::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { + for(ITERATOR key = firstKey; key != lastKey; ++key) { + assert(!internalAt(*key).empty()); + index_.erase(*key); + } +} + +} diff --git a/gtsam/inference/VariableIndexUnordered.cpp b/gtsam/inference/VariableIndexUnordered.cpp new file mode 100644 index 000000000..79eb196f6 --- /dev/null +++ b/gtsam/inference/VariableIndexUnordered.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VariableIndex.cpp + * @author Richard Roberts + * @date March 26, 2013 + */ + +#include + +#include + +namespace gtsam { + +using namespace std; + +/* ************************************************************************* */ +bool VariableIndexUnordered::equals(const VariableIndexUnordered& other, double tol) const { + return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ + && this->index_ == other.index_; +} + +/* ************************************************************************* */ +void VariableIndexUnordered::print(const string& str) const { + cout << str; + cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; + BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + cout << "var " << key_factors.first << ":"; + BOOST_FOREACH(const size_t factor, key_factors.second) + cout << " " << factor; + cout << "\n"; + } + cout.flush(); +} + +/* ************************************************************************* */ +void VariableIndexUnordered::outputMetisFormat(ostream& os) const { + os << size() << " " << nFactors() << "\n"; + // run over variables, which will be hyper-edges. + BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + // every variable is a hyper-edge covering its factors + BOOST_FOREACH(const size_t factor, key_factors.second) + os << (factor+1) << " "; // base 1 + os << "\n"; + } + os << flush; +} + +} diff --git a/gtsam/inference/VariableIndexUnordered.h b/gtsam/inference/VariableIndexUnordered.h new file mode 100644 index 000000000..fb9393b69 --- /dev/null +++ b/gtsam/inference/VariableIndexUnordered.h @@ -0,0 +1,181 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VariableIndex.h + * @author Richard Roberts + * @date March 26, 2013 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * The VariableIndex class computes and stores the block column structure of a + * factor graph. The factor graph stores a collection of factors, each of + * which involves a set of variables. In contrast, the VariableIndex is built + * from a factor graph prior to elimination, and stores the list of factors + * that involve each variable. This information is stored as a deque of + * lists of factor indices. + * \nosubgrouping + */ +class GTSAM_EXPORT VariableIndexUnordered { +public: + + typedef boost::shared_ptr shared_ptr; + typedef FastList Factors; + typedef Factors::iterator Factor_iterator; + typedef Factors::const_iterator Factor_const_iterator; + +protected: + typedef FastMap KeyMap; + KeyMap index_; + size_t nFactors_; // Number of factors in the original factor graph. + size_t nEntries_; // Sum of involved variable counts of each factor. + +public: + typedef KeyMap::const_iterator const_iterator; + typedef KeyMap::const_iterator iterator; + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor, creates an empty VariableIndex */ + VariableIndexUnordered() : nFactors_(0), nEntries_(0) {} + + /** + * Create a VariableIndex that computes and stores the block column structure + * of a factor graph. + */ + template + VariableIndexUnordered(const FG& factorGraph) { augment(factorGraph); } + + /// @} + /// @name Standard Interface + /// @{ + + /** + * The number of variable entries. This is one greater than the variable + * with the highest index. + */ + Index size() const { return index_.size(); } + + /** The number of factors in the original factor graph */ + size_t nFactors() const { return nFactors_; } + + /** The number of nonzero blocks, i.e. the number of variable-factor entries */ + size_t nEntries() const { return nEntries_; } + + /** Access a list of factors by variable */ + const Factors& operator[](Key variable) const { + KeyMap::const_iterator item = index_.find(variable); + if(item == index_.end()) + throw std::invalid_argument("Requested non-existent variable from VariableIndex"); + else + return item->second; + } + + /// @} + /// @name Testable + /// @{ + + /** Test for equality (for unit tests and debug assertions). */ + bool equals(const VariableIndexUnordered& other, double tol=0.0) const; + + /** Print the variable index (for unit tests and debugging). */ + void print(const std::string& str = "VariableIndex: ") const; + + /** + * Output dual hypergraph to Metis file format for use with hmetis + * In the dual graph, variables are hyperedges, factors are nodes. + */ + void outputMetisFormat(std::ostream& os) const; + + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FG& factors); + + /** + * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement + * nFactors_ because the factor indices need to remain consistent. Removing factors from a factor + * graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than + * the highest-numbered factor referenced in a VariableIndex. + * + * @param indices The indices of the factors to remove, which must match \c factors + * @param factors The factors being removed, which must symbolically correspond exactly to the + * factors with the specified \c indices that were added. + */ + template + void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); + + /** Remove unused empty variables at the end of the ordering (in debug mode + * verifies they are empty). + * @param nToRemove The number of unused variables at the end to remove + */ + template + void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); + + /** Iterator to the first variable entry */ + const_iterator begin() const { return index_.begin(); } + + /** Iterator to the first variable entry */ + const_iterator end() const { return index_.end(); } + + /** Find the iterator for the requested variable entry */ + const_iterator find(Key key) const { return index_.find(key); } + +protected: + Factor_iterator factorsBegin(Index variable) { return internalAt(variable).begin(); } + Factor_iterator factorsEnd(Index variable) { return internalAt(variable).end(); } + + Factor_const_iterator factorsBegin(Index variable) const { return internalAt(variable).begin(); } + Factor_const_iterator factorsEnd(Index variable) const { return internalAt(variable).end(); } + + /// Internal version of 'at' that asserts existence + const Factors& internalAt(Key variable) const { + const KeyMap::const_iterator item = index_.find(variable); + assert(item != index_.end()); + return item->second; } + + /// Internal version of 'at' that asserts existence + Factors& internalAt(Key variable) { + const KeyMap::iterator item = index_.find(variable); + assert(item != index_.end()); + return item->second; } + + /// @} +}; + +} + +#include diff --git a/gtsam/inference/tests/testVariableIndexUnordered.cpp b/gtsam/inference/tests/testVariableIndexUnordered.cpp new file mode 100644 index 000000000..097acdda6 --- /dev/null +++ b/gtsam/inference/tests/testVariableIndexUnordered.cpp @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testVariableIndex.cpp + * @brief + * @author Richard Roberts + * @date Sep 26, 2010 + */ + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(VariableIndexUnordered, augment) { + + SymbolicFactorGraphUnordered fg1, fg2; + fg1.push_factor(0, 1); + fg1.push_factor(0, 2); + fg1.push_factor(5, 9); + fg1.push_factor(2, 3); + fg2.push_factor(1, 3); + fg2.push_factor(2, 4); + fg2.push_factor(3, 5); + fg2.push_factor(5, 6); + + SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); + + VariableIndex expected(fgCombined); + VariableIndex actual(fg1); + actual.augment(fg2); + + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(8, actual.nFactors()); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(VariableIndex, remove) { + + SymbolicFactorGraph fg1, fg2; + fg1.push_factor(0, 1); + fg1.push_factor(0, 2); + fg1.push_factor(5, 9); + fg1.push_factor(2, 3); + fg2.push_factor(1, 3); + fg2.push_factor(2, 4); + fg2.push_factor(3, 5); + fg2.push_factor(5, 6); + + SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); + + // Create a factor graph containing only the factors from fg2 and with null + // factors in the place of those of fg1, so that the factor indices are correct. + SymbolicFactorGraph fg2removed(fgCombined); + fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); + + // The expected VariableIndex has the same factor indices as fgCombined but + // with entries from fg1 removed, and still has all 10 variables. + VariableIndex expected(fg2removed, 10); + VariableIndex actual(fgCombined); + vector indices; + indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); + actual.remove(indices, fg1); + + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(VariableIndex, deep_copy) { + + SymbolicFactorGraph fg1, fg2; + fg1.push_factor(0, 1); + fg1.push_factor(0, 2); + fg1.push_factor(5, 9); + fg1.push_factor(2, 3); + fg2.push_factor(1, 3); + fg2.push_factor(2, 4); + fg2.push_factor(3, 5); + fg2.push_factor(5, 6); + + // Create original graph and VariableIndex + SymbolicFactorGraph fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); + VariableIndex original(fgOriginal); + VariableIndex expectedOriginal(fgOriginal); + + // Create a factor graph containing only the factors from fg2 and with null + // factors in the place of those of fg1, so that the factor indices are correct. + SymbolicFactorGraph fg2removed(fgOriginal); + fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); + VariableIndex expectedRemoved(fg2removed); + + // Create a clone and modify the clone - the original should not change + VariableIndex clone(original); + vector indices; + indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); + clone.remove(indices, fg1); + + // When modifying the clone, the original should have stayed the same + EXPECT(assert_equal(expectedOriginal, original)); + EXPECT(assert_equal(expectedRemoved, clone)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */