Added a bunch of Unordered classes, elimination algorithm in progress
Richard Roberts
parent
a446fa4801
commit
ffc55ad026
|
|
@ -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 <boost/range.hpp>
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
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 FACTOR>
|
||||
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<FACTOR> This;
|
||||
|
||||
/** View of the frontal keys (call frontals()) */
|
||||
typedef boost::iterator_range<const_iterator> Frontals;
|
||||
|
||||
/** View of the separator keys (call parents()) */
|
||||
typedef boost::iterator_range<const_iterator> 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();
|
||||
} ///<TODO: comment
|
||||
iterator endFrontals() {
|
||||
return FactorType::begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
iterator beginParents() {
|
||||
return FactorType::begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
iterator endParents() {
|
||||
return FactorType::end();
|
||||
} ///<TODO: comment
|
||||
|
||||
private:
|
||||
// Cast to derived type (non-const)
|
||||
FACTOR& asDerived() { return static_cast<FACTOR&>(*this); }
|
||||
|
||||
// Cast to derived type (const)
|
||||
const FACTOR& asDerived() const { return static_cast<const FACTOR&>(*this); }
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void ConditionalUnordered<FACTOR>::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
|
||||
|
|
@ -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 <gtsam/base/timing.h>
|
||||
#include <gtsam/inference/EliminationTreeUnordered.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(const FactorGraphType& graph,
|
||||
const VariableIndexUnordered& structure, const std::vector<Key>& 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<size_t>::max();
|
||||
|
||||
// Allocate result parent vector and vector of last factor columns
|
||||
std::vector<shared_ptr> nodes(n);
|
||||
std::vector<size_t> parents(n, none);
|
||||
std::vector<size_t> prevCol(m, none);
|
||||
std::vector<bool> 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<EliminationTreeUnordered<FACTOR> >(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<class BAYESNET, class GRAPH>
|
||||
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(
|
||||
const FactorGraphType& factorGraph, const std::vector<Key>& 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<class FACTOR>
|
||||
struct EliminationNode {
|
||||
bool expanded;
|
||||
Key key;
|
||||
std::vector<boost::shared_ptr<FACTOR> > factors;
|
||||
EliminationNode<FACTOR>* parent;
|
||||
template<typename ITERATOR> EliminationNode(
|
||||
Key _key, size_t nFactorsToReserve, ITERATOR firstFactor, ITERATOR lastFactor, EliminationNode<FACTOR>* _parent) :
|
||||
expanded(false), key(_key), parent(_parent) {
|
||||
factors.reserve(nFactorsToReserve);
|
||||
factors.insert(factors.end(), firstFactor, lastFactor);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
|
||||
EliminationTreeUnordered<BAYESNET,GRAPH>::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<FactorType> EliminationNode;
|
||||
std::stack<EliminationNode, std::vector<EliminationNode> > eliminationStack;
|
||||
|
||||
// Create empty Bayes net and factor graph to hold result
|
||||
boost::shared_ptr<BayesNetType> bayesNet = boost::make_shared<BayesNetType>();
|
||||
// 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<FactorGraphType> remainingFactors =
|
||||
boost::make_shared<FactorGraphType>(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<ConditionalType>, boost::shared_ptr<FactorType> > 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<class BAYESNET, class GRAPH>
|
||||
void EliminationTreeUnordered<BAYESNET,GRAPH>::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<class BAYESNET, class GRAPH>
|
||||
bool EliminationTreeUnordered<BAYESNET,GRAPH>::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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/inference/EliminationTreeUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
namespace internal {
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <utility>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
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 BAYESNET, class GRAPH>
|
||||
class EliminationTreeUnordered {
|
||||
|
||||
public:
|
||||
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
|
||||
typedef EliminationTreeUnordered<BAYESNET,GRAPH> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
typedef typename boost::shared_ptr<FactorType> 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<Node> shared_ptr;
|
||||
typedef FastList<sharedFactor> Factors;
|
||||
typedef FastList<shared_ptr> 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<sharedNode> 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<DERIVEDFACTOR>&)
|
||||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
EliminationTreeUnordered(const FactorGraphType& factorGraph,
|
||||
const VariableIndexUnordered& structure, const std::vector<Key>& 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<Key>& 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<BayesNetType>, boost::shared_ptr<FactorGraphType> >
|
||||
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 <gtsam/inference/EliminationTree-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 <gtsam/base/FastSet.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/iterator/transform_iterator.hpp>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <list>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void FactorGraphUnordered<FACTOR>::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<class FACTOR>
|
||||
bool FactorGraphUnordered<FACTOR>::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<class FACTOR>
|
||||
size_t FactorGraphUnordered<FACTOR>::nrFactors() const {
|
||||
size_t size_ = 0;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
if (factor) size_++;
|
||||
return size_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
FastSet<Key> FactorGraphUnordered<FACTOR>::keys() const {
|
||||
FastSet<Key> allKeys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_)
|
||||
if (factor)
|
||||
allKeys.insert(factor->begin(), factor->end());
|
||||
return allKeys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
|
|
@ -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 <boost/serialization/nvp.hpp>
|
||||
#include <boost/function.hpp>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
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 FACTOR>
|
||||
class FactorGraphUnordered {
|
||||
|
||||
public:
|
||||
|
||||
typedef FACTOR FactorType; ///< factor type
|
||||
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
|
||||
typedef FactorGraphUnordered<FACTOR> This; ///< Typedef for this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
|
||||
typedef typename std::vector<sharedFactor>::iterator iterator;
|
||||
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
|
||||
|
||||
/** typedef for elimination result */
|
||||
typedef std::pair<sharedConditional, sharedFactor> EliminationResult;
|
||||
|
||||
/** typedef for an eliminate subroutine */
|
||||
typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
|
||||
|
||||
protected:
|
||||
|
||||
/** concept check, makes sure FACTOR defines print and equals */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
|
||||
/** Collection of factors */
|
||||
std::vector<sharedFactor> 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<class CONDITIONAL>
|
||||
//FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
|
||||
///** convert from Bayes tree */
|
||||
//template<class CONDITIONAL, class CLIQUE>
|
||||
//FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
|
||||
|
||||
///** convert from a derived type */
|
||||
//template<class DERIVEDFACTOR>
|
||||
//FactorGraph(const FactorGraph<DERIVEDFACTOR>& 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<class DERIVEDFACTOR>
|
||||
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
|
||||
factors_.push_back(boost::shared_ptr<FACTOR>(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<typename ITERATOR>
|
||||
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
factors_.insert(end(), firstFactor, lastFactor);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Add a vector of derived factors
|
||||
* @param factors to add
|
||||
*/
|
||||
//template<typename DERIVEDFACTOR>
|
||||
//void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& 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<sharedConditional, FactorGraph<FactorType> > 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<sharedConditional, FactorGraph<FactorType> > eliminate(
|
||||
// const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
|
||||
// boost::optional<const VariableIndex&> variableIndex = boost::none) const;
|
||||
|
||||
///** Eliminate a single variable, by calling FactorGraph::eliminate. */
|
||||
//std::pair<sharedConditional, FactorGraph<FactorType> > eliminateOne(
|
||||
// KeyType variable, const Eliminate& eliminateFcn,
|
||||
// boost::optional<const VariableIndex&> variableIndex = boost::none) const {
|
||||
// std::vector<size_t> 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<Key> keys() const;
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(factors_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // FactorGraph
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/FactorGraphUnordered-inl.h>
|
||||
|
|
@ -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 <gtsam/inference/FactorUnordered.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <vector>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
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<FactorUnordered> shared_ptr;
|
||||
|
||||
/// Iterator over keys
|
||||
typedef std::vector<Key>::iterator iterator;
|
||||
|
||||
/// Const iterator over keys
|
||||
typedef std::vector<Key>::const_iterator const_iterator;
|
||||
|
||||
protected:
|
||||
|
||||
/// The keys involved in this factor
|
||||
std::vector<Key> 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<class CONTAINER>
|
||||
static FactorUnordered FromKeys(const CONTAINER& keys) { return FromIterator(keys.begin(), keys.end()); }
|
||||
|
||||
/** Construct n-way factor from iterator over keys. */
|
||||
template<class ITERATOR> 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<Key>& 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<Key>& 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<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(keys_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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 <string>
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering::Ordering(const std::list<Key> & 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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/pool/pool_alloc.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* An ordering is a map from symbols (non-typed keys) to integer indices
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Ordering {
|
||||
protected:
|
||||
typedef FastMap<Key, Index> Map;
|
||||
typedef std::vector<Map::iterator> OrderingIndex;
|
||||
Map order_;
|
||||
OrderingIndex orderingIndex_;
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Ordering> shared_ptr;
|
||||
|
||||
typedef std::pair<const Key, Index> 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<Key> & 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<iterator,bool> 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<boost::assign_detail::call_push_back<Ordering>, Key>
|
||||
operator+=(Key key) {
|
||||
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<Ordering>(*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<class ARCHIVE>
|
||||
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<class ARCHIVE>
|
||||
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<Index> {
|
||||
public:
|
||||
/** Default constructor creates empty ordering */
|
||||
Unordered() { }
|
||||
|
||||
/** Create from a single symbol */
|
||||
Unordered(Index key) { insert(key); }
|
||||
|
||||
/** Copy constructor */
|
||||
Unordered(const std::set<Index>& keys_in) : std::set<Index>(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
|
||||
|
||||
|
|
@ -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 <gtsam/inference/SymbolicConditionalUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/base/types.h>
|
||||
#include <gtsam/inference/SymbolicFactorUnordered.h>
|
||||
#include <gtsam/inference/ConditionalUnordered.h>
|
||||
|
||||
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<This>, which is a generic interface
|
||||
* class for conditionals.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class SymbolicConditionalUnordered : public SymbolicFactorUnordered, public ConditionalUnordered<SymbolicFactorUnordered> {
|
||||
|
||||
public:
|
||||
|
||||
typedef SymbolicConditionalUnordered This; /// Typedef to this class
|
||||
typedef SymbolicFactorUnordered BaseFactor; /// Typedef to the factor base class
|
||||
typedef ConditionalUnordered<SymbolicFactorUnordered> BaseConditional; /// Typedef to the conditional base class
|
||||
typedef boost::shared_ptr<This> 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<class ITERATOR>
|
||||
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<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/inference/SymbolicBayesNetUnordered.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraphUnordered.h>
|
||||
#include <gtsam/inference/EliminationTreeUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class SymbolicEliminationTreeUnordered : public EliminationTreeUnordered<SymbolicBayesNetUnordered,SymbolicFactorGraphUnordered> {
|
||||
public:
|
||||
typedef EliminationTreeUnordered<SymbolicBayesNetUnordered,SymbolicFactorGraphUnordered> Base; ///< Base class
|
||||
typedef SymbolicEliminationTreeUnordered This; ///< This class
|
||||
typedef boost::shared_ptr<This> 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<DERIVEDFACTOR>&)
|
||||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph,
|
||||
const VariableIndexUnordered& structure, const std::vector<Key>& 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<Key>& 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; }
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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 <boost/make_shared.hpp>
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraphUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraphUnordered::push_factor(Key key) {
|
||||
push_back(boost::make_shared<SymbolicFactorUnordered>(key));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraphUnordered::push_factor(Key key1, Key key2) {
|
||||
push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraphUnordered::push_factor(Key key1, Key key2, Key key3) {
|
||||
push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2,key3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SymbolicFactorGraphUnordered::push_factor(Key key1, Key key2, Key key3, Key key4) {
|
||||
push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2,key3,key4));
|
||||
}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
|
||||
// SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const
|
||||
// {
|
||||
// return FactorGraph<IndexFactor>::eliminateFrontals(nFrontals, EliminateSymbolic);
|
||||
// }
|
||||
//
|
||||
// /* ************************************************************************* */
|
||||
// std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
|
||||
// SymbolicFactorGraph::eliminate(const std::vector<Index>& variables) const
|
||||
// {
|
||||
// return FactorGraph<IndexFactor>::eliminate(variables, EliminateSymbolic);
|
||||
// }
|
||||
//
|
||||
// /* ************************************************************************* */
|
||||
// std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
|
||||
// SymbolicFactorGraph::eliminateOne(Index variable) const
|
||||
// {
|
||||
// return FactorGraph<IndexFactor>::eliminateOne(variable, EliminateSymbolic);
|
||||
// }
|
||||
//
|
||||
// /* ************************************************************************* */
|
||||
// IndexFactor::shared_ptr CombineSymbolic(
|
||||
// const FactorGraph<IndexFactor>& factors, const FastMap<Index,
|
||||
// vector<Index> >& variableSlots) {
|
||||
// IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, variableSlots));
|
||||
//// combined->assertInvariants();
|
||||
// return combined;
|
||||
// }
|
||||
//
|
||||
// /* ************************************************************************* */
|
||||
// pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> //
|
||||
// EliminateSymbolic(const FactorGraph<IndexFactor>& factors, size_t nrFrontals) {
|
||||
//
|
||||
// FastSet<Index> 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<Index> newKeys(keys.begin(), keys.end());
|
||||
// return make_pair(boost::make_shared<IndexConditional>(newKeys, nrFrontals),
|
||||
// boost::make_shared<IndexFactor>(newKeys.begin() + nrFrontals, newKeys.end()));
|
||||
// }
|
||||
|
||||
/* ************************************************************************* */
|
||||
}
|
||||
|
|
@ -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 <gtsam/base/types.h>
|
||||
#include <gtsam/inference/FactorGraphUnordered.h>
|
||||
#include <gtsam/inference/SymbolicFactorUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Symbolic Factor Graph
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
|
||||
|
||||
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<IndexFactor>::eliminateFrontals with EliminateSymbolic
|
||||
// * as the eliminate function argument.
|
||||
// */
|
||||
//GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> 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<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
|
||||
// * function argument.
|
||||
// */
|
||||
//GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminate(const std::vector<Index>& variables) const;
|
||||
|
||||
///** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */
|
||||
//GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> 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<IndexFactor>& factors,
|
||||
const FastMap<Index, std::vector<Index> >& 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<IndexConditional>, boost::shared_ptr<IndexFactor> >
|
||||
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -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 <gtsam/inference/SymbolicFactorUnordered.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
} // gtsam
|
||||
|
|
@ -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 <gtsam/inference/FactorUnordered.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
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<This> 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<class KeyIterator> SymbolicFactorUnordered(KeyIterator beginKey, KeyIterator endKey) : Base(beginKey, endKey) {}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
|
||||
}; // IndexFactor
|
||||
|
||||
}
|
||||
|
|
@ -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 <gtsam/inference/VariableIndexUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG>
|
||||
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<typename ITERATOR, class FG>
|
||||
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<typename ITERATOR>
|
||||
void VariableIndexUnordered::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
|
||||
for(ITERATOR key = firstKey; key != lastKey; ++key) {
|
||||
assert(!internalAt(*key).empty());
|
||||
index_.erase(*key);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <iostream>
|
||||
|
||||
#include <gtsam/inference/VariableIndexUnordered.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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 <vector>
|
||||
#include <deque>
|
||||
#include <stdexcept>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
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<VariableIndexUnordered> shared_ptr;
|
||||
typedef FastList<size_t> Factors;
|
||||
typedef Factors::iterator Factor_iterator;
|
||||
typedef Factors::const_iterator Factor_const_iterator;
|
||||
|
||||
protected:
|
||||
typedef FastMap<Key,Factors> 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<class FG>
|
||||
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<class FG>
|
||||
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<typename ITERATOR, class FG>
|
||||
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<typename ITERATOR>
|
||||
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 <gtsam/inference/VariableIndexUnordered-inl.h>
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <gtsam/inference/VariableIndexUnordered.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
|
||||
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<size_t> 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<size_t> 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue