12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Continued work on unordered classes and elimination algorithm

release/4.3a0
Richard Roberts 2013-06-06 15:36:11 +00:00
parent 5d05737798
commit d5e721a1d0
35 changed files with 335 additions and 579 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/LocalizationExample.cpp
View File

@ -27,7 +27,7 @@
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
// As in OdometryExample.cpp, we use a BetweenFactor to model odometry measurements.
#include <gtsam/slam/BetweenFactor.h>

2
examples/Pose2SLAMExample.cpp
View File

@ -29,7 +29,7 @@
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.

2
examples/Pose2SLAMwSPCG.cpp
View File

@ -33,7 +33,7 @@
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use simple integer keys
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.

35
gtsam/inference/ConditionalUnordered.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <iostream>
#include <gtsam/inference/Key.h>
@ -35,7 +36,7 @@ namespace gtsam {
* IndexConditional and GaussianConditional for examples.
* \nosubgrouping
*/
template<class FACTOR>
template<class FACTOR, class DERIVEDCONDITIONAL>
class ConditionalUnordered {
protected:
@ -51,7 +52,7 @@ namespace gtsam {
public:
typedef ConditionalUnordered<FACTOR> This;
typedef ConditionalUnordered<FACTOR,DERIVEDCONDITIONAL> This;
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<const_iterator> Frontals;
@ -86,12 +87,12 @@ namespace gtsam {
size_t nrFrontals() const { return nrFrontals_; }
/** return the number of parents */
size_t nrParents() const { return asDerived.size() - nrFrontals_; }
size_t nrParents() const { return asFactor().size() - nrFrontals_; }
/** Convenience function to get the first frontal key */
Key firstFrontalKey() const {
if(nrFrontals_ > 0)
return asDerived().front();
return asFactor().front();
else
throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
}
@ -103,16 +104,16 @@ namespace gtsam {
Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
/** Iterator pointing to first frontal key. */
const_iterator beginFrontals() const { return asDerived().begin(); }
const_iterator beginFrontals() const { return asFactor().begin(); }
/** Iterator pointing past the last frontal key. */
const_iterator endFrontals() const { return asDerived().begin() + nrFrontals_; }
const_iterator endFrontals() const { return asFactor().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(); }
const_iterator endParents() const { return asFactor().end(); }
/// @}
/// @name Advanced Interface
@ -120,24 +121,24 @@ namespace gtsam {
/** Mutable iterators and accessors */
iterator beginFrontals() {
return FactorType::begin();
return asFactor().begin();
} ///<TODO: comment
iterator endFrontals() {
return FactorType::begin() + nrFrontals_;
return asFactor().begin() + nrFrontals_;
} ///<TODO: comment
iterator beginParents() {
return FactorType::begin() + nrFrontals_;
return asFactor().begin() + nrFrontals_;
} ///<TODO: comment
iterator endParents() {
return FactorType::end();
return asFactor().end();
} ///<TODO: comment
private:
// Cast to derived type (non-const)
FACTOR& asDerived() { return static_cast<FACTOR&>(*this); }
// Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
// Cast to derived type (const)
const FACTOR& asDerived() const { return static_cast<const FACTOR&>(*this); }
// Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
/** Serialization function */
friend class boost::serialization::access;
@ -152,8 +153,8 @@ namespace gtsam {
};
/* ************************************************************************* */
template<class FACTOR>
void ConditionalUnordered<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
template<class FACTOR, class DERIVEDFACTOR>
void ConditionalUnordered<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals())
std::cout << " " << formatter(key);

187
gtsam/inference/EliminationTreeUnordered-inl.h
View File

@ -17,10 +17,12 @@
*/
#pragma once
#include <boost/foreach.hpp>
#include <stack>
#include <gtsam/base/timing.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
#include <boost/foreach.hpp>
#include <gtsam/inference/VariableIndexUnordered.h>
namespace gtsam {
@ -39,7 +41,7 @@ namespace gtsam {
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<sharedNode> nodes(n);
std::vector<size_t> parents(n, none);
std::vector<size_t> prevCol(m, none);
std::vector<bool> factorUsed(m, false);
@ -50,7 +52,8 @@ namespace gtsam {
{
// 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]);
nodes[j] = boost::make_shared<Node>();
nodes[j]->key = order[j];
// for row i \in Struct[A*j] do
BOOST_FOREACH(const size_t i, factors) {
@ -71,12 +74,12 @@ namespace gtsam {
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]);
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]);
nodes[j]->factors.push_back(graph[i]);
factorUsed[i] = true;
}
prevCol[i] = j;
@ -115,17 +118,51 @@ namespace gtsam {
remainingFactors_.swap(temp.remainingFactors_);
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>&
EliminationTreeUnordered<BAYESNET,GRAPH>::operator=(const EliminationTreeUnordered<BAYESNET,GRAPH>& other)
{
// Start by duplicating the roots.
roots_.clear();
std::stack<sharedNode, std::vector<sharedNode> > stack;
BOOST_FOREACH(const sharedNode& root, other.roots_) {
roots_.push_back(boost::make_shared<Node>(*root));
stack.push(roots_.back());
}
// Traverse the tree, duplicating each node's children and fixing the pointers as we go. We do
// not clone any factors, only copy their pointers (this is a standard in GTSAM).
while(!stack.empty()) {
sharedNode node = stack.top();
stack.pop();
BOOST_FOREACH(sharedNode& child, node->subTrees) {
// Important: since we are working with a *reference* to a shared pointer in this
// BOOST_FOREACH loop, the next statement modifies the pointer in the current node's child
// list - it replaces it with a pointer to a copy of the child.
child = boost::make_shared<Node>(*child);
stack.push(child);
}
}
// Assign the remaining factors - these are pointers to factors in the original factor graph and
// we do not clone them.
remainingFactors_ = other.remainingFactors_;
return *this;
}
/* ************************************************************************* */
namespace {
template<class FACTOR>
template<class ELIMINATIONTREE>
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) {
const typename ELIMINATIONTREE::Node* const treeNode;
std::vector<typename ELIMINATIONTREE::sharedFactor> factors;
EliminationNode<ELIMINATIONTREE>* const parent;
template<typename ITERATOR> EliminationNode(const typename ELIMINATIONTREE::Node* _treeNode, size_t nFactorsToReserve,
ITERATOR firstFactor, ITERATOR lastFactor, EliminationNode<ELIMINATIONTREE>* _parent) :
expanded(false), treeNode(_treeNode), parent(_parent) {
factors.reserve(nFactorsToReserve);
factors.insert(factors.end(), firstFactor, lastFactor);
}
@ -135,7 +172,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
EliminationTreeUnordered<BAYESNET,GRAPH>::eliminate(Eliminate function)
EliminationTreeUnordered<BAYESNET,GRAPH>::eliminate(Eliminate function) const
{
// 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
@ -143,7 +180,7 @@ namespace gtsam {
// 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;
typedef EliminationNode<This> EliminationNode;
std::stack<EliminationNode, std::vector<EliminationNode> > eliminationStack;
// Create empty Bayes net and factor graph to hold result
@ -152,25 +189,25 @@ namespace gtsam {
// 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_);
boost::make_shared<FactorGraphType>(remainingFactors_.begin(), remainingFactors_.end());
// Add roots to the stack
BOOST_FOREACH(const sharedNode& root, roots_) {
eliminationStack.push(
EliminationNode(root->key, root->factors.size() + root->subTrees.size(),
EliminationNode(root.get(), 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 mark it
// expanded - we'll come back and eliminate it later after the children have been processed.
EliminationNode& node = nodeStack.top();
EliminationNode& node = eliminationStack.top();
if(node.expanded) {
// Remove from stack
nodeStack.pop();
eliminationStack.pop();
// Do a dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = node.key;
std::vector<Key> keyAsVector(1); keyAsVector[0] = node.treeNode->key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(node.factors, keyAsVector);
@ -185,10 +222,10 @@ namespace gtsam {
} 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)); }
BOOST_FOREACH(const sharedNode& child, node.treeNode->subTrees) {
eliminationStack.push(
EliminationNode(child.get(), child->factors.size() + child->subTrees.size(),
child->factors.begin(), child->factors.end(), &node)); }
}
}
@ -198,28 +235,98 @@ namespace gtsam {
/* ************************************************************************* */
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); }
void EliminationTreeUnordered<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
{
// Depth-first-traversal stack
std::stack<std::pair<sharedNode, std::string> > stack;
// Add roots
BOOST_FOREACH(const sharedNode& node, roots_) { stack.push(std::make_pair(node, " ")); }
// Traverse
while(!stack.empty()) {
std::pair<sharedNode,string> node_string = stack.top();
stack.pop();
std::cout << node_string.second << " (" << formatter(node_string.first->key) << ")\n";
BOOST_FOREACH(const sharedFactor& factor, node_string.first->factors) {
if(factor)
factor->print(node_string.second + " ");
else
std::cout << node_string.second << " null factor\n";
}
BOOST_FOREACH(const sharedNode& child, node_string.first->subTrees) {
stack.push(std::make_pair(child, node_string.second + " "));
}
}
}
/* ************************************************************************* */
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))
bool EliminationTreeUnordered<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
{
// Depth-first-traversal stacks
std::stack<sharedNode, std::vector<sharedNode> > stack1, stack2;
// Add roots in sorted order
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(make_pair(root->key, root)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
}
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(make_pair(root->key, root)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
}
// Traverse, adding children in sorted order
while(!stack1.empty() && !stack2.empty()) {
// Pop nodes
sharedNode node1 = stack1.top();
stack1.pop();
sharedNode node2 = stack2.top();
stack2.pop();
// Compare nodes
if(node1->key != node2->key)
return false;
if(node1->factors.size() != node2->factors.size()) {
return false;
} else {
for(Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin();
it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts
{
if(*it1 && *it2) {
if(!(*it1)->equals(**it2, tol))
return false;
} else if(*it1 && !*it2 || *it2 && !*it1) {
return false;
return true;
} else
}
}
}
// Add children in sorted order
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& node, node1->subTrees) { keys.insert(make_pair(node->key, node)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
}
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& node, node2->subTrees) { keys.insert(make_pair(node->key, node)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
}
}
// If either stack is not empty, the number of nodes differed
if(!stack1.empty() || !stack2.empty())
return false;
return true;
}
}

26
gtsam/inference/EliminationTreeUnordered.h
View File

@ -19,12 +19,14 @@
#include <utility>
#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
namespace { template<class ELIMINATIONTREE> struct EliminationNode; }
namespace gtsam {
@ -59,28 +61,29 @@ namespace gtsam {
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
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef boost::function<std::pair<sharedConditional,sharedFactor>(std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
private:
class Node {
public:
typedef boost::shared_ptr<Node> shared_ptr;
typedef FastList<sharedFactor> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef FastList<boost::shared_ptr<Node> > 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
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
FastList<sharedNode> roots_;
FactorGraphType remainingFactors_;
std::vector<sharedFactor> remainingFactors_;
public:
@ -107,11 +110,11 @@ namespace gtsam {
/** TODO: 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) { throw std::runtime_error("Not implemented"); }
EliminationTreeUnordered(const This& other) { *this = other; }
/** TODO: 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) { throw std::runtime_error("Not implemented"); }
This& operator=(const This& other);
/// @}
/// @name Standard Interface
@ -138,13 +141,18 @@ namespace gtsam {
/// @}
private:
protected:
/// Protected default constructor
EliminationTreeUnordered() {}
private:
/// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeUnorderedTester;
friend struct EliminationNode<This>;
};
}
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/EliminationTreeUnordered-inl.h>

6
gtsam/inference/FactorGraphUnordered-inl.h
View File

@ -38,14 +38,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
void FactorGraphUnordered<FACTOR>::print(const std::string& s,
const KeyFormatter& formatter) const {
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);
if (factors_[i])
factors_[i]->print(ss.str(), formatter);
}
}

11
gtsam/inference/FactorGraphUnordered.h
View File

@ -22,7 +22,6 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
@ -48,12 +47,6 @@ namespace gtsam {
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 */
@ -70,6 +63,10 @@ namespace gtsam {
/** Default constructor */
FactorGraphUnordered() {}
/** Constructor from iterator over factors */
template<typename ITERATOR>
FactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
/// @}
/// @name Advanced Constructors
/// @{

12
gtsam/inference/FactorUnordered.cpp
View File

@ -19,12 +19,15 @@
// \callgraph
#include <boost/foreach.hpp>
#include <iostream>
#include <gtsam/inference/FactorUnordered.h>
namespace gtsam {
/* ************************************************************************* */
void FactorUnordered::print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const
void FactorUnordered::print(const std::string& s, const KeyFormatter& formatter) const
{
return this->printKeys(s, formatter);
}
@ -32,8 +35,13 @@ namespace gtsam {
/* ************************************************************************* */
void FactorUnordered::printKeys(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key);
BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key);
std::cout << std::endl;
}
/* ************************************************************************* */
bool FactorUnordered::equals(const This& other, double tol) const {
return keys_ == other.keys_;
}
}

6
gtsam/inference/FactorUnordered.h
View File

@ -48,7 +48,7 @@ namespace gtsam {
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/
class FactorUnordered {
class GTSAM_EXPORT FactorUnordered {
public:
@ -108,7 +108,9 @@ public:
/** Construct n-way factor from iterator over keys. */
template<typename ITERATOR> static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) {
FactorUnordered result; result.keys_.assign(first, last); }
FactorUnordered result;
result.keys_.assign(first, last);
return result; }
/** Construct n-way factor from container of keys. */
template<class CONTAINER>

2
gtsam/inference/Key.cpp
View File

@ -22,7 +22,7 @@
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/LabeledSymbol.h>
namespace gtsam {

130
gtsam/inference/OrderingUnordered.cpp
View File

@ -1,130 +0,0 @@
/* ----------------------------------------------------------------------------
* 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;
}
}

271
gtsam/inference/OrderingUnordered.h
View File

@ -1,271 +0,0 @@
/* ----------------------------------------------------------------------------
* 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

2
gtsam/inference/VariableIndexUnordered-inl.h
View File

@ -34,7 +34,7 @@ void VariableIndexUnordered::augment(const FG& factors)
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]) {
BOOST_FOREACH(const Key key, *factors[i]) {
index_[key].push_back(globalI);
++ nEntries_;
}

2
gtsam/inference/VariableIndexUnordered.h
View File

@ -70,7 +70,7 @@ public:
* of a factor graph.
*/
template<class FG>
VariableIndexUnordered(const FG& factorGraph) { augment(factorGraph); }
VariableIndexUnordered(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }
/// @}
/// @name Standard Interface

2
gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/navigation/InertialNavFactor_GlobalVelocity.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>

2
gtsam/nonlinear/Ordering.h
View File

@ -17,7 +17,7 @@
#pragma once
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/inference.h>
#include <gtsam/base/FastMap.h>

3
gtsam/nonlinear/Symbol.h
View File

@ -18,9 +18,10 @@
#pragma once
#include <gtsam/nonlinear/Key.h>
#include <boost/serialization/nvp.hpp>
#include <gtsam/inference/Key.h>
namespace gtsam {
/**

12
gtsam/nonlinear/Values.h
View File

@ -24,12 +24,6 @@
#pragma once
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Ordering.h>
#include <boost/pool/pool_alloc.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include <boost/iterator/transform_iterator.hpp>
@ -49,6 +43,12 @@
#include <string>
#include <utility>
#include <gtsam/base/Value.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
// Forward declarations / utilities

3
gtsam/nonlinear/nonlinearExceptions.h
View File

@ -17,10 +17,11 @@
*/
#pragma once
#include <gtsam/nonlinear/Key.h>
#include <boost/lexical_cast.hpp>
#include <exception>
#include <gtsam/inference/Key.h>
namespace gtsam {
/**

2
gtsam/symbolic/CMakeLists.txt
View File

@ -4,11 +4,11 @@ install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
# Components to link tests in this subfolder against
set(symbolic_local_libs
symbolic
inference
geometry
base
ccolamd
symbolic
)
# Files to exclude from compilation of tests and timing scripts

4
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/types.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
namespace gtsam {
@ -30,6 +31,9 @@ namespace gtsam {
public:
typedef SymbolicFactorGraphUnordered Base;
typedef SymbolicConditionalUnordered ConditionalType;
/// @name Standard Constructors
/// @{

14
gtsam/symbolic/SymbolicConditionalUnordered.h
View File

@ -32,13 +32,13 @@ namespace gtsam {
* class for conditionals.
* \nosubgrouping
*/
class SymbolicConditionalUnordered : public SymbolicFactorUnordered, public ConditionalUnordered<SymbolicFactorUnordered> {
class GTSAM_EXPORT SymbolicConditionalUnordered : public SymbolicFactorUnordered, public ConditionalUnordered<SymbolicFactorUnordered,SymbolicConditionalUnordered> {
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 ConditionalUnordered<SymbolicFactorUnordered,SymbolicConditionalUnordered> 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
@ -66,18 +66,14 @@ namespace gtsam {
static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
{
SymbolicConditionalUnordered result;
result = BaseFactor::FromIterator(firstKey, lastKey);
(BaseFactor&)result = BaseFactor::FromIterator(firstKey, lastKey);
result.nrFrontals_ = nrFrontals;
return result; }
/** Named constructor from an arbitrary number of keys and frontals */
template<class CONTAINER>
static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals)
{
SymbolicConditionalUnordered result;
result = BaseFactor::FromKeys(keys);
result.nrFrontals_ = nrFrontals;
return result; }
static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals) {
return FromIterator(keys.begin(), keys.end(), nrFrontals); }
/// @}

9
gtsam/symbolic/SymbolicEliminationTreeUnordered.h
View File

@ -16,9 +16,9 @@
* @author Richard Roberts
*/
#include <gtsam/inference/EliminationTreeUnordered.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
namespace gtsam {
@ -57,6 +57,13 @@ namespace gtsam {
* copied, factors are not cloned. */
This& operator=(const This& other) { (void) Base::operator=(other); return *this; }
private:
/// Private default constructor
SymbolicEliminationTreeUnordered() {}
friend class ::EliminationTreeUnorderedTester;
};
}

17
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

@ -27,31 +27,38 @@ namespace gtsam {
/** Symbolic Factor Graph
* \nosubgrouping
*/
class SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
class GTSAM_EXPORT SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
public:
typedef SymbolicFactorGraphUnordered This;
typedef FactorGraphUnordered<SymbolicFactorUnordered> Base;
/// @name Standard Constructors
/// @{
/** Construct empty factor graph */
SymbolicFactorGraphUnordered() {}
/** Constructor from iterator over factors */
template<typename ITERATOR>
SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
/// @}
/// @name Standard Interface
/// @{
/** Push back unary factor */
GTSAM_EXPORT void push_factor(Key key);
void push_factor(Key key);
/** Push back binary factor */
GTSAM_EXPORT void push_factor(Key key1, Key key2);
void push_factor(Key key1, Key key2);
/** Push back ternary factor */
GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3);
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);
void push_factor(Key key1, Key key2, Key key3, Key key4);
/// @}
};

6
gtsam/symbolic/SymbolicFactorUnordered.cpp
View File

@ -34,6 +34,12 @@ namespace gtsam {
BOOST_FOREACH(const SymbolicFactorUnordered::shared_ptr& factor, factors) {
allKeys.insert(factor->begin(), factor->end()); }
// Check keys
BOOST_FOREACH(Key key, keys) {
if(allKeys.find(key) == allKeys.end())
throw std::runtime_error("Requested to eliminate a key that is not in the factors");
}
// Sort frontal keys
FastSet<Key> frontals(keys);
const size_t nFrontals = keys.size();

19
gtsam/symbolic/SymbolicFactorUnordered.h
View File

@ -17,6 +17,9 @@
#pragma once
#include <utility>
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorUnordered.h>
#include <gtsam/inference/Key.h>
@ -33,7 +36,7 @@ namespace gtsam {
* It derives from Factor with a key type of Key, an unsigned integer.
* \nosubgrouping
*/
class SymbolicFactorUnordered: public FactorUnordered {
class GTSAM_EXPORT SymbolicFactorUnordered: public FactorUnordered {
public:
@ -47,12 +50,6 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/** Virtual destructor */
~SymbolicFactorUnordered() {}
/** Copy constructor */
SymbolicFactorUnordered(const This& f) : Base(f) {}
/** Default constructor for I/O */
SymbolicFactorUnordered() {}
@ -83,14 +80,14 @@ namespace gtsam {
template<typename KEYITERATOR>
static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) {
SymbolicFactorUnordered result;
result = Base::FromIterator(beginKey, endKey);
(Base&)result = Base::FromIterator(beginKey, endKey);
return result; }
/** Constructor from a collection of keys */
template<class CONTAINER>
static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) {
SymbolicFactorUnordered result;
result = Base::FromKeys(keys);
(Base&)result = Base::FromKeys(keys);
return result; }
/// @}
@ -105,7 +102,7 @@ namespace gtsam {
}; // IndexFactor
std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys);
GTSAM_EXPORT std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
EliminateSymbolicUnordered(const std::vector<SymbolicFactorUnordered::shared_ptr>& factors, const std::vector<Key>& keys);
}

88
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -17,6 +17,9 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
@ -28,45 +31,54 @@ public:
// build hardcoded tree
static SymbolicEliminationTreeUnordered buildHardcodedTree(const SymbolicFactorGraphUnordered& fg) {
SymbolicEliminationTreeUnordered::sharedNode leaf0(new SymbolicEliminationTree);
leaf0->add(fg[0]);
leaf0->add(fg[1]);
SymbolicEliminationTreeUnordered::sharedNode leaf0(new SymbolicEliminationTreeUnordered::Node);
leaf0->key = 0;
leaf0->factors.push_back(fg[0]);
leaf0->factors.push_back(fg[1]);
SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTree(1));
node1->add(fg[2]);
node1->add(leaf0);
SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTreeUnordered::Node);
node1->key = 1;
node1->factors.push_back(fg[2]);
node1->subTrees.push_back(leaf0);
SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTree(2));
node2->add(fg[3]);
node2->add(node1);
SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTreeUnordered::Node);
node2->key = 2;
node2->factors.push_back(fg[3]);
node2->subTrees.push_back(node1);
SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTree(3));
leaf3->add(fg[4]);
SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTreeUnordered::Node);
leaf3->key = 3;
leaf3->factors.push_back(fg[4]);
SymbolicEliminationTreeUnordered::sharedNode etree(new SymbolicEliminationTree(4));
etree->add(leaf3);
etree->add(node2);
SymbolicEliminationTreeUnordered::sharedNode root(new SymbolicEliminationTreeUnordered::Node);
root->key = 4;
root->subTrees.push_back(leaf3);
root->subTrees.push_back(node2);
return etree;
SymbolicEliminationTreeUnordered tree;
tree.roots_.push_back(root);
return tree;
}
};
TEST(EliminationTree, Create)
{
// create example factor graph
SymbolicFactorGraph fg;
SymbolicFactorGraphUnordered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
fg.push_factor(2, 4);
fg.push_factor(3, 4);
SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg);
SymbolicEliminationTreeUnordered expected = EliminationTreeUnorderedTester::buildHardcodedTree(fg);
// Build from factor graph
SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg);
vector<size_t> order;
order += 0,1,2,3,4;
SymbolicEliminationTreeUnordered actual(fg, order);
CHECK(assert_equal(*expected,*actual));
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
@ -74,18 +86,18 @@ TEST(EliminationTree, Create)
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
/* ************************************************************************* */
TEST(EliminationTree, eliminate )
TEST_UNSAFE(EliminationTree, eliminate )
{
// create expected Chordal bayes Net
SymbolicBayesNet expected;
expected.push_front(boost::make_shared<IndexConditional>(4));
expected.push_front(boost::make_shared<IndexConditional>(3,4));
expected.push_front(boost::make_shared<IndexConditional>(2,4));
expected.push_front(boost::make_shared<IndexConditional>(1,2,4));
expected.push_front(boost::make_shared<IndexConditional>(0,1,2));
SymbolicBayesNetUnordered expected;
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4));
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(3,4));
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2,4));
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(1,2,4));
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(0,1,2));
// Create factor graph
SymbolicFactorGraph fg;
SymbolicFactorGraphUnordered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
@ -93,21 +105,23 @@ TEST(EliminationTree, eliminate )
fg.push_factor(3, 4);
// eliminate
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
vector<size_t> order;
order += 0,1,2,3,4;
SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg,order).eliminate(EliminateSymbolicUnordered).first;
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(EliminationTree, disconnected_graph) {
SymbolicFactorGraph fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 2);
fg.push_factor(3, 4);
CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException);
}
//TEST(EliminationTree, disconnected_graph) {
// SymbolicFactorGraph fg;
// fg.push_factor(0, 1);
// fg.push_factor(0, 2);
// fg.push_factor(1, 2);
// fg.push_factor(3, 4);
//
// CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException);
//}
/* ************************************************************************* */
int main() {

2
gtsam_unstable/nonlinear/FixedLagSmoother.h
View File

@ -21,9 +21,9 @@
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>
#include <map>
namespace gtsam {

14
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -18,16 +18,16 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/debug.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace std;
using namespace gtsam;

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -26,7 +26,7 @@
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h>

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -26,7 +26,7 @@
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>

10
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -19,17 +19,17 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/base/debug.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/debug.h>
using namespace std;
using namespace gtsam;

6
gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp
View File

@ -17,12 +17,12 @@
*/
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;

1
tests/CMakeLists.txt
View File

@ -15,6 +15,7 @@ set(tests_local_libs
linear
discrete
inference
symbolic
geometry
base
ccolamd