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

Refactoring to use a new Symbol key instead of strings in Bayes*, Gaussian*, Ordering, Symbolic*, VectorConfig. Renamed existing type-checking key Symbol<C,T> to TypedSymbol<C,T>

release/4.3a0
Richard Roberts 2010-01-17 19:34:57 +00:00
parent ea14959835
commit aef0b42562
80 changed files with 647 additions and 459 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/BayesNet-inl.h
View File

@ -24,9 +24,9 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
void BayesNet<Conditional>::print(const string& s) const { void BayesNet<Conditional>::print(const string& s) const {
cout << s << ":\n"; cout << s << ":\n";
std::string key; Symbol key;
BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_) BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_)
conditional->print("Node[" + conditional->key() + "]"); conditional->print("Node[" + (string)(conditional->key()) + "]");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -63,10 +63,10 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
typename BayesNet<Conditional>::sharedConditional typename BayesNet<Conditional>::sharedConditional
BayesNet<Conditional>::operator[](const std::string& key) const { BayesNet<Conditional>::operator[](const Symbol& key) const {
const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),onKey<Conditional>(key)); const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),onKey<Conditional>(key));
if (it == conditionals_.end()) throw(invalid_argument( if (it == conditionals_.end()) throw(invalid_argument(
"BayesNet::operator['"+key+"']: not found")); "BayesNet::operator['"+(std::string)key+"']: not found"));
return *it; return *it;
} }

3
cpp/BayesNet.h
View File

@ -14,6 +14,7 @@
#include <boost/serialization/shared_ptr.hpp> #include <boost/serialization/shared_ptr.hpp>
#include "Testable.h" #include "Testable.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -85,7 +86,7 @@ namespace gtsam {
Ordering ordering() const; Ordering ordering() const;
/** SLOW O(n) random access to Conditional by key */ /** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](const std::string& key) const; sharedConditional operator[](const Symbol& key) const;
/** return last node in ordering */ /** return last node in ordering */
inline sharedConditional back() { return conditionals_.back(); } inline sharedConditional back() { return conditionals_.back(); }

38
cpp/BayesTree-inl.h
View File

@ -38,11 +38,11 @@ namespace gtsam {
void BayesTree<Conditional>::Clique::print(const string& s) const { void BayesTree<Conditional>::Clique::print(const string& s) const {
cout << s; cout << s;
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
cout << " " << conditional->key(); cout << " " << (string)(conditional->key());
if (!separator_.empty()) { if (!separator_.empty()) {
cout << " :"; cout << " :";
BOOST_FOREACH(string key, separator_) BOOST_FOREACH(const Symbol& key, separator_)
cout << " " << key; cout << " " << (std::string)key;
} }
cout << endl; cout << endl;
} }
@ -105,22 +105,22 @@ namespace gtsam {
Ordering ordering = separator_; Ordering ordering = separator_;
// remove any variables in the root, after this integrands = Cp\R, ordering = S\R // remove any variables in the root, after this integrands = Cp\R, ordering = S\R
BOOST_FOREACH(string key, R->ordering()) { BOOST_FOREACH(const Symbol& key, R->ordering()) {
integrands.remove(key); integrands.remove(key);
ordering.remove(key); ordering.remove(key);
} }
// remove any variables in the separator, after this integrands = Cp\R\S // remove any variables in the separator, after this integrands = Cp\R\S
BOOST_FOREACH(string key, separator_) integrands.remove(key); BOOST_FOREACH(const Symbol& key, separator_) integrands.remove(key);
// form the ordering as [Cp\R\S S\R] // form the ordering as [Cp\R\S S\R]
BOOST_REVERSE_FOREACH(string key, integrands) ordering.push_front(key); BOOST_REVERSE_FOREACH(const Symbol& key, integrands) ordering.push_front(key);
// eliminate to get marginal // eliminate to get marginal
BayesNet<Conditional> p_S_R = eliminate<Factor,Conditional>(p_Cp_R,ordering); BayesNet<Conditional> p_S_R = eliminate<Factor,Conditional>(p_Cp_R,ordering);
// remove all integrands // remove all integrands
BOOST_FOREACH(string key, integrands) p_S_R.pop_front(); BOOST_FOREACH(const Symbol& key, integrands) p_S_R.pop_front();
// return the parent shortcut P(Sp|R) // return the parent shortcut P(Sp|R)
return p_S_R; return p_S_R;
@ -167,7 +167,7 @@ namespace gtsam {
// Find the keys of both C1 and C2 // Find the keys of both C1 and C2
Ordering keys12 = keys(); Ordering keys12 = keys();
BOOST_FOREACH(string key,C2->keys()) keys12.push_back(key); BOOST_FOREACH(const Symbol& key,C2->keys()) keys12.push_back(key);
keys12.unique(); keys12.unique();
// Calculate the marginal // Calculate the marginal
@ -214,7 +214,7 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_) BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_.reset(); child->parent_.reset();
BOOST_FOREACH(string key, clique->ordering()) { BOOST_FOREACH(const Symbol& key, clique->ordering()) {
nodes_.erase(key); nodes_.erase(key);
} }
} }
@ -248,8 +248,8 @@ namespace gtsam {
// binary predicate to test equality of a pair for use in equals // binary predicate to test equality of a pair for use in equals
template<class Conditional> template<class Conditional>
bool check_pair( bool check_pair(
const pair<string,typename BayesTree<Conditional>::sharedClique >& v1, const pair<Symbol,typename BayesTree<Conditional>::sharedClique >& v1,
const pair<string,typename BayesTree<Conditional>::sharedClique >& v2 const pair<Symbol,typename BayesTree<Conditional>::sharedClique >& v2
) { ) {
return v1.first == v2.first && v1.second->equals(*(v2.second)); return v1.first == v2.first && v1.second->equals(*(v2.second));
} }
@ -267,8 +267,8 @@ namespace gtsam {
void BayesTree<Conditional>::insert(const sharedConditional& conditional) void BayesTree<Conditional>::insert(const sharedConditional& conditional)
{ {
// get key and parents // get key and parents
string key = conditional->key(); const Symbol& key = conditional->key();
list<string> parents = conditional->parents(); list<Symbol> parents = conditional->parents(); // rtodo: const reference?
// if no parents, start a new root clique // if no parents, start a new root clique
if (parents.empty()) { if (parents.empty()) {
@ -277,7 +277,7 @@ namespace gtsam {
} }
// otherwise, find the parent clique // otherwise, find the parent clique
string parent = parents.front(); Symbol parent = parents.front();
sharedClique parent_clique = (*this)[parent]; sharedClique parent_clique = (*this)[parent];
// if the parents and parent clique have the same size, add to parent clique // if the parents and parent clique have the same size, add to parent clique
@ -297,7 +297,7 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
template<class Factor> template<class Factor>
FactorGraph<Factor> FactorGraph<Factor>
BayesTree<Conditional>::marginal(const string& key) const { BayesTree<Conditional>::marginal(const Symbol& key) const {
// get clique containing key // get clique containing key
sharedClique clique = (*this)[key]; sharedClique clique = (*this)[key];
@ -318,7 +318,7 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
template<class Factor> template<class Factor>
BayesNet<Conditional> BayesNet<Conditional>
BayesTree<Conditional>::marginalBayesNet(const string& key) const { BayesTree<Conditional>::marginalBayesNet(const Symbol& key) const {
// calculate marginal as a factor graph // calculate marginal as a factor graph
FactorGraph<Factor> fg = this->marginal<Factor>(key); FactorGraph<Factor> fg = this->marginal<Factor>(key);
@ -333,7 +333,7 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
template<class Factor> template<class Factor>
FactorGraph<Factor> FactorGraph<Factor>
BayesTree<Conditional>::joint(const string& key1, const string& key2) const { BayesTree<Conditional>::joint(const Symbol& key1, const Symbol& key2) const {
// get clique C1 and C2 // get clique C1 and C2
sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
@ -356,7 +356,7 @@ namespace gtsam {
template<class Conditional> template<class Conditional>
template<class Factor> template<class Factor>
BayesNet<Conditional> BayesNet<Conditional>
BayesTree<Conditional>::jointBayesNet(const string& key1, const string& key2) const { BayesTree<Conditional>::jointBayesNet(const Symbol& key1, const Symbol& key2) const {
// calculate marginal as a factor graph // calculate marginal as a factor graph
FactorGraph<Factor> fg = this->joint<Factor>(key1,key2); FactorGraph<Factor> fg = this->joint<Factor>(key1,key2);
@ -406,7 +406,7 @@ namespace gtsam {
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) { FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(string key, newFactor->keys()) BOOST_FOREACH(const Symbol& key, newFactor->keys())
try { try {
// get the clique and remove it from orphans (if it exists) // get the clique and remove it from orphans (if it exists)
sharedClique clique = (*this)[key]; sharedClique clique = (*this)[key];

17
cpp/BayesTree.h
View File

@ -18,6 +18,7 @@
#include "Testable.h" #include "Testable.h"
#include "FactorGraph.h" #include "FactorGraph.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -43,7 +44,7 @@ namespace gtsam {
typedef typename boost::shared_ptr<Clique> shared_ptr; typedef typename boost::shared_ptr<Clique> shared_ptr;
shared_ptr parent_; shared_ptr parent_;
std::list<shared_ptr> children_; std::list<shared_ptr> children_;
std::list<std::string> separator_; /** separator keys */ std::list<Symbol> separator_; /** separator keys */
//* Constructor */ //* Constructor */
Clique(const sharedConditional& conditional); Clique(const sharedConditional& conditional);
@ -94,7 +95,7 @@ namespace gtsam {
private: private:
/** Map from keys to Clique */ /** Map from keys to Clique */
typedef std::map<std::string, sharedClique> Nodes; typedef std::map<Symbol, sharedClique> Nodes;
Nodes nodes_; Nodes nodes_;
/** Root clique */ /** Root clique */
@ -141,29 +142,29 @@ namespace gtsam {
} }
/** find the clique to which key belongs */ /** find the clique to which key belongs */
sharedClique operator[](const std::string& key) const { sharedClique operator[](const Symbol& key) const {
typename Nodes::const_iterator it = nodes_.find(key); typename Nodes::const_iterator it = nodes_.find(key);
if (it == nodes_.end()) throw(std::invalid_argument( if (it == nodes_.end()) throw(std::invalid_argument(
"BayesTree::operator['" + key + "']: key not found")); "BayesTree::operator['" + (std::string)key + "']: key not found"));
sharedClique clique = it->second; sharedClique clique = it->second;
return clique; return clique;
} }
/** return marginal on any variable */ /** return marginal on any variable */
template<class Factor> template<class Factor>
FactorGraph<Factor> marginal(const std::string& key) const; FactorGraph<Factor> marginal(const Symbol& key) const;
/** return marginal on any variable, as a Bayes Net */ /** return marginal on any variable, as a Bayes Net */
template<class Factor> template<class Factor>
BayesNet<Conditional> marginalBayesNet(const std::string& key) const; BayesNet<Conditional> marginalBayesNet(const Symbol& key) const;
/** return joint on two variables */ /** return joint on two variables */
template<class Factor> template<class Factor>
FactorGraph<Factor> joint(const std::string& key1, const std::string& key2) const; FactorGraph<Factor> joint(const Symbol& key1, const Symbol& key2) const;
/** return joint on two variables as a BayesNet */ /** return joint on two variables as a BayesNet */
template<class Factor> template<class Factor>
BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const; BayesNet<Conditional> jointBayesNet(const Symbol& key1, const Symbol& key2) const;
/** /**
* Remove path from clique to root and return that path as factors * Remove path from clique to root and return that path as factors

15
cpp/BinaryConditional.h
View File

@ -16,6 +16,7 @@
#include <boost/serialization/list.hpp> #include <boost/serialization/list.hpp>
#include <boost/serialization/vector.hpp> #include <boost/serialization/vector.hpp>
#include "Conditional.h" #include "Conditional.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -26,7 +27,7 @@ namespace gtsam {
private: private:
std::list<std::string> parents_; std::list<Symbol> parents_;
std::vector<double> cpt_; std::vector<double> cpt_;
public: public:
@ -42,7 +43,7 @@ namespace gtsam {
/** /**
* No parents * No parents
*/ */
BinaryConditional(const std::string& key, double p) : BinaryConditional(const Symbol& key, double p) :
Conditional(key) { Conditional(key) {
cpt_.push_back(1-p); cpt_.push_back(1-p);
cpt_.push_back(p); cpt_.push_back(p);
@ -51,7 +52,7 @@ namespace gtsam {
/** /**
* Single parent * Single parent
*/ */
BinaryConditional(const std::string& key, const std::string& parent, const std::vector<double>& cpt) : BinaryConditional(const Symbol& key, const Symbol& parent, const std::vector<double>& cpt) :
Conditional(key) { Conditional(key) {
parents_.push_back(parent); parents_.push_back(parent);
for( int i = 0 ; i < cpt.size() ; i++ ) for( int i = 0 ; i < cpt.size() ; i++ )
@ -59,9 +60,9 @@ namespace gtsam {
cpt_.insert(cpt_.end(),cpt.begin(),cpt.end()); // p(x|parents) cpt_.insert(cpt_.end(),cpt.begin(),cpt.end()); // p(x|parents)
} }
double probability( std::map<std::string,bool> config) { double probability( std::map<Symbol,bool> config) {
int index = 0, count = 1; int index = 0, count = 1;
BOOST_FOREACH( std::string parent, parents_){ BOOST_FOREACH(const Symbol& parent, parents_){
index += count*(int)(config[parent]); index += count*(int)(config[parent]);
count = count << 1; count = count << 1;
} }
@ -72,7 +73,7 @@ namespace gtsam {
/** print */ /** print */
void print(const std::string& s = "BinaryConditional") const { void print(const std::string& s = "BinaryConditional") const {
std::cout << s << " P(" << key_; std::cout << s << " P(" << (std::string)key_;
if (parents_.size()>0) std::cout << " |"; if (parents_.size()>0) std::cout << " |";
BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent; BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
@ -90,7 +91,7 @@ namespace gtsam {
} }
/** return parents */ /** return parents */
std::list<std::string> parents() const { return parents_;} std::list<Symbol> parents() const { return parents_;}
/** return Conditional probability table*/ /** return Conditional probability table*/
std::vector<double> cpt() const { return cpt_;} std::vector<double> cpt() const { return cpt_;}

19
cpp/Conditional.h
View File

@ -13,6 +13,7 @@
#include <boost/serialization/access.hpp> #include <boost/serialization/access.hpp>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include "Testable.h" #include "Testable.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -27,19 +28,15 @@ class Conditional: boost::noncopyable, public Testable<Conditional> {
protected: protected:
/** key of random variable */ /** key of random variable */
std::string key_; Symbol key_;
public: public:
/** empty constructor for serialization */ /** empty constructor for serialization */
Conditional() : Conditional() {}
key_("__unitialized__") {
}
/** constructor */ /** constructor */
Conditional(const std::string& key) : Conditional(const Symbol& key) : key_(key) {}
key_(key) {
}
/* destructor */ /* destructor */
virtual ~Conditional() { virtual ~Conditional() {
@ -51,12 +48,12 @@ public:
} }
/** return key */ /** return key */
inline const std::string& key() const { inline const Symbol& key() const {
return key_; return key_;
} }
/** return parent keys */ /** return parent keys */
virtual std::list<std::string> parents() const = 0; virtual std::list<Symbol> parents() const = 0;
/** return the number of parents */ /** return the number of parents */
virtual std::size_t nrParents() const = 0; virtual std::size_t nrParents() const = 0;
@ -73,9 +70,9 @@ private:
// predicate to check whether a conditional has the sought key // predicate to check whether a conditional has the sought key
template<class Conditional> template<class Conditional>
class onKey { class onKey {
const std::string& key_; const Symbol& key_;
public: public:
onKey(const std::string& key) : onKey(const Symbol& key) :
key_(key) { key_(key) {
} }
bool operator()(const typename Conditional::shared_ptr& conditional) { bool operator()(const typename Conditional::shared_ptr& conditional) {

5
cpp/Factor.h
View File

@ -15,11 +15,12 @@
#include <list> #include <list>
#include <boost/utility.hpp> // for noncopyable #include <boost/utility.hpp> // for noncopyable
#include "Testable.h" #include "Testable.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
/** A map from key to dimension, useful in various contexts */ /** A map from key to dimension, useful in various contexts */
typedef std::map<std::string,int> Dimensions; typedef std::map<Symbol,int> Dimensions;
/** /**
* A simple factor class to use in a factor graph. * A simple factor class to use in a factor graph.
@ -50,7 +51,7 @@ namespace gtsam {
/** /**
* return keys in preferred order * return keys in preferred order
*/ */
virtual std::list<std::string> keys() const = 0; virtual std::list<Symbol> keys() const = 0;
/** /**
* @return the number of nodes the factor connects * @return the number of nodes the factor connects

27
cpp/FactorGraph-inl.h
View File

@ -118,15 +118,14 @@ void FactorGraph<Factor>::replace(int index, sharedFactor factor) {
if(factors_[index] != NULL) { if(factors_[index] != NULL) {
// Remove this factor from its variables' index lists // Remove this factor from its variables' index lists
list<string> keys(factor->keys()); BOOST_FOREACH(const Symbol& key, factor->keys()) {
BOOST_FOREACH(string key, keys) {
Indices::iterator indices = indices_.find(key); Indices::iterator indices = indices_.find(key);
if(indices != indices_.end()) { if(indices != indices_.end()) {
indices->second.remove(index); indices->second.remove(index);
} else { } else {
throw invalid_argument(boost::str(boost::format( throw invalid_argument(boost::str(boost::format(
"Factor graph inconsistency! Factor %d involves variable %s but " "Factor graph inconsistency! Factor %d involves variable %s but "
"is missing from its factor index list.") % index % key)); "is missing from its factor index list.") % index % (std::string)key));
} }
} }
} }
@ -198,16 +197,15 @@ boost::shared_ptr<Ordering> FactorGraph<Factor>::getOrdering_() const{
// A factor graph is really laid out in row-major format, each factor a row // A factor graph is really laid out in row-major format, each factor a row
// Below, we compute a symbolic matrix stored in sparse columns. // Below, we compute a symbolic matrix stored in sparse columns.
typedef string Key; // default case with string keys map<Symbol, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
map<Key, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
int nrNonZeros = 0; // number of non-zero entries int nrNonZeros = 0; // number of non-zero entries
int n_row = factors_.size(); /* colamd arg 1: number of rows in A */ int n_row = factors_.size(); /* colamd arg 1: number of rows in A */
// loop over all factors = rows // loop over all factors = rows
for (int i = 0; i < n_row; i++) { for (int i = 0; i < n_row; i++) {
if (factors_[i]==NULL) continue; if (factors_[i]==NULL) continue;
list<Key> keys = factors_[i]->keys(); list<Symbol> keys = factors_[i]->keys();
BOOST_FOREACH(Key key, keys) columns[key].push_back(i); BOOST_FOREACH(const Symbol& key, keys) columns[key].push_back(i);
nrNonZeros+= keys.size(); nrNonZeros+= keys.size();
} }
int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */ int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
@ -229,7 +227,7 @@ Ordering FactorGraph<Factor>::getOrdering() const {
/** O(1) */ /** O(1) */
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
list<int> FactorGraph<Factor>::factors(const string& key) const { list<int> FactorGraph<Factor>::factors(const Symbol& key) const {
Indices::const_iterator it = indices_.find(key); Indices::const_iterator it = indices_.find(key);
return it->second; return it->second;
} }
@ -239,13 +237,13 @@ list<int> FactorGraph<Factor>::factors(const string& key) const {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
vector<boost::shared_ptr<Factor> > vector<boost::shared_ptr<Factor> >
FactorGraph<Factor>::findAndRemoveFactors(const string& key) { FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
vector<sharedFactor> found; vector<sharedFactor> found;
Indices::iterator it = indices_.find(key); Indices::iterator it = indices_.find(key);
if (it == indices_.end()) if (it == indices_.end())
throw(invalid_argument throw(invalid_argument
("FactorGraph::findAndRemoveFactors invalid key: " + key)); ("FactorGraph::findAndRemoveFactors invalid key: " + (std::string)key));
list<int> *indices_ptr; // pointer to indices list in indices_ map list<int> *indices_ptr; // pointer to indices list in indices_ map
indices_ptr = &(it->second); indices_ptr = &(it->second);
@ -261,15 +259,17 @@ FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor>
void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) { void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
list<string> keys = factor->keys(); // get keys for factor list<Symbol> keys = factor->keys(); // get keys for factor
// rtodo: Can optimize factor->keys to return a const reference
BOOST_FOREACH(string key, keys){ // for each key push i onto list BOOST_FOREACH(const Symbol& key, keys){ // for each key push i onto list
Indices::iterator it = indices_.find(key); // old list for that key (if exists) Indices::iterator it = indices_.find(key); // old list for that key (if exists)
if (it==indices_.end()){ // there's no list yet if (it==indices_.end()){ // there's no list yet
list<int> indices(1,index); // so make one list<int> indices(1,index); // so make one
indices_.insert(make_pair(key,indices)); // insert new indices into factorMap indices_.insert(make_pair(key,indices)); // insert new indices into factorMap
} }
else { else {
// rtodo: what is going on with this pointer?
list<int> *indices_ptr = &(it->second); // get the list list<int> *indices_ptr = &(it->second); // get the list
indices_ptr->push_back(index); // add the index i to it indices_ptr->push_back(index); // add the index i to it
} }
@ -326,12 +326,11 @@ void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree, FactorGraph<Fac
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */ /* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> boost::shared_ptr<Factor> template<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const string& key) removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key)
{ {
vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key); vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
boost::shared_ptr<Factor> new_factor(new Factor(found)); boost::shared_ptr<Factor> new_factor(new Factor(found));

12
cpp/FactorGraph.h
View File

@ -18,6 +18,7 @@
#include "Testable.h" #include "Testable.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "graph.h" #include "graph.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -36,11 +37,12 @@ namespace gtsam {
typedef typename std::vector<sharedFactor>::const_iterator const_iterator; typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
protected: protected:
/** Collection of factors */ /** Collection of factors */
std::vector<sharedFactor> factors_; std::vector<sharedFactor> factors_;
/** For each variable a list of factor indices connected to it */ /** For each variable a list of factor indices connected to it */
typedef std::map<std::string, std::list<int> > Indices; typedef std::map<Symbol, std::list<int> > Indices;
Indices indices_; Indices indices_;
public: public:
@ -90,7 +92,7 @@ namespace gtsam {
Ordering keys() const; Ordering keys() const;
/** Check whether a factor with this variable exists */ /** Check whether a factor with this variable exists */
bool involves(const std::string& key) const { bool involves(const Symbol& key) const {
return !(indices_.find(key)==indices_.end()); return !(indices_.find(key)==indices_.end());
} }
@ -108,14 +110,14 @@ namespace gtsam {
* Return indices for all factors that involve the given node * Return indices for all factors that involve the given node
* @param key the key for the given node * @param key the key for the given node
*/ */
std::list<int> factors(const std::string& key) const; std::list<int> factors(const Symbol& key) const;
/** /**
* find all the factors that involve the given node and remove them * find all the factors that involve the given node and remove them
* from the factor graph * from the factor graph
* @param key the key for the given node * @param key the key for the given node
*/ */
std::vector<sharedFactor> findAndRemoveFactors(const std::string& key); std::vector<sharedFactor> findAndRemoveFactors(const Symbol& key);
/** /**
* find the minimum spanning tree. * find the minimum spanning tree.
@ -149,7 +151,7 @@ namespace gtsam {
* @return the combined linear factor * @return the combined linear factor
*/ */
template<class Factor> boost::shared_ptr<Factor> template<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const std::string& key); removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const Symbol& key);
/** /**
* static function that combines two factor graphs * static function that combines two factor graphs

28
cpp/GaussianBayesNet.cpp
View File

@ -25,7 +25,7 @@ template class BayesNet<GaussianConditional>;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) { GaussianBayesNet scalarGaussian(const Symbol& key, double mu, double sigma) {
GaussianBayesNet bn; GaussianBayesNet bn;
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
conditional(new GaussianConditional(key, Vector_(1,mu), eye(1), Vector_(1,sigma))); conditional(new GaussianConditional(key, Vector_(1,mu), eye(1), Vector_(1,sigma)));
@ -34,7 +34,7 @@ GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigma) { GaussianBayesNet simpleGaussian(const Symbol& key, const Vector& mu, double sigma) {
GaussianBayesNet bn; GaussianBayesNet bn;
size_t n = mu.size(); size_t n = mu.size();
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
@ -44,15 +44,15 @@ GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigm
} }
/* ************************************************************************* */ /* ************************************************************************* */
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const string& name1, Matrix S, Vector sigmas) { const Symbol& name1, Matrix S, Vector sigmas) {
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas)); GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas));
bn.push_front(cg); bn.push_front(cg);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) { const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas) {
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas)); GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas));
bn.push_front(cg); bn.push_front(cg);
} }
@ -78,11 +78,11 @@ VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y) {
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) { BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) {
// i^th part of R*x=y, x=inv(R)*y // i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
const string& i = cg->key(); const Symbol& i = cg->key();
Vector zi = emul(y[i],cg->get_sigmas()); Vector zi = emul(y[i],cg->get_sigmas());
GaussianConditional::const_iterator it; GaussianConditional::const_iterator it;
for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) { for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) {
const string& j = it->first; const Symbol& j = it->first;
const Matrix& Rij = it->second; const Matrix& Rij = it->second;
zi -= Rij * x[j]; zi -= Rij * x[j];
} }
@ -102,7 +102,7 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
// Initialize gy from gx // Initialize gy from gx
VectorConfig gy; VectorConfig gy;
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) { BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
const string& j = cg->key(); const Symbol& j = cg->key();
Vector gyj = gx.contains(j) ? gx[j] : zero(cg->dim()); Vector gyj = gx.contains(j) ? gx[j] : zero(cg->dim());
gy.insert(j,gyj); // initialize result gy.insert(j,gyj); // initialize result
} }
@ -110,12 +110,12 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
// we loop from first-eliminated to last-eliminated // we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L // i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) { BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
const string& j = cg->key(); const Symbol& j = cg->key();
Vector& gyj = gy.getReference(j); // should never fail Vector& gyj = gy.getReference(j); // should never fail
gyj = gtsam::backSubstituteUpper(gyj,cg->get_R()); gyj = gtsam::backSubstituteUpper(gyj,cg->get_R());
GaussianConditional::const_iterator it; GaussianConditional::const_iterator it;
for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) { for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) {
const string& i = it->first; const Symbol& i = it->first;
const Matrix& Rij = it->second; const Matrix& Rij = it->second;
Vector& gyi = gy.getReference(i); // should never fail Vector& gyi = gy.getReference(i); // should never fail
Matrix Lji = trans(Rij); // TODO avoid transpose of matrix ? Matrix Lji = trans(Rij); // TODO avoid transpose of matrix ?
@ -125,7 +125,7 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
// Scale gy // Scale gy
BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) { BOOST_FOREACH(GaussianConditional::shared_ptr cg, bn) {
const string& j = cg->key(); const Symbol& j = cg->key();
Vector& gyj = gy.getReference(j); // should never fail Vector& gyj = gy.getReference(j); // should never fail
gyj = emul(gyj,cg->get_sigmas()); gyj = emul(gyj,cg->get_sigmas());
} }
@ -138,7 +138,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// add the dimensions of all variables to get matrix dimension // add the dimensions of all variables to get matrix dimension
// and at the same time create a mapping from keys to indices // and at the same time create a mapping from keys to indices
size_t N=0; map<string,size_t> mapping; size_t N=0; map<Symbol,size_t> mapping;
BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) { BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) {
mapping.insert(make_pair(cg->key(),N)); mapping.insert(make_pair(cg->key(),N));
N += cg->dim(); N += cg->dim();
@ -147,7 +147,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// create matrix and copy in values // create matrix and copy in values
Matrix R = zeros(N,N); Matrix R = zeros(N,N);
Vector d(N); Vector d(N);
string key; size_t I; Symbol key; size_t I;
FOREACH_PAIR(key,I,mapping) { FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional // find corresponding conditional
GaussianConditional::shared_ptr cg = bn[key]; GaussianConditional::shared_ptr cg = bn[key];

13
cpp/GaussianBayesNet.h
View File

@ -13,6 +13,7 @@
#include "GaussianConditional.h" #include "GaussianConditional.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -20,24 +21,24 @@ namespace gtsam {
typedef BayesNet<GaussianConditional> GaussianBayesNet; typedef BayesNet<GaussianConditional> GaussianBayesNet;
/** Create a scalar Gaussian */ /** Create a scalar Gaussian */
GaussianBayesNet scalarGaussian(const std::string& key, double mu=0.0, double sigma=1.0); GaussianBayesNet scalarGaussian(const Symbol& key, double mu=0.0, double sigma=1.0);
/** Create a simple Gaussian on a single multivariate variable */ /** Create a simple Gaussian on a single multivariate variable */
GaussianBayesNet simpleGaussian(const std::string& key, const Vector& mu, double sigma=1.0); GaussianBayesNet simpleGaussian(const Symbol& key, const Vector& mu, double sigma=1.0);
/** /**
* Add a conditional node with one parent * Add a conditional node with one parent
* |Rx+Sy-d| * |Rx+Sy-d|
*/ */
void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R, void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const std::string& name1, Matrix S, Vector sigmas); const Symbol& name1, Matrix S, Vector sigmas);
/** /**
* Add a conditional node with two parents * Add a conditional node with two parents
* |Rx+Sy+Tz-d| * |Rx+Sy+Tz-d|
*/ */
void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R, void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas); const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas);
/** /**
* optimize, i.e. return x = inv(R)*d * optimize, i.e. return x = inv(R)*d

28
cpp/GaussianConditional.cpp
View File

@ -13,41 +13,41 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(const string& key,Vector d, Matrix R, Vector sigmas) : GaussianConditional::GaussianConditional(const Symbol& key,Vector d, Matrix R, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas),d_(d) Conditional (key), R_(R),sigmas_(sigmas),d_(d)
{ {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(const string& key, Vector d, Matrix R, GaussianConditional::GaussianConditional(const Symbol& key, Vector d, Matrix R,
const string& name1, Matrix S, Vector sigmas) : const Symbol& name1, Matrix S, Vector sigmas) :
Conditional (key), R_(R), sigmas_(sigmas), d_(d) { Conditional (key), R_(R), sigmas_(sigmas), d_(d) {
parents_.insert(make_pair(name1, S)); parents_.insert(make_pair(name1, S));
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(const string& key, Vector d, Matrix R, GaussianConditional::GaussianConditional(const Symbol& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) : const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas), d_(d) { Conditional (key), R_(R),sigmas_(sigmas), d_(d) {
parents_.insert(make_pair(name1, S)); parents_.insert(make_pair(name1, S));
parents_.insert(make_pair(name2, T)); parents_.insert(make_pair(name2, T));
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(const string& key, GaussianConditional::GaussianConditional(const Symbol& key,
const Vector& d, const Matrix& R, const map<string, Matrix>& parents, Vector sigmas) : const Vector& d, const Matrix& R, const map<Symbol, Matrix>& parents, Vector sigmas) :
Conditional (key), R_(R),sigmas_(sigmas), d_(d), parents_(parents) { Conditional (key), R_(R),sigmas_(sigmas), d_(d), parents_(parents) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditional::print(const string &s) const void GaussianConditional::print(const string &s) const
{ {
cout << s << ": density on " << key_ << endl; cout << s << ": density on " << (string)key_ << endl;
gtsam::print(R_,"R"); gtsam::print(R_,"R");
for(Parents::const_iterator it = parents_.begin() ; it != parents_.end() ; it++ ) { for(Parents::const_iterator it = parents_.begin() ; it != parents_.end() ; it++ ) {
const string& j = it->first; const Symbol& j = it->first;
const Matrix& Aj = it->second; const Matrix& Aj = it->second;
gtsam::print(Aj, "A["+j+"]"); gtsam::print(Aj, "A["+(string)j+"]");
} }
gtsam::print(d_,"d"); gtsam::print(d_,"d");
gtsam::print(sigmas_,"sigmas"); gtsam::print(sigmas_,"sigmas");
@ -75,7 +75,7 @@ bool GaussianConditional::equals(const Conditional &c, double tol) const {
// check if the matrices are the same // check if the matrices are the same
// iterate over the parents_ map // iterate over the parents_ map
for (it = parents_.begin(); it != parents_.end(); it++) { for (it = parents_.begin(); it != parents_.end(); it++) {
Parents::const_iterator it2 = p->parents_.find(it->first.c_str()); Parents::const_iterator it2 = p->parents_.find(it->first);
if (it2 != p->parents_.end()) { if (it2 != p->parents_.end()) {
if (!(equal_with_abs_tol(it->second, it2->second, tol))) return false; if (!(equal_with_abs_tol(it->second, it2->second, tol))) return false;
} else } else
@ -85,8 +85,8 @@ bool GaussianConditional::equals(const Conditional &c, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<string> GaussianConditional::parents() const { list<Symbol> GaussianConditional::parents() const {
list<string> result; list<Symbol> result;
for (Parents::const_iterator it = parents_.begin(); it != parents_.end(); it++) for (Parents::const_iterator it = parents_.begin(); it != parents_.end(); it++)
result.push_back(it->first); result.push_back(it->first);
return result; return result;
@ -97,7 +97,7 @@ Vector GaussianConditional::solve(const VectorConfig& x) const {
Vector rhs = d_; Vector rhs = d_;
for (Parents::const_iterator it = parents_.begin(); it for (Parents::const_iterator it = parents_.begin(); it
!= parents_.end(); it++) { != parents_.end(); it++) {
const string& j = it->first; const Symbol& j = it->first;
const Matrix& Aj = it->second; const Matrix& Aj = it->second;
rhs -= Aj * x[j]; rhs -= Aj * x[j];
} }

23
cpp/GaussianConditional.h
View File

@ -18,6 +18,7 @@
#include "Conditional.h" #include "Conditional.h"
#include "VectorConfig.h" #include "VectorConfig.h"
#include "Matrix.h" #include "Matrix.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -31,7 +32,7 @@ class Ordering;
class GaussianConditional : public Conditional { class GaussianConditional : public Conditional {
public: public:
typedef std::map<std::string, Matrix> Parents; typedef std::map<Symbol, Matrix> Parents;
typedef Parents::const_iterator const_iterator; typedef Parents::const_iterator const_iterator;
typedef boost::shared_ptr<GaussianConditional> shared_ptr; typedef boost::shared_ptr<GaussianConditional> shared_ptr;
@ -55,31 +56,31 @@ public:
GaussianConditional(){} GaussianConditional(){}
/** constructor */ /** constructor */
GaussianConditional(const std::string& key) : GaussianConditional(const Symbol& key) :
Conditional (key) {} Conditional (key) {}
/** constructor with no parents /** constructor with no parents
* |Rx-d| * |Rx-d|
*/ */
GaussianConditional(const std::string& key, Vector d, Matrix R, Vector sigmas); GaussianConditional(const Symbol& key, Vector d, Matrix R, Vector sigmas);
/** constructor with only one parent /** constructor with only one parent
* |Rx+Sy-d| * |Rx+Sy-d|
*/ */
GaussianConditional(const std::string& key, Vector d, Matrix R, GaussianConditional(const Symbol& key, Vector d, Matrix R,
const std::string& name1, Matrix S, Vector sigmas); const Symbol& name1, Matrix S, Vector sigmas);
/** constructor with two parents /** constructor with two parents
* |Rx+Sy+Tz-d| * |Rx+Sy+Tz-d|
*/ */
GaussianConditional(const std::string& key, Vector d, Matrix R, GaussianConditional(const Symbol& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas); const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas);
/** /**
* constructor with number of arbitrary parents * constructor with number of arbitrary parents
* |Rx+sum(Ai*xi)-d| * |Rx+sum(Ai*xi)-d|
*/ */
GaussianConditional(const std::string& key, const Vector& d, GaussianConditional(const Symbol& key, const Vector& d,
const Matrix& R, const Parents& parents, Vector sigmas); const Matrix& R, const Parents& parents, Vector sigmas);
/** deconstructor */ /** deconstructor */
@ -95,7 +96,7 @@ public:
size_t dim() const { return R_.size2();} size_t dim() const { return R_.size2();}
/** return all parents */ /** return all parents */
std::list<std::string> parents() const; std::list<Symbol> parents() const;
/** return stuff contained in GaussianConditional */ /** return stuff contained in GaussianConditional */
const Vector& get_d() const {return d_;} const Vector& get_d() const {return d_;}
@ -118,7 +119,7 @@ public:
} }
/** determine whether a key is among the parents */ /** determine whether a key is among the parents */
size_t contains(const std::string& key) const { size_t contains(const Symbol& key) const {
return parents_.count(key); return parents_.count(key);
} }
/** /**
@ -131,7 +132,7 @@ public:
/** /**
* adds a parent * adds a parent
*/ */
void add(const std::string key, Matrix S){ void add(const Symbol& key, Matrix S){
parents_.insert(make_pair(key, S)); parents_.insert(make_pair(key, S));
} }

58
cpp/GaussianFactor.cpp
View File

@ -23,15 +23,16 @@ namespace ublas = boost::numeric::ublas;
using namespace gtsam; using namespace gtsam;
typedef pair<const string, Matrix>& mypair; // richard: commented out this typedef because appears to be unused?
//typedef pair<const Symbol, Matrix>& mypair;
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(const boost::shared_ptr<GaussianConditional>& cg) : GaussianFactor::GaussianFactor(const boost::shared_ptr<GaussianConditional>& cg) :
b_(cg->get_d()) { b_(cg->get_d()) {
As_.insert(make_pair(cg->key(), cg->get_R())); As_.insert(make_pair(cg->key(), cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin(); std::map<Symbol, Matrix>::const_iterator it = cg->parentsBegin();
for (; it != cg->parentsEnd(); it++) { for (; it != cg->parentsEnd(); it++) {
const std::string& j = it->first; const Symbol& j = it->first;
const Matrix& Aj = it->second; const Matrix& Aj = it->second;
As_.insert(make_pair(j, Aj)); As_.insert(make_pair(j, Aj));
} }
@ -80,15 +81,15 @@ void GaussianFactor::print(const string& s) const {
cout << s << endl; cout << s << endl;
if (empty()) cout << " empty" << endl; if (empty()) cout << " empty" << endl;
else { else {
string j; Matrix A; Symbol j; Matrix A;
FOREACH_PAIR(j,A,As_) gtsam::print(A, "A["+j+"]=\n"); FOREACH_PAIR(j,A,As_) gtsam::print(A, "A["+(string)j+"]=\n");
gtsam::print(b_,"b="); gtsam::print(b_,"b=");
gtsam::print(sigmas_, "sigmas = "); gtsam::print(sigmas_, "sigmas = ");
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
size_t GaussianFactor::getDim(const std::string& key) const { size_t GaussianFactor::getDim(const Symbol& key) const {
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
if (it != As_.end()) if (it != As_.end())
return it->second.size2(); return it->second.size2();
@ -109,7 +110,7 @@ bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
if(As_.size() != lf->As_.size()) return false; if(As_.size() != lf->As_.size()) return false;
for(; it1 != As_.end(); it1++, it2++) { for(; it1 != As_.end(); it1++, it2++) {
const string& j1 = it1->first, j2 = it2->first; const Symbol& j1 = it1->first, j2 = it2->first;
const Matrix A1 = it1->second, A2 = it2->second; const Matrix A1 = it1->second, A2 = it2->second;
if (j1 != j2) return false; if (j1 != j2) return false;
if (!equal_with_abs_tol(A1,A2,tol)) if (!equal_with_abs_tol(A1,A2,tol))
@ -129,7 +130,7 @@ bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
Vector GaussianFactor::unweighted_error(const VectorConfig& c) const { Vector GaussianFactor::unweighted_error(const VectorConfig& c) const {
Vector e = -b_; Vector e = -b_;
if (empty()) return e; if (empty()) return e;
string j; Matrix Aj; Symbol j; Matrix Aj; // rtodo: copying matrix here?
FOREACH_PAIR(j, Aj, As_) FOREACH_PAIR(j, Aj, As_)
e += (Aj * c[j]); e += (Aj * c[j]);
return e; return e;
@ -144,14 +145,14 @@ Vector GaussianFactor::error_vector(const VectorConfig& c) const {
/* ************************************************************************* */ /* ************************************************************************* */
double GaussianFactor::error(const VectorConfig& c) const { double GaussianFactor::error(const VectorConfig& c) const {
if (empty()) return 0; if (empty()) return 0;
Vector weighted = error_vector(c); Vector weighted = error_vector(c); // rtodo: copying vector here?
return 0.5 * inner_prod(weighted,weighted); return 0.5 * inner_prod(weighted,weighted);
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<string> GaussianFactor::keys() const { list<Symbol> GaussianFactor::keys() const {
list<string> result; list<Symbol> result;
string j; Matrix A; Symbol j; Matrix A; // rtodo: copying matrix here?
FOREACH_PAIR(j,A,As_) FOREACH_PAIR(j,A,As_)
result.push_back(j); result.push_back(j);
return result; return result;
@ -160,16 +161,16 @@ list<string> GaussianFactor::keys() const {
/* ************************************************************************* */ /* ************************************************************************* */
Dimensions GaussianFactor::dimensions() const { Dimensions GaussianFactor::dimensions() const {
Dimensions result; Dimensions result;
string j; Matrix A; Symbol j; Matrix A; // rtodo: copying matrix here?
FOREACH_PAIR(j,A,As_) FOREACH_PAIR(j,A,As_)
result.insert(make_pair(j,A.size2())); result.insert(make_pair(j,A.size2()));
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactor::tally_separator(const string& key, set<string>& separator) const { void GaussianFactor::tally_separator(const Symbol& key, set<Symbol>& separator) const {
if(involves(key)) { if(involves(key)) {
string j; Matrix A; Symbol j; Matrix A; // rtodo: copying matrix here?
FOREACH_PAIR(j,A,As_) FOREACH_PAIR(j,A,As_)
if(j != key) separator.insert(j); if(j != key) separator.insert(j);
} }
@ -181,7 +182,7 @@ Vector GaussianFactor::operator*(const VectorConfig& x) const {
if (empty()) return Ax; if (empty()) return Ax;
// Just iterate over all A matrices and multiply in correct config part // Just iterate over all A matrices and multiply in correct config part
string j; Matrix Aj; Symbol j; Matrix Aj; // rtodo: copying matrix here?
FOREACH_PAIR(j, Aj, As_) FOREACH_PAIR(j, Aj, As_)
Ax += (Aj * x[j]); Ax += (Aj * x[j]);
@ -193,7 +194,7 @@ VectorConfig GaussianFactor::operator^(const Vector& e) const {
Vector E = ediv_(e,sigmas_); Vector E = ediv_(e,sigmas_);
VectorConfig x; VectorConfig x;
// Just iterate over all A matrices and insert Ai^e into VectorConfig // Just iterate over all A matrices and insert Ai^e into VectorConfig
string j; Matrix Aj; Symbol j; Matrix Aj; // rtodo: copying matrix here?
FOREACH_PAIR(j, Aj, As_) FOREACH_PAIR(j, Aj, As_)
x.insert(j,Aj^E); x.insert(j,Aj^E);
return x; return x;
@ -202,9 +203,10 @@ VectorConfig GaussianFactor::operator^(const Vector& e) const {
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight) const { pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight) const {
// rtodo: this is called in eliminate, potential function to optimize?
// get pointers to the matrices // get pointers to the matrices
vector<const Matrix *> matrices; vector<const Matrix *> matrices;
BOOST_FOREACH(string j, ordering) { BOOST_FOREACH(const Symbol& j, ordering) {
const Matrix& Aj = get_A(j); const Matrix& Aj = get_A(j);
matrices.push_back(&Aj); matrices.push_back(&Aj);
} }
@ -227,7 +229,7 @@ pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight
Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const { Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
// get pointers to the matrices // get pointers to the matrices
vector<const Matrix *> matrices; vector<const Matrix *> matrices;
BOOST_FOREACH(string j, ordering) { BOOST_FOREACH(const Symbol& j, ordering) {
const Matrix& Aj = get_A(j); const Matrix& Aj = get_A(j);
matrices.push_back(&Aj); matrices.push_back(&Aj);
} }
@ -250,7 +252,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
list<double> S; list<double> S;
// iterate over all matrices in the factor // iterate over all matrices in the factor
string key; Matrix Aj; Symbol key; Matrix Aj; // rtodo: copying matrix?
FOREACH_PAIR( key, Aj, As_) { FOREACH_PAIR( key, Aj, As_) {
// find first column index for this key // find first column index for this key
// TODO: check if end() and throw exception if not found // TODO: check if end() and throw exception if not found
@ -275,7 +277,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos) { void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos) {
// iterate over all matrices from the factor f // iterate over all matrices from the factor f
string key; Matrix A; Symbol key; Matrix A; // rtodo: copying matrix?
FOREACH_PAIR( key, A, f->As_) { FOREACH_PAIR( key, A, f->As_) {
// find the corresponding matrix among As // find the corresponding matrix among As
@ -306,10 +308,10 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const string& key) const GaussianFactor::eliminate(const Symbol& key) const
{ {
bool verbose = false; bool verbose = false;
if (verbose) cout << "GaussianFactor::eliminate(" << key << ")" << endl; if (verbose) cout << "GaussianFactor::eliminate(" << (string)key << ")" << endl;
// if this factor does not involve key, we exit with empty CG and LF // if this factor does not involve key, we exit with empty CG and LF
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
@ -323,7 +325,7 @@ GaussianFactor::eliminate(const string& key) const
// create an internal ordering that eliminates key first // create an internal ordering that eliminates key first
Ordering ordering; Ordering ordering;
ordering += key; ordering += key;
BOOST_FOREACH(string k, keys()) BOOST_FOREACH(const Symbol& k, keys())
if (k != key) ordering += k; if (k != key) ordering += k;
// extract A, b from the combined linear factor (ensure that x is leading) // extract A, b from the combined linear factor (ensure that x is leading)
@ -367,7 +369,7 @@ GaussianFactor::eliminate(const string& key) const
// extract the block matrices for parents in both CG and LF // extract the block matrices for parents in both CG and LF
GaussianFactor::shared_ptr factor(new GaussianFactor); GaussianFactor::shared_ptr factor(new GaussianFactor);
size_t j = n1; size_t j = n1;
BOOST_FOREACH(string cur_key, ordering) BOOST_FOREACH(Symbol& cur_key, ordering)
if (cur_key!=key) { if (cur_key!=key) {
size_t dim = getDim(cur_key); size_t dim = getDim(cur_key);
conditional->add(cur_key, sub(R, 0, n1, j, j+dim)); conditional->add(cur_key, sub(R, 0, n1, j, j+dim));
@ -391,13 +393,13 @@ GaussianFactor::eliminate(const string& key) const
// Factor |A1*x+A2*y-b|/sigma -> |A1*(x0+alpha*dx)+A2*(y0+alpha*dy)-b|/sigma // Factor |A1*x+A2*y-b|/sigma -> |A1*(x0+alpha*dx)+A2*(y0+alpha*dy)-b|/sigma
// -> |(A1*dx+A2*dy)*alpha-(b-A1*x0-A2*y0)|/sigma // -> |(A1*dx+A2*dy)*alpha-(b-A1*x0-A2*y0)|/sigma
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x, GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const Symbol& key, const VectorConfig& x,
const VectorConfig& d) const { const VectorConfig& d) const {
// Calculate A matrix // Calculate A matrix
size_t m = b_.size(); size_t m = b_.size();
Vector A = zero(m); Vector A = zero(m);
string j; Matrix Aj; Symbol j; Matrix Aj; // rtodo: copying matrix?
FOREACH_PAIR(j, Aj, As_) FOREACH_PAIR(j, Aj, As_)
A += Aj * d[j]; A += Aj * d[j];
@ -405,7 +407,7 @@ GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x,
Vector b = - unweighted_error(x); Vector b = - unweighted_error(x);
// construct factor // construct factor
shared_ptr factor(new GaussianFactor("alpha",Matrix_(A),b,sigmas_)); shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,sigmas_));
return factor; return factor;
} }

53
cpp/GaussianFactor.h
View File

@ -12,6 +12,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/serialization/map.hpp> #include <boost/serialization/map.hpp>
#include <list>
#include "Factor.h" #include "Factor.h"
#include "Matrix.h" #include "Matrix.h"
@ -31,12 +32,12 @@ class GaussianFactor: boost::noncopyable, public Factor<VectorConfig> {
public: public:
typedef boost::shared_ptr<GaussianFactor> shared_ptr; typedef boost::shared_ptr<GaussianFactor> shared_ptr;
typedef std::map<std::string, Matrix>::iterator iterator; typedef std::map<Symbol, Matrix>::iterator iterator;
typedef std::map<std::string, Matrix>::const_iterator const_iterator; typedef std::map<Symbol, Matrix>::const_iterator const_iterator;
protected: protected:
std::map<std::string, Matrix> As_; // linear matrices std::map<Symbol, Matrix> As_; // linear matrices
Vector b_; // right-hand-side Vector b_; // right-hand-side
Vector sigmas_; // vector of standard deviations for each row in the factor Vector sigmas_; // vector of standard deviations for each row in the factor
@ -52,22 +53,22 @@ public:
} }
/** Construct unary factor */ /** Construct unary factor */
GaussianFactor(const std::string& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, double sigma) : const Vector& b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) { b_(b), sigmas_(repeat(b.size(),sigma)) {
As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key1, A1));
} }
/** Construct unary factor with vector of sigmas*/ /** Construct unary factor with vector of sigmas*/
GaussianFactor(const std::string& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, const Vector& sigmas) : const Vector& b, const Vector& sigmas) :
b_(b), sigmas_(sigmas) { b_(b), sigmas_(sigmas) {
As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key1, A1));
} }
/** Construct binary factor */ /** Construct binary factor */
GaussianFactor(const std::string& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) : const Vector& b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) { b_(b), sigmas_(repeat(b.size(),sigma)) {
As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key1, A1));
@ -75,9 +76,9 @@ public:
} }
/** Construct ternary factor */ /** Construct ternary factor */
GaussianFactor(const std::string& key1, const Matrix& A1, GaussianFactor(const Symbol& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const Symbol& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3, const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) : const Vector& b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) { b_(b), sigmas_(repeat(b.size(),sigma)) {
As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key1, A1));
@ -86,7 +87,7 @@ public:
} }
/** Construct an n-ary factor */ /** Construct an n-ary factor */
GaussianFactor(const std::vector<std::pair<std::string, Matrix> > &terms, GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, double sigma) : const Vector &b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) { b_(b), sigmas_(repeat(b.size(),sigma)) {
for(unsigned int i=0; i<terms.size(); i++) for(unsigned int i=0; i<terms.size(); i++)
@ -94,7 +95,7 @@ public:
} }
/** Construct an n-ary factor with a multiple sigmas*/ /** Construct an n-ary factor with a multiple sigmas*/
GaussianFactor(const std::vector<std::pair<std::string, Matrix> > &terms, GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, const Vector& sigmas) : const Vector &b, const Vector& sigmas) :
b_(b), sigmas_(sigmas) { b_(b), sigmas_(sigmas) {
for (unsigned int i = 0; i < terms.size(); i++) for (unsigned int i = 0; i < terms.size(); i++)
@ -141,20 +142,20 @@ public:
* get a copy of the A matrix from a specific node * get a copy of the A matrix from a specific node
* O(log n) * O(log n)
*/ */
const Matrix& get_A(const std::string& key) const { const Matrix& get_A(const Symbol& key) const {
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
if (it == As_.end()) if (it == As_.end())
throw(std::invalid_argument("GaussianFactor::[] invalid key: " + key)); throw(std::invalid_argument("GaussianFactor::[] invalid key: " + (std::string)key));
return it->second; return it->second;
} }
/** operator[] syntax for get */ /** operator[] syntax for get */
inline const Matrix& operator[](const std::string& name) const { inline const Matrix& operator[](const Symbol& name) const {
return get_A(name); return get_A(name);
} }
/** Check if factor involves variable with key */ /** Check if factor involves variable with key */
bool involves(const std::string& key) const { bool involves(const Symbol& key) const {
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
return (it != As_.end()); return (it != As_.end());
} }
@ -169,19 +170,19 @@ public:
* Find all variables * Find all variables
* @return The set of all variable keys * @return The set of all variable keys
*/ */
std::list<std::string> keys() const; std::list<Symbol> keys() const;
/** /**
* return the first key * return the first key
* @return The set of all variable keys * @return The set of all variable keys
*/ */
std::string key1() const { return As_.begin()->first; } Symbol key1() const { return As_.begin()->first; }
/** /**
* return the first key * return the first key
* @return The set of all variable keys * @return The set of all variable keys
*/ */
std::string key2() const { Symbol key2() const {
if (As_.size() < 2) throw std::invalid_argument("GaussianFactor: less than 2 keys!"); if (As_.size() < 2) throw std::invalid_argument("GaussianFactor: less than 2 keys!");
return (++(As_.begin()))->first; return (++(As_.begin()))->first;
} }
@ -198,15 +199,15 @@ public:
* @param key is the name of the variable * @param key is the name of the variable
* @return the size of the variable * @return the size of the variable
*/ */
size_t getDim(const std::string& key) const; size_t getDim(const Symbol& key) const;
/** /**
* Add to separator set if this factor involves key, but don't add key itself * Add to separator set if this factor involves key, but don't add key itself
* @param key * @param key
* @param separator set to add to * @param separator set to add to
*/ */
void tally_separator(const std::string& key, void tally_separator(const Symbol& key,
std::set<std::string>& separator) const; std::set<Symbol>& separator) const;
/** /**
* Return A*x * Return A*x
@ -247,14 +248,14 @@ public:
* @param x: starting point for search * @param x: starting point for search
* @param d: search direction * @param d: search direction
*/ */
shared_ptr alphaFactor(const VectorConfig& x, const VectorConfig& d) const; shared_ptr alphaFactor(const Symbol& key, const VectorConfig& x, const VectorConfig& d) const;
/* ************************************************************************* */ /* ************************************************************************* */
// MUTABLE functions. FD:on the path to being eradicated // MUTABLE functions. FD:on the path to being eradicated
/* ************************************************************************* */ /* ************************************************************************* */
/** insert, copies A */ /** insert, copies A */
void insert(const std::string& key, const Matrix& A) { void insert(const Symbol& key, const Matrix& A) {
As_.insert(std::make_pair(key, A)); As_.insert(std::make_pair(key, A));
} }
@ -264,7 +265,7 @@ public:
} }
// set A matrices for the linear factor, same as insert ? // set A matrices for the linear factor, same as insert ?
inline void set_A(const std::string& key, const Matrix &A) { inline void set_A(const Symbol& key, const Matrix &A) {
insert(key, A); insert(key, A);
} }
@ -275,7 +276,7 @@ public:
* @return a new factor and a conditional gaussian on the eliminated variable * @return a new factor and a conditional gaussian on the eliminated variable
*/ */
std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr> std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
eliminate(const std::string& key) const; eliminate(const Symbol& key) const;
/** /**
* Take the factor f, and append to current matrices. Not very general. * Take the factor f, and append to current matrices. Not very general.

21
cpp/GaussianFactorGraph.cpp
View File

@ -73,9 +73,9 @@ VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
set<string> GaussianFactorGraph::find_separator(const string& key) const set<Symbol> GaussianFactorGraph::find_separator(const Symbol& key) const
{ {
set<string> separator; set<Symbol> separator;
BOOST_FOREACH(sharedFactor factor,factors_) BOOST_FOREACH(sharedFactor factor,factors_)
factor->tally_separator(key,separator); factor->tally_separator(key,separator);
@ -84,7 +84,7 @@ set<string> GaussianFactorGraph::find_separator(const string& key) const
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
GaussianFactorGraph::eliminateOne(const std::string& key) { GaussianFactorGraph::eliminateOne(const Symbol& key) {
return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*this, key); return gtsam::eliminateOne<GaussianFactor,GaussianConditional>(*this, key);
} }
@ -93,7 +93,7 @@ GaussianBayesNet
GaussianFactorGraph::eliminate(const Ordering& ordering) GaussianFactorGraph::eliminate(const Ordering& ordering)
{ {
GaussianBayesNet chordalBayesNet; // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = eliminateOne(key); GaussianConditional::shared_ptr cg = eliminateOne(key);
chordalBayesNet.push_back(cg); chordalBayesNet.push_back(cg);
} }
@ -115,7 +115,7 @@ boost::shared_ptr<GaussianBayesNet>
GaussianFactorGraph::eliminate_(const Ordering& ordering) GaussianFactorGraph::eliminate_(const Ordering& ordering)
{ {
boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = eliminateOne(key); GaussianConditional::shared_ptr cg = eliminateOne(key);
chordalBayesNet->push_back(cg); chordalBayesNet->push_back(cg);
} }
@ -156,7 +156,7 @@ Dimensions GaussianFactorGraph::dimensions() const {
Dimensions result; Dimensions result;
BOOST_FOREACH(sharedFactor factor,factors_) { BOOST_FOREACH(sharedFactor factor,factors_) {
Dimensions vs = factor->dimensions(); Dimensions vs = factor->dimensions();
string key; int dim; Symbol key; int dim;
FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim)); FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim));
} }
return result; return result;
@ -172,7 +172,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
Dimensions vs = dimensions(); Dimensions vs = dimensions();
// for each of the variables, add a prior // for each of the variables, add a prior
string key; int dim; Symbol key; int dim;
FOREACH_PAIR(key,dim,vs) { FOREACH_PAIR(key,dim,vs) {
Matrix A = eye(dim); Matrix A = eye(dim);
Vector b = zero(dim); Vector b = zero(dim);
@ -214,7 +214,7 @@ Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
// Find the starting index and dimensions for all variables given the order // Find the starting index and dimensions for all variables given the order
size_t j = 1; size_t j = 1;
Dimensions result; Dimensions result;
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
// associate key with first column index // associate key with first column index
result.insert(make_pair(key,j)); result.insert(make_pair(key,j));
// find dimension for this key // find dimension for this key
@ -275,13 +275,14 @@ VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x,
// create a new graph on step-size // create a new graph on step-size
GaussianFactorGraph alphaGraph; GaussianFactorGraph alphaGraph;
Symbol alphaKey('\224', 1);
BOOST_FOREACH(sharedFactor factor,factors_) { BOOST_FOREACH(sharedFactor factor,factors_) {
sharedFactor alphaFactor = factor->alphaFactor(x,d); sharedFactor alphaFactor = factor->alphaFactor(alphaKey, x,d);
alphaGraph.push_back(alphaFactor); alphaGraph.push_back(alphaFactor);
} }
// solve it for optimal step-size alpha // solve it for optimal step-size alpha
GaussianConditional::shared_ptr gc = alphaGraph.eliminateOne("alpha"); GaussianConditional::shared_ptr gc = alphaGraph.eliminateOne(alphaKey);
double alpha = gc->get_d()(0); double alpha = gc->get_d()(0);
cout << alpha << endl; cout << alpha << endl;

18
cpp/GaussianFactorGraph.h
View File

@ -47,28 +47,28 @@ namespace gtsam {
} }
/** Add a unary factor */ /** Add a unary factor */
inline void add(const std::string& key1, const Matrix& A1, inline void add(const Symbol& key1, const Matrix& A1,
const Vector& b, double sigma) { const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,b,sigma))); push_back(sharedFactor(new GaussianFactor(key1,A1,b,sigma)));
} }
/** Add a binary factor */ /** Add a binary factor */
inline void add(const std::string& key1, const Matrix& A1, inline void add(const Symbol& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) { const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,sigma))); push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,sigma)));
} }
/** Add a ternary factor */ /** Add a ternary factor */
inline void add(const std::string& key1, const Matrix& A1, inline void add(const Symbol& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2, const Symbol& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3, const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) { const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,sigma))); push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,sigma)));
} }
/** Add an n-ary factor */ /** Add an n-ary factor */
inline void add(const std::vector<std::pair<std::string, Matrix> > &terms, inline void add(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, double sigma) { const Vector &b, double sigma) {
push_back(sharedFactor(new GaussianFactor(terms,b,sigma))); push_back(sharedFactor(new GaussianFactor(terms,b,sigma)));
} }
@ -101,14 +101,14 @@ namespace gtsam {
* find the separator, i.e. all the nodes that have at least one * find the separator, i.e. all the nodes that have at least one
* common factor with the given node. FD: not used AFAIK. * common factor with the given node. FD: not used AFAIK.
*/ */
std::set<std::string> find_separator(const std::string& key) const; std::set<Symbol> find_separator(const Symbol& key) const;
/** /**
* Eliminate a single node yielding a conditional Gaussian * Eliminate a single node yielding a conditional Gaussian
* Eliminates the factors from the factor graph through findAndRemoveFactors * Eliminates the factors from the factor graph through findAndRemoveFactors
* and adds a new factor on the separator to the factor graph * and adds a new factor on the separator to the factor graph
*/ */
GaussianConditional::shared_ptr eliminateOne(const std::string& key); GaussianConditional::shared_ptr eliminateOne(const Symbol& key);
/** /**
* eliminate factor graph in place(!) in the given order, yielding * eliminate factor graph in place(!) in the given order, yielding

4
cpp/ISAM-inl.h
View File

@ -41,7 +41,7 @@ namespace gtsam {
if (true) { if (true) {
ordering = factors.getOrdering(); ordering = factors.getOrdering();
} else { } else {
list<string> keys = factors.keys(); list<Symbol> keys = factors.keys();
keys.sort(); // todo: correct sorting order? keys.sort(); // todo: correct sorting order?
ordering = keys; ordering = keys;
} }
@ -58,7 +58,7 @@ namespace gtsam {
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
string key = orphan->separator_.front(); Symbol key = orphan->separator_.front();
sharedClique parent = (*this)[key]; sharedClique parent = (*this)[key];
parent->children_ += orphan; parent->children_ += orphan;

20
cpp/ISAM2-inl.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
using namespace std; using namespace std;
// from inference-inl.h - need to additionally return the newly created factor for caching // from inference-inl.h - need to additionally return the newly created factor for caching
boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const string& key) { boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Symbol& key) {
// combine the factors of all nodes connected to the variable to be eliminated // combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor // if no factors are connected to key, returns an empty factor
@ -47,7 +47,7 @@ namespace gtsam {
// from GaussianFactorGraph.cpp, see _eliminateOne above // from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) { GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
chordalBayesNet.push_back(cg); chordalBayesNet.push_back(cg);
} }
@ -98,12 +98,12 @@ namespace gtsam {
BOOST_FOREACH(FactorGraph<GaussianFactor>::sharedFactor fac, affectedFactors) { BOOST_FOREACH(FactorGraph<GaussianFactor>::sharedFactor fac, affectedFactors) {
// retrieve correspondent factor from nonlinearFactors_ // retrieve correspondent factor from nonlinearFactors_
Ordering keys = fac->keys(); Ordering keys = fac->keys();
BOOST_FOREACH(string key, keys) { BOOST_FOREACH(const Symbol& key, keys) {
list<int> indices = nonlinearFactors_.factors(key); list<int> indices = nonlinearFactors_.factors(key);
BOOST_FOREACH(int idx, indices) { BOOST_FOREACH(int idx, indices) {
// todo - only insert index if factor is subset of keys... not needed once we do relinearization - but then how to deal with overlap with orphans? // todo - only insert index if factor is subset of keys... not needed once we do relinearization - but then how to deal with overlap with orphans?
bool subset = true; bool subset = true;
BOOST_FOREACH(string k, nonlinearFactors_[idx]->keys()) { BOOST_FOREACH(const Symbol& k, nonlinearFactors_[idx]->keys()) {
if (find(keys.begin(), keys.end(), k)==keys.end()) subset = false; if (find(keys.begin(), keys.end(), k)==keys.end()) subset = false;
} }
if (subset) { if (subset) {
@ -124,7 +124,7 @@ namespace gtsam {
// retrieve all factors that ONLY contain the affected variables // retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
list<string> affectedKeys = affectedFactors.keys(); list<Symbol> affectedKeys = affectedFactors.keys();
typename FactorGraph<NonlinearFactor<Config> >::iterator it; typename FactorGraph<NonlinearFactor<Config> >::iterator it;
for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) { for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
bool inside = true; bool inside = true;
@ -143,10 +143,10 @@ namespace gtsam {
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that is not part of the separator // find the last variable that is not part of the separator
string oneTooFar = orphan->separator_.front(); string oneTooFar = orphan->separator_.front();
list<string> keys = orphan->keys(); list<Symbol> keys = orphan->keys();
list<string>::iterator it = find(keys.begin(), keys.end(), oneTooFar); list<Symbol>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
it--; it--;
string key = *it; const Symbol& key = *it;
// retrieve the cached factor and add to boundary // retrieve the cached factor and add to boundary
cachedBoundary.push_back(cached[key]); cachedBoundary.push_back(cached[key]);
} }
@ -176,7 +176,7 @@ namespace gtsam {
if (true) { if (true) {
ordering = factors.getOrdering(); ordering = factors.getOrdering();
} else { } else {
list<string> keys = factors.keys(); list<Symbol> keys = factors.keys();
keys.sort(); // todo: correct sorting order? keys.sort(); // todo: correct sorting order?
ordering = keys; ordering = keys;
} }
@ -220,7 +220,7 @@ namespace gtsam {
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
string key = orphan->separator_.front(); Symbol key = orphan->separator_.front();
sharedClique parent = (*this)[key]; sharedClique parent = (*this)[key];
parent->children_ += orphan; parent->children_ += orphan;

3
cpp/ISAM2.h
View File

@ -20,10 +20,11 @@
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "BayesTree.h" #include "BayesTree.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
typedef std::map<std::string, GaussianFactor::shared_ptr> cachedFactors; typedef std::map<Symbol, GaussianFactor::shared_ptr> cachedFactors;
template<class Conditional, class Config> template<class Conditional, class Config>
class ISAM2: public BayesTree<Conditional> { class ISAM2: public BayesTree<Conditional> {

99
cpp/Key.h
View File

@ -10,17 +10,21 @@
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/serialization/serialization.hpp> #include <boost/serialization/serialization.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/integer_traits.hpp>
#define ALPHA '\224'
namespace gtsam { namespace gtsam {
/** /**
* Symbol key class is templated on * TypedSymbol key class is templated on
* 1) the type T it is supposed to retrieve, for extra type checking * 1) the type T it is supposed to retrieve, for extra type checking
* 2) the character constant used for its string representation * 2) the character constant used for its string representation
* TODO: make Testable * TODO: make Testable
*/ */
template <class T, char C> template <class T, char C>
class Symbol { class TypedSymbol {
private: private:
size_t j_; size_t j_;
@ -29,8 +33,8 @@ namespace gtsam {
// Constructors: // Constructors:
Symbol():j_(999999) {} TypedSymbol():j_(boost::integer_traits<size_t>::const_max) {}
Symbol(size_t j):j_(j) {} TypedSymbol(size_t j):j_(j) {}
// Get stuff: // Get stuff:
@ -41,9 +45,9 @@ namespace gtsam {
// logic: // logic:
bool operator< (const Symbol& compare) const { return j_<compare.j_;} bool operator< (const TypedSymbol& compare) const { return j_<compare.j_;}
bool operator== (const Symbol& compare) const { return j_==compare.j_;} bool operator== (const TypedSymbol& compare) const { return j_==compare.j_;}
int compare(const Symbol& compare) const {return j_-compare.j_;} int compare(const TypedSymbol& compare) const {return j_-compare.j_;}
private: private:
@ -53,7 +57,88 @@ namespace gtsam {
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_); ar & BOOST_SERIALIZATION_NVP(j_);
} }
};
/**
* Character and index key used in VectorConfig, GaussianFactorGraph,
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes.
*
* richard: temporarily named 'Symbol' to make refactoring easier
*/
class Symbol {
private:
unsigned char c_;
size_t j_;
public:
/** Default constructor */
Symbol() : c_(0), j_(boost::integer_traits<size_t>::const_max) {}
/** Copy constructor */
Symbol(const Symbol& key) : c_(key.c_), j_(key.j_) {}
/** Constructor */
Symbol(unsigned char c, size_t j): c_(c), j_(j) {}
/** Casting constructor from TypedSymbol */
template<class T, char C>
Symbol(const TypedSymbol<T,C>& symbol): c_(C), j_(symbol.index()) {}
/** "Magic" key casting constructor from string */
#ifdef GTSAM_MAGIC_KEY
Symbol(const std::string& str) {
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
const char *c_str = str.c_str();
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
Symbol(const char *c_str) {
std::string str(c_str);
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
#endif
/** Retrieve key character */
unsigned char chr() const { return c_; }
/** Retrieve key index */
size_t index() const { return j_; }
/** Create a string from the key */
operator std::string() const { return str(boost::format("%c%d") % c_ % j_); }
/** Comparison for use in maps */
bool operator< (const Symbol& comp) const { return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); }
bool operator== (const Symbol& comp) const { return comp.c_ == c_ && comp.j_ == j_; }
bool operator!= (const Symbol& comp) const { return comp.c_ != c_ || comp.j_ != j_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_);
}
}; };
} // namespace gtsam } // namespace gtsam

6
cpp/LieConfig-inl.h
View File

@ -80,9 +80,9 @@ namespace gtsam {
BOOST_FOREACH(const Value& value, c) { BOOST_FOREACH(const Value& value, c) {
const J& j = value.first; const J& j = value.first;
const T& pj = value.second; const T& pj = value.second;
string jstr = (string)j; Symbol jkey = (Symbol)j;
if (delta.contains(jstr)) { if (delta.contains(jkey)) {
const Vector& dj = delta[jstr]; const Vector& dj = delta[jkey];
newConfig.insert(j, expmap(pj,dj)); newConfig.insert(j, expmap(pj,dj));
} else } else
newConfig.insert(j, pj); newConfig.insert(j, pj);

15
cpp/NonlinearFactor.h
View File

@ -46,11 +46,11 @@ namespace gtsam {
protected: protected:
double sigma_; // noise standard deviation
std::list<std::string> keys_; // keys
typedef NonlinearFactor<Config> This; typedef NonlinearFactor<Config> This;
double sigma_; // noise standard deviation
std::list<Symbol> keys_; // keys
public: public:
/** Default constructor for I/O only */ /** Default constructor for I/O only */
@ -96,7 +96,7 @@ namespace gtsam {
; ;
/** return keys */ /** return keys */
std::list<std::string> keys() const { std::list<Symbol> keys() const {
return keys_; return keys_;
} }
@ -194,8 +194,8 @@ namespace gtsam {
const X& xj = x[key_]; const X& xj = x[key_];
Matrix A; Matrix A;
Vector b = -evaluateError(xj, A); Vector b = -evaluateError(xj, A);
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, return GaussianFactor::shared_ptr(new GaussianFactor(
this->sigma())); key_, A, b, this->sigma()));
} }
/* /*
@ -288,7 +288,8 @@ namespace gtsam {
const X2& x2 = c[key2_]; const X2& x2 = c[key2_];
Matrix A1, A2; Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2); Vector b = -evaluateError(x1, x2, A1, A2);
return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_, return GaussianFactor::shared_ptr(new GaussianFactor(
key1_, A1, key2_,
A2, b, this->sigma())); A2, b, this->sigma()));
} }

6
cpp/Ordering.cpp
View File

@ -22,7 +22,7 @@ using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::subtract(const Ordering& keys) const { Ordering Ordering::subtract(const Ordering& keys) const {
Ordering newOrdering = *this; Ordering newOrdering = *this;
BOOST_FOREACH(string key, keys) { BOOST_FOREACH(const Symbol& key, keys) {
newOrdering.remove(key); newOrdering.remove(key);
} }
return newOrdering; return newOrdering;
@ -31,8 +31,8 @@ Ordering Ordering::subtract(const Ordering& keys) const {
/* ************************************************************************* */ /* ************************************************************************* */
void Ordering::print(const string& s) const { void Ordering::print(const string& s) const {
cout << s << " (" << size() << "):"; cout << s << " (" << size() << "):";
BOOST_FOREACH(string key, *this) BOOST_FOREACH(const Symbol& key, *this)
cout << " " << key; cout << " " << (string)key;
cout << endl; cout << endl;
} }

9
cpp/Ordering.h
View File

@ -11,6 +11,7 @@
#include <map> #include <map>
#include "Testable.h" #include "Testable.h"
#include "graph.h" #include "graph.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -18,7 +19,7 @@ namespace gtsam {
* @class Ordering * @class Ordering
* @brief ordering of indices for eliminating a factor graph * @brief ordering of indices for eliminating a factor graph
*/ */
class Ordering: public std::list<std::string>, public Testable<Ordering> { class Ordering: public std::list<Symbol>, public Testable<Ordering> {
public: public:
/** /**
* Default constructor creates empty ordering * Default constructor creates empty ordering
@ -29,15 +30,15 @@ namespace gtsam {
/** /**
* Create from a single string * Create from a single string
*/ */
Ordering(std::string key) { Ordering(Symbol key) {
push_back(key); push_back(key);
} }
/** /**
* Copy constructor from string vector * Copy constructor from string vector
*/ */
Ordering(const std::list<std::string>& strings_in) : Ordering(const std::list<Symbol>& keys_in) :
std::list<std::string>(strings_in) { std::list<Symbol>(keys_in) {
} }
/** /**

7
cpp/Simulated3D.h
View File

@ -11,6 +11,7 @@
#include "Matrix.h" #include "Matrix.h"
#include "VectorConfig.h" #include "VectorConfig.h"
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "Key.h"
// \namespace // \namespace
@ -18,10 +19,8 @@ namespace simulated3D {
typedef gtsam::VectorConfig VectorConfig; typedef gtsam::VectorConfig VectorConfig;
struct PoseKey: public std::string { typedef gtsam::Symbol PoseKey;
}; typedef gtsam::Symbol PointKey;
struct PointKey: public std::string {
};
/** /**
* Prior on a single pose * Prior on a single pose

6
cpp/SubgraphPreconditioner.cpp
View File

@ -30,7 +30,7 @@ namespace gtsam {
// Use BayesNet order to add y contributions in order // Use BayesNet order to add y contributions in order
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) { BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
const string& j = cg->key(); const Symbol& j = cg->key();
e.push_back(y[j]); // append y e.push_back(y[j]); // append y
} }
@ -59,7 +59,7 @@ namespace gtsam {
// Use BayesNet order to add y contributions in order // Use BayesNet order to add y contributions in order
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) { BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
const string& j = cg->key(); const Symbol& j = cg->key();
e.push_back(y[j]); // append y e.push_back(y[j]); // append y
} }
@ -80,7 +80,7 @@ namespace gtsam {
// Use BayesNet order to remove y contributions in order // Use BayesNet order to remove y contributions in order
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) { BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
const string& j = cg->key(); const Symbol& j = cg->key();
const Vector& ej = *(it++); const Vector& ej = *(it++);
y1.insert(j,ej); y1.insert(j,ej);
} }

5
cpp/SymbolicBayesNet.h
View File

@ -15,6 +15,7 @@
#include "Testable.h" #include "Testable.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "SymbolicConditional.h" #include "SymbolicConditional.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -32,8 +33,8 @@ namespace gtsam {
typename BayesNet<Conditional>::const_iterator it = bn.begin(); typename BayesNet<Conditional>::const_iterator it = bn.begin();
for (; it != bn.end(); it++) { for (; it != bn.end(); it++) {
boost::shared_ptr<Conditional> conditional = *it; boost::shared_ptr<Conditional> conditional = *it;
std::string key = conditional->key(); Symbol key = conditional->key();
std::list<std::string> parents = conditional->parents(); std::list<Symbol> parents = conditional->parents();
SymbolicConditional::shared_ptr c(new SymbolicConditional(key, parents)); SymbolicConditional::shared_ptr c(new SymbolicConditional(key, parents));
result.push_back(c); result.push_back(c);
} }

26
cpp/SymbolicConditional.h
View File

@ -15,6 +15,7 @@
#include <boost/foreach.hpp> // TODO: make cpp file #include <boost/foreach.hpp> // TODO: make cpp file
#include <boost/serialization/list.hpp> #include <boost/serialization/list.hpp>
#include "Conditional.h" #include "Conditional.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -24,8 +25,7 @@ namespace gtsam {
class SymbolicConditional: public Conditional { class SymbolicConditional: public Conditional {
private: private:
std::list<Symbol> parents_;
std::list<std::string> parents_;
public: public:
@ -40,14 +40,14 @@ namespace gtsam {
/** /**
* No parents * No parents
*/ */
SymbolicConditional(const std::string& key) : SymbolicConditional(const Symbol& key) :
Conditional(key) { Conditional(key) {
} }
/** /**
* Single parent * Single parent
*/ */
SymbolicConditional(const std::string& key, const std::string& parent) : SymbolicConditional(const Symbol& key, const Symbol& parent) :
Conditional(key) { Conditional(key) {
parents_.push_back(parent); parents_.push_back(parent);
} }
@ -55,8 +55,8 @@ namespace gtsam {
/** /**
* Two parents * Two parents
*/ */
SymbolicConditional(const std::string& key, const std::string& parent1, SymbolicConditional(const Symbol& key, const Symbol& parent1,
const std::string& parent2) : const Symbol& parent2) :
Conditional(key) { Conditional(key) {
parents_.push_back(parent1); parents_.push_back(parent1);
parents_.push_back(parent2); parents_.push_back(parent2);
@ -65,8 +65,8 @@ namespace gtsam {
/** /**
* Three parents * Three parents
*/ */
SymbolicConditional(const std::string& key, const std::string& parent1, SymbolicConditional(const Symbol& key, const Symbol& parent1,
const std::string& parent2, const std::string& parent3) : const Symbol& parent2, const Symbol& parent3) :
Conditional(key) { Conditional(key) {
parents_.push_back(parent1); parents_.push_back(parent1);
parents_.push_back(parent2); parents_.push_back(parent2);
@ -76,16 +76,16 @@ namespace gtsam {
/** /**
* A list * A list
*/ */
SymbolicConditional(const std::string& key, SymbolicConditional(const Symbol& key,
const std::list<std::string>& parents) : const std::list<Symbol>& parents) :
Conditional(key), parents_(parents) { Conditional(key), parents_(parents) {
} }
/** print */ /** print */
void print(const std::string& s = "SymbolicConditional") const { void print(const std::string& s = "SymbolicConditional") const {
std::cout << s << " P(" << key_; std::cout << s << " P(" << (std::string)key_;
if (parents_.size()>0) std::cout << " |"; if (parents_.size()>0) std::cout << " |";
BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent; BOOST_FOREACH(const Symbol& parent, parents_) std::cout << " " << (std::string)parent;
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
} }
@ -98,7 +98,7 @@ namespace gtsam {
} }
/** return parents */ /** return parents */
std::list<std::string> parents() const { return parents_;} std::list<Symbol> parents() const { return parents_;}
/** find the number of parents */ /** find the number of parents */
size_t nrParents() const { size_t nrParents() const {

14
cpp/SymbolicFactor.cpp
View File

@ -31,13 +31,13 @@ namespace gtsam {
SymbolicFactor::SymbolicFactor(const vector<shared_ptr> & factors) { SymbolicFactor::SymbolicFactor(const vector<shared_ptr> & factors) {
// store keys in a map to make them unique (set is not portable) // store keys in a map to make them unique (set is not portable)
map<string, string> map; map<Symbol, Symbol> map;
BOOST_FOREACH(shared_ptr factor, factors) BOOST_FOREACH(shared_ptr factor, factors)
BOOST_FOREACH(string key, factor->keys()) BOOST_FOREACH(const Symbol& key, factor->keys())
map.insert(make_pair(key,key)); map.insert(make_pair(key,key));
// create the unique keys // create the unique keys
string key,val; Symbol key,val;
FOREACH_PAIR(key, val, map) FOREACH_PAIR(key, val, map)
keys_.push_back(key); keys_.push_back(key);
} }
@ -45,7 +45,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void SymbolicFactor::print(const string& s) const { void SymbolicFactor::print(const string& s) const {
cout << s << " "; cout << s << " ";
BOOST_FOREACH(string key, keys_) cout << " " << key; BOOST_FOREACH(const Symbol& key, keys_) cout << " " << (string)key;
cout << endl; cout << endl;
} }
@ -56,11 +56,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr> pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
SymbolicFactor::eliminate(const string& key) const SymbolicFactor::eliminate(const Symbol& key) const
{ {
// get keys from input factor // get keys from input factor
list<string> separator; list<Symbol> separator;
BOOST_FOREACH(string j,keys_) BOOST_FOREACH(const Symbol& j,keys_)
if (j!=key) separator.push_back(j); if (j!=key) separator.push_back(j);
// start empty remaining factor to be returned // start empty remaining factor to be returned

13
cpp/SymbolicFactor.h
View File

@ -14,6 +14,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include "Testable.h" #include "Testable.h"
#include "Ordering.h" #include "Ordering.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -23,7 +24,7 @@ namespace gtsam {
class SymbolicFactor: public Testable<SymbolicFactor> { class SymbolicFactor: public Testable<SymbolicFactor> {
private: private:
std::list<std::string> keys_; std::list<Symbol> keys_;
public: public:
@ -38,18 +39,18 @@ namespace gtsam {
} }
/** Construct unary factor */ /** Construct unary factor */
SymbolicFactor(const std::string& key) { SymbolicFactor(const Symbol& key) {
keys_.push_back(key); keys_.push_back(key);
} }
/** Construct binary factor */ /** Construct binary factor */
SymbolicFactor(const std::string& key1, const std::string& key2) { SymbolicFactor(const Symbol& key1, const Symbol& key2) {
keys_.push_back(key1); keys_.push_back(key1);
keys_.push_back(key2); keys_.push_back(key2);
} }
/** Construct ternary factor */ /** Construct ternary factor */
SymbolicFactor(const std::string& key1, const std::string& key2, const std::string& key3) { SymbolicFactor(const Symbol& key1, const Symbol& key2, const Symbol& key3) {
keys_.push_back(key1); keys_.push_back(key1);
keys_.push_back(key2); keys_.push_back(key2);
keys_.push_back(key3); keys_.push_back(key3);
@ -71,7 +72,7 @@ namespace gtsam {
* Find all variables * Find all variables
* @return The set of all variable keys * @return The set of all variable keys
*/ */
std::list<std::string> keys() const { std::list<Symbol> keys() const {
return keys_; return keys_;
} }
@ -81,7 +82,7 @@ namespace gtsam {
* @return a new factor and a symbolic conditional on the eliminated variable * @return a new factor and a symbolic conditional on the eliminated variable
*/ */
std::pair<boost::shared_ptr<SymbolicConditional>, shared_ptr> std::pair<boost::shared_ptr<SymbolicConditional>, shared_ptr>
eliminate(const std::string& key) const; eliminate(const Symbol& key) const;
/** /**
* Check if empty factor * Check if empty factor

4
cpp/SymbolicFactorGraph.cpp
View File

@ -20,7 +20,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<SymbolicConditional> boost::shared_ptr<SymbolicConditional>
SymbolicFactorGraph::eliminateOne(const std::string& key){ SymbolicFactorGraph::eliminateOne(const Symbol& key){
return gtsam::eliminateOne<SymbolicFactor,SymbolicConditional>(*this, key); return gtsam::eliminateOne<SymbolicFactor,SymbolicConditional>(*this, key);
} }
@ -30,7 +30,7 @@ namespace gtsam {
{ {
SymbolicBayesNet bayesNet; SymbolicBayesNet bayesNet;
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
SymbolicConditional::shared_ptr conditional = SymbolicConditional::shared_ptr conditional =
gtsam::eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key); gtsam::eliminateOne<SymbolicFactor,SymbolicConditional>(*this,key);
bayesNet.push_back(conditional); bayesNet.push_back(conditional);

11
cpp/SymbolicFactorGraph.h
View File

@ -13,6 +13,7 @@
#include "FactorGraph.h" #include "FactorGraph.h"
#include "SymbolicFactor.h" #include "SymbolicFactor.h"
#include "SymbolicBayesNet.h" #include "SymbolicBayesNet.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -26,19 +27,19 @@ namespace gtsam {
SymbolicFactorGraph() {} SymbolicFactorGraph() {}
/** Push back unary factor */ /** Push back unary factor */
void push_factor(const std::string& key) { void push_factor(const Symbol& key) {
boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key)); boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key));
push_back(factor); push_back(factor);
} }
/** Push back binary factor */ /** Push back binary factor */
void push_factor(const std::string& key1, const std::string& key2) { void push_factor(const Symbol& key1, const Symbol& key2) {
boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key1,key2)); boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key1,key2));
push_back(factor); push_back(factor);
} }
/** Push back ternary factor */ /** Push back ternary factor */
void push_factor(const std::string& key1, const std::string& key2, const std::string& key3) { void push_factor(const Symbol& key1, const Symbol& key2, const Symbol& key3) {
boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key1,key2,key3)); boost::shared_ptr<SymbolicFactor> factor(new SymbolicFactor(key1,key2,key3));
push_back(factor); push_back(factor);
} }
@ -50,7 +51,7 @@ namespace gtsam {
SymbolicFactorGraph(const FactorGraph<Factor>& fg) { SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
for (size_t i = 0; i < fg.size(); i++) { for (size_t i = 0; i < fg.size(); i++) {
boost::shared_ptr<Factor> f = fg[i]; boost::shared_ptr<Factor> f = fg[i];
std::list<std::string> keys = f->keys(); std::list<Symbol> keys = f->keys();
SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys)); SymbolicFactor::shared_ptr factor(new SymbolicFactor(keys));
push_back(factor); push_back(factor);
} }
@ -61,7 +62,7 @@ namespace gtsam {
* Eliminates the factors from the factor graph through findAndRemoveFactors * Eliminates the factors from the factor graph through findAndRemoveFactors
* and adds a new factor on the separator to the factor graph * and adds a new factor on the separator to the factor graph
*/ */
boost::shared_ptr<SymbolicConditional> eliminateOne(const std::string& key); boost::shared_ptr<SymbolicConditional> eliminateOne(const Symbol& key);
/** /**
* eliminate factor graph in place(!) in the given order, yielding * eliminate factor graph in place(!) in the given order, yielding

44
cpp/VectorConfig.cpp
View File

@ -18,9 +18,9 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void check_size(const string& key, const Vector & vj, const Vector & dj) { void check_size(const Symbol& key, const Vector & vj, const Vector & dj) {
if (dj.size()!=vj.size()) { if (dj.size()!=vj.size()) {
cout << "For key \"" << key << "\"" << endl; cout << "For key \"" << (string)key << "\"" << endl;
cout << "vj.size = " << vj.size() << endl; cout << "vj.size = " << vj.size() << endl;
cout << "dj.size = " << dj.size() << endl; cout << "dj.size = " << dj.size() << endl;
throw(std::invalid_argument("VectorConfig::+ mismatched dimensions")); throw(std::invalid_argument("VectorConfig::+ mismatched dimensions"));
@ -28,21 +28,21 @@ void check_size(const string& key, const Vector & vj, const Vector & dj) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<std::string> VectorConfig::get_names() const { std::vector<Symbol> VectorConfig::get_names() const {
std::vector<std::string> names; std::vector<Symbol> names;
for(const_iterator it=values.begin(); it!=values.end(); it++) for(const_iterator it=values.begin(); it!=values.end(); it++)
names.push_back(it->first); names.push_back(it->first);
return names; return names;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig& VectorConfig::insert(const std::string& name, const Vector& val) { VectorConfig& VectorConfig::insert(const Symbol& name, const Vector& val) {
values.insert(std::make_pair(name,val)); values.insert(std::make_pair(name,val));
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorConfig::add(const std::string& j, const Vector& a) { void VectorConfig::add(const Symbol& j, const Vector& a) {
Vector& vj = values[j]; Vector& vj = values[j];
if (vj.size()==0) vj = a; else vj += a; if (vj.size()==0) vj = a; else vj += a;
} }
@ -50,7 +50,7 @@ void VectorConfig::add(const std::string& j, const Vector& a) {
/* ************************************************************************* */ /* ************************************************************************* */
size_t VectorConfig::dim() const { size_t VectorConfig::dim() const {
size_t result=0; size_t result=0;
string key; Vector v; Symbol key; Vector v; // rtodo: copying vector?
FOREACH_PAIR(key, v, values) result += v.size(); FOREACH_PAIR(key, v, values) result += v.size();
return result; return result;
} }
@ -58,7 +58,7 @@ size_t VectorConfig::dim() const {
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig VectorConfig::scale(double s) const { VectorConfig VectorConfig::scale(double s) const {
VectorConfig scaled; VectorConfig scaled;
string key; Vector val; Symbol key; Vector val; // rtodo: copying vector?
FOREACH_PAIR(key, val, values) FOREACH_PAIR(key, val, values)
scaled.insert(key, s*val); scaled.insert(key, s*val);
return scaled; return scaled;
@ -72,7 +72,7 @@ VectorConfig VectorConfig::operator*(double s) const {
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig VectorConfig::operator-() const { VectorConfig VectorConfig::operator-() const {
VectorConfig result; VectorConfig result;
string j; Vector v; Symbol j; Vector v; // rtodo: copying vector?
FOREACH_PAIR(j, v, values) FOREACH_PAIR(j, v, values)
result.insert(j, -v); result.insert(j, -v);
return result; return result;
@ -80,7 +80,7 @@ VectorConfig VectorConfig::operator-() const {
/* ************************************************************************* */ /* ************************************************************************* */
void VectorConfig::operator+=(const VectorConfig& b) { void VectorConfig::operator+=(const VectorConfig& b) {
string j; Vector b_j; Symbol j; Vector b_j; // rtodo: copying vector?
FOREACH_PAIR(j, b_j, b.values) { FOREACH_PAIR(j, b_j, b.values) {
iterator it = values.find(j); iterator it = values.find(j);
if (it==values.end()) if (it==values.end())
@ -100,7 +100,7 @@ VectorConfig VectorConfig::operator+(const VectorConfig& b) const {
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig VectorConfig::operator-(const VectorConfig& b) const { VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
VectorConfig result; VectorConfig result;
string j; Vector v; Symbol j; Vector v; // rtodo: copying vector?
FOREACH_PAIR(j, v, values) FOREACH_PAIR(j, v, values)
result.insert(j, v - b.get(j)); result.insert(j, v - b.get(j));
return result; return result;
@ -110,7 +110,7 @@ VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta) VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta)
{ {
VectorConfig newConfig; VectorConfig newConfig;
string j; Vector vj; Symbol j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) { FOREACH_PAIR(j, vj, original.values) {
if (delta.contains(j)) { if (delta.contains(j)) {
const Vector& dj = delta[j]; const Vector& dj = delta[j];
@ -128,7 +128,7 @@ VectorConfig expmap(const VectorConfig& original, const Vector& delta)
{ {
VectorConfig newConfig; VectorConfig newConfig;
size_t i = 0; size_t i = 0;
string j; Vector vj; Symbol j; Vector vj; // rtodo: copying vector?
FOREACH_PAIR(j, vj, original.values) { FOREACH_PAIR(j, vj, original.values) {
size_t mj = vj.size(); size_t mj = vj.size();
Vector dj = sub(delta, i, i+mj); Vector dj = sub(delta, i, i+mj);
@ -139,41 +139,41 @@ VectorConfig expmap(const VectorConfig& original, const Vector& delta)
} }
/* ************************************************************************* */ /* ************************************************************************* */
const Vector& VectorConfig::get(const std::string& name) const { const Vector& VectorConfig::get(const Symbol& name) const {
const_iterator it = values.find(name); const_iterator it = values.find(name);
if (it==values.end()) { if (it==values.end()) {
print(); print();
cout << "asked for key " << name << endl; cout << "asked for key " << (string)name << endl;
throw(std::invalid_argument("VectorConfig::[] invalid key")); throw(std::invalid_argument("VectorConfig::[] invalid key"));
} }
return it->second; return it->second;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector& VectorConfig::getReference(const std::string& name) { Vector& VectorConfig::getReference(const Symbol& name) {
iterator it = values.find(name); iterator it = values.find(name);
if (it==values.end()) { if (it==values.end()) {
print(); print();
cout << "asked for key " << name << endl; cout << "asked for key " << (string)name << endl;
throw(std::invalid_argument("VectorConfig::[] invalid key")); throw(std::invalid_argument("VectorConfig::[] invalid key"));
} }
return it->second; return it->second;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorConfig::print(const std::string& name) const { void VectorConfig::print(const string& name) const {
odprintf("VectorConfig %s\n", name.c_str()); odprintf("VectorConfig %s\n", name.c_str());
odprintf("size: %d\n", values.size()); odprintf("size: %d\n", values.size());
string j; Vector v; Symbol j; Vector v; // rtodo: copying vector
FOREACH_PAIR(j, v, values) { FOREACH_PAIR(j, v, values) {
odprintf("%s:", j.c_str()); odprintf("%s:", ((string)j).c_str());
gtsam::print(v); gtsam::print(v);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool VectorConfig::equals(const VectorConfig& expected, double tol) const { bool VectorConfig::equals(const VectorConfig& expected, double tol) const {
string j; Vector vActual; Symbol j; Vector vActual; // rtodo: copying vector
if( values.size() != expected.size() ) return false; if( values.size() != expected.size() ) return false;
// iterate over all nodes // iterate over all nodes
@ -187,7 +187,7 @@ bool VectorConfig::equals(const VectorConfig& expected, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
double VectorConfig::dot(const VectorConfig& b) const { double VectorConfig::dot(const VectorConfig& b) const {
string j; Vector v; double result = 0.0; Symbol j; Vector v; double result = 0.0; // rtodo: copying vector
FOREACH_PAIR(j, v, values) result += gtsam::dot(v,b.get(j)); FOREACH_PAIR(j, v, values) result += gtsam::dot(v,b.get(j));
return result; return result;
} }

23
cpp/VectorConfig.h
View File

@ -14,6 +14,7 @@
#include "Testable.h" #include "Testable.h"
#include "Vector.h" #include "Vector.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -22,42 +23,42 @@ namespace gtsam {
protected: protected:
/** Map from string indices to values */ /** Map from string indices to values */
std::map<std::string, Vector> values; std::map<Symbol, Vector> values;
public: public:
typedef std::map<std::string, Vector>::iterator iterator; typedef std::map<Symbol, Vector>::iterator iterator;
typedef std::map<std::string, Vector>::const_iterator const_iterator; typedef std::map<Symbol, Vector>::const_iterator const_iterator;
VectorConfig() {} VectorConfig() {}
VectorConfig(const VectorConfig& cfg_in): values(cfg_in.values) {} VectorConfig(const VectorConfig& cfg_in): values(cfg_in.values) {}
VectorConfig(const std::string& j, const Vector& a) { add(j,a); } VectorConfig(const Symbol& j, const Vector& a) { add(j,a); }
virtual ~VectorConfig() {} virtual ~VectorConfig() {}
/** return all the nodes in the graph **/ /** return all the nodes in the graph **/
std::vector<std::string> get_names() const; std::vector<Symbol> get_names() const;
/** Insert a value into the configuration with a given index */ /** Insert a value into the configuration with a given index */
VectorConfig& insert(const std::string& name, const Vector& val); VectorConfig& insert(const Symbol& name, const Vector& val);
/** Add to vector at position j */ /** Add to vector at position j */
void add(const std::string& j, const Vector& a); void add(const Symbol& j, const Vector& a);
const_iterator begin() const {return values.begin();} const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();} const_iterator end() const {return values.end();}
/** get a vector in the configuration by name */ /** get a vector in the configuration by name */
const Vector& get(const std::string& name) const; const Vector& get(const Symbol& name) const;
/** get a vector reference by name */ /** get a vector reference by name */
Vector& getReference(const std::string& name); Vector& getReference(const Symbol& name);
/** operator[] syntax for get */ /** operator[] syntax for get */
inline const Vector& operator[](const std::string& name) const { inline const Vector& operator[](const Symbol& name) const {
return get(name); return get(name);
} }
bool contains(const std::string& name) const { bool contains(const Symbol& name) const {
const_iterator it = values.find(name); const_iterator it = values.find(name);
return (it!=values.end()); return (it!=values.end());
} }

2
cpp/graph.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
template<class Key> template<class Key>
class PredecessorMap: public std::map<Key, Key> { class PredecessorMap: public std::map<Key, Key> {
public: public:
/** convenience insert so we can pass ints for Symbol keys */ /** convenience insert so we can pass ints for TypedSymbol keys */
inline void insert(const Key& key, const Key& parent) { inline void insert(const Key& key, const Key& parent) {
std::map<Key, Key>::insert(std::make_pair(key, parent)); std::map<Key, Key>::insert(std::make_pair(key, parent));
} }

9
cpp/inference-inl.h
View File

@ -4,9 +4,10 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
//#include "inference.h" #include "inference.h"
#include "FactorGraph-inl.h" #include "FactorGraph-inl.h"
#include "BayesNet-inl.h" #include "BayesNet-inl.h"
#include "Key.h"
using namespace std; using namespace std;
@ -16,7 +17,7 @@ namespace gtsam {
/* eliminate one node from the factor graph */ /* eliminate one node from the factor graph */
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor,class Conditional> template<class Factor,class Conditional>
boost::shared_ptr<Conditional> eliminateOne(FactorGraph<Factor>& graph, const string& key) { boost::shared_ptr<Conditional> eliminateOne(FactorGraph<Factor>& graph, const Symbol& key) {
// combine the factors of all nodes connected to the variable to be eliminated // combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor // if no factors are connected to key, returns an empty factor
@ -44,7 +45,7 @@ namespace gtsam {
{ {
BayesNet<Conditional> bayesNet; // empty BayesNet<Conditional> bayesNet; // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(Symbol key, ordering) {
boost::shared_ptr<Conditional> cg = eliminateOne<Factor,Conditional>(factorGraph,key); boost::shared_ptr<Conditional> cg = eliminateOne<Factor,Conditional>(factorGraph,key);
bayesNet.push_back(cg); bayesNet.push_back(cg);
} }
@ -61,7 +62,7 @@ namespace gtsam {
// Get the keys of all variables and remove all keys we want the marginal for // Get the keys of all variables and remove all keys we want the marginal for
Ordering ord = bn.ordering(); Ordering ord = bn.ordering();
BOOST_FOREACH(string key, keys) ord.remove(key); // TODO: O(n*k), faster possible? BOOST_FOREACH(const Symbol& key, keys) ord.remove(key); // TODO: O(n*k), faster possible?
// eliminate partially, // eliminate partially,
BayesNet<Conditional> conditional = eliminate<Factor,Conditional>(factorGraph,ord); BayesNet<Conditional> conditional = eliminate<Factor,Conditional>(factorGraph,ord);

3
cpp/inference.h
View File

@ -9,6 +9,7 @@
#include "FactorGraph.h" #include "FactorGraph.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
@ -23,7 +24,7 @@ namespace gtsam {
*/ */
template<class Factor, class Conditional> template<class Factor, class Conditional>
boost::shared_ptr<Conditional> boost::shared_ptr<Conditional>
eliminateOne(FactorGraph<Factor>& factorGraph, const std::string& key); eliminateOne(FactorGraph<Factor>& factorGraph, const Symbol& key);
/** /**
* eliminate factor graph using the given (not necessarily complete) * eliminate factor graph using the given (not necessarily complete)

4
cpp/planarSLAM.h
View File

@ -79,8 +79,8 @@ namespace gtsam {
namespace planarSLAM { namespace planarSLAM {
// Keys and Config // Keys and Config
typedef Symbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Symbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config; typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
// Factors // Factors

2
cpp/pose2SLAM.h
View File

@ -21,7 +21,7 @@ namespace gtsam {
namespace pose2SLAM { namespace pose2SLAM {
// Keys and Config // Keys and Config
typedef Symbol<Pose2, 'x'> Key; typedef TypedSymbol<Pose2, 'x'> Key;
typedef LieConfig<Key, Pose2> Config; typedef LieConfig<Key, Pose2> Config;
/** /**

2
cpp/pose3SLAM.h
View File

@ -21,7 +21,7 @@ namespace gtsam {
namespace pose3SLAM { namespace pose3SLAM {
// Keys and Config // Keys and Config
typedef Symbol<Pose3, 'x'> Key; typedef TypedSymbol<Pose3, 'x'> Key;
typedef LieConfig<Key, Pose3> Config; typedef LieConfig<Key, Pose3> Config;
/** /**

19
cpp/simulated2D.h
View File

@ -10,14 +10,15 @@
#include "VectorConfig.h" #include "VectorConfig.h"
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "Key.h"
// \namespace // \namespace
namespace simulated2D { namespace simulated2D {
typedef gtsam::VectorConfig VectorConfig; typedef gtsam::VectorConfig VectorConfig;
typedef std::string PoseKey; typedef gtsam::Symbol PoseKey;
typedef std::string PointKey; typedef gtsam::Symbol PointKey;
/** /**
* Prior on a single pose, and optional derivative version * Prior on a single pose, and optional derivative version
@ -42,13 +43,13 @@ namespace simulated2D {
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
struct Prior: public gtsam::NonlinearFactor1<VectorConfig, std::string, struct Prior: public gtsam::NonlinearFactor1<VectorConfig, PoseKey,
Vector> { Vector> {
Vector z_; Vector z_;
Prior(const Vector& z, double sigma, const std::string& key) : Prior(const Vector& z, double sigma, const PoseKey& key) :
gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key), gtsam::NonlinearFactor1<VectorConfig, PoseKey, Vector>(sigma, key),
z_(z) { z_(z) {
} }
@ -66,8 +67,8 @@ namespace simulated2D {
Vector, PointKey, Vector> { Vector, PointKey, Vector> {
Vector z_; Vector z_;
Odometry(const Vector& z, double sigma, const std::string& j1, Odometry(const Vector& z, double sigma, const PoseKey& j1,
const std::string& j2) : const PoseKey& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) { Vector>(sigma, j1, j2) {
} }
@ -87,8 +88,8 @@ namespace simulated2D {
Vector z_; Vector z_;
Measurement(const Vector& z, double sigma, const std::string& j1, Measurement(const Vector& z, double sigma, const PoseKey& j1,
const std::string& j2) : const PointKey& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) { Vector>(sigma, j1, j2) {
} }

2
cpp/smallExample.cpp
View File

@ -12,6 +12,8 @@
using namespace std; using namespace std;
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "Matrix.h" #include "Matrix.h"
#include "NonlinearFactor.h" #include "NonlinearFactor.h"

4
cpp/testBayesNetPreconditioner.cpp
View File

@ -8,6 +8,8 @@
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"
#include "BayesNetPreconditioner.h" #include "BayesNetPreconditioner.h"
@ -83,7 +85,7 @@ TEST( BayesNetPreconditioner, conjugateGradients )
// Create zero config y0 and perturbed config y1 // Create zero config y0 and perturbed config y1
VectorConfig y0; VectorConfig y0;
Vector z2 = zero(2); Vector z2 = zero(2);
BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2); BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2);
VectorConfig y1 = y0; VectorConfig y1 = y0;
y1.getReference("x23") = Vector_(2, 1.0, -1.0); y1.getReference("x23") = Vector_(2, 1.0, -1.0);

2
cpp/testBayesTree.cpp
View File

@ -11,6 +11,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "SymbolicBayesNet.h" #include "SymbolicBayesNet.h"
#include "SymbolicFactorGraph.h" #include "SymbolicFactorGraph.h"
#include "Ordering.h" #include "Ordering.h"

6
cpp/testBinaryBayesNet.cpp
View File

@ -20,6 +20,8 @@ using namespace boost::assign;
#include <boost/archive/text_iarchive.hpp> #include <boost/archive/text_iarchive.hpp>
#endif //HAVE_BOOST_SERIALIZATION #endif //HAVE_BOOST_SERIALIZATION
#define GTSAM_MAGIC_KEY
#include "BinaryConditional.h" #include "BinaryConditional.h"
#include "BayesNet-inl.h" #include "BayesNet-inl.h"
#include "smallExample.h" #include "smallExample.h"
@ -32,7 +34,7 @@ using namespace gtsam;
typedef BayesNet<BinaryConditional> BinaryBayesNet; typedef BayesNet<BinaryConditional> BinaryBayesNet;
double probability( BinaryBayesNet & bbn, map<string,bool> & config) double probability( BinaryBayesNet & bbn, map<Symbol,bool> & config)
{ {
double result = 1.0; double result = 1.0;
BinaryBayesNet::const_iterator it = bbn.begin(); BinaryBayesNet::const_iterator it = bbn.begin();
@ -51,7 +53,7 @@ TEST( BinaryBayesNet, constructor )
// p(x|y=0) = 0.3 // p(x|y=0) = 0.3
// p(x|y=1) = 0.6 // p(x|y=1) = 0.6
map<string,bool> config; map<Symbol,bool> config;
config["y"] = false; config["y"] = false;
config["x"] = false; config["x"] = false;
// unary conditional for y // unary conditional for y

2
cpp/testGaussianBayesNet.cpp
View File

@ -19,6 +19,8 @@ using namespace boost::assign;
#include <boost/archive/text_iarchive.hpp> #include <boost/archive/text_iarchive.hpp>
#endif //HAVE_BOOST_SERIALIZATION #endif //HAVE_BOOST_SERIALIZATION
#define GTSAM_MAGIC_KEY
#include "GaussianBayesNet.h" #include "GaussianBayesNet.h"
#include "BayesNet.h" #include "BayesNet.h"
#include "smallExample.h" #include "smallExample.h"

2
cpp/testGaussianConditional.cpp
View File

@ -14,6 +14,8 @@
#include <boost/archive/text_iarchive.hpp> #include <boost/archive/text_iarchive.hpp>
#endif //HAVE_BOOST_SERIALIZATION #endif //HAVE_BOOST_SERIALIZATION
#define GTSAM_MAGIC_KEY
#include "Matrix.h" #include "Matrix.h"
#include "GaussianConditional.h" #include "GaussianConditional.h"

27
cpp/testGaussianFactor.cpp
View File

@ -15,6 +15,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Matrix.h" #include "Matrix.h"
#include "Ordering.h" #include "Ordering.h"
#include "GaussianConditional.h" #include "GaussianConditional.h"
@ -70,7 +72,7 @@ TEST( GaussianFactor, keys )
// get the factor "f2" from the small linear factor graph // get the factor "f2" from the small linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
GaussianFactor::shared_ptr lf = fg[1]; GaussianFactor::shared_ptr lf = fg[1];
list<string> expected; list<Symbol> expected;
expected.push_back("x1"); expected.push_back("x1");
expected.push_back("x2"); expected.push_back("x2");
CHECK(lf->keys() == expected); CHECK(lf->keys() == expected);
@ -151,7 +153,7 @@ TEST( GaussianFactor, combine )
b2(3) = -0.1; b2(3) = -0.1;
// use general constructor for making arbitrary factors // use general constructor for making arbitrary factors
vector<pair<string, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
@ -209,7 +211,7 @@ TEST( NonlinearFactorGraph, combine2){
exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2; exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2;
exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4; exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4;
vector<pair<string, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("x1", A22)); meas.push_back(make_pair("x1", A22));
GaussianFactor expected(meas, exb, sigmas); GaussianFactor expected(meas, exb, sigmas);
CHECK(assert_equal(expected,combined)); CHECK(assert_equal(expected,combined));
@ -250,7 +252,7 @@ TEST( GaussianFactor, linearFactorN){
GaussianFactor combinedFactor(f); GaussianFactor combinedFactor(f);
vector<pair<string, Matrix> > combinedMeasurement; vector<pair<Symbol, Matrix> > combinedMeasurement;
combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2, combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2,
1.0, 0.0, 1.0, 0.0,
0.0, 1.0, 0.0, 1.0,
@ -410,9 +412,9 @@ TEST( GaussianFactor, eliminate2 )
b2(2) = 0.2; b2(2) = 0.2;
b2(3) = -0.1; b2(3) = -0.1;
vector<pair<string, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1x1", Al1x1)); meas.push_back(make_pair("l11", Al1x1));
GaussianFactor combined(meas, b2, sigmas); GaussianFactor combined(meas, b2, sigmas);
// eliminate the combined factor // eliminate the combined factor
@ -435,7 +437,7 @@ TEST( GaussianFactor, eliminate2 )
x2Sigmas(0) = 0.0894427; x2Sigmas(0) = 0.0894427;
x2Sigmas(1) = 0.0894427; x2Sigmas(1) = 0.0894427;
GaussianConditional expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas); GaussianConditional expectedCG("x2",d,R11,"l11",S12,x2Sigmas);
// the expected linear factor // the expected linear factor
double sigma = 0.2236; double sigma = 0.2236;
@ -448,7 +450,7 @@ TEST( GaussianFactor, eliminate2 )
// the RHS // the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
GaussianFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma); GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, sigma);
// check if the result matches // check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4)); CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@ -641,10 +643,10 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
sigmas(0) = 0.29907; sigmas(0) = 0.29907;
sigmas(1) = 0.29907; sigmas(1) = 0.29907;
GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l1x1",S12,sigmas)); GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l11",S12,sigmas));
GaussianFactor actualLF(CG); GaussianFactor actualLF(CG);
// actualLF.print(); // actualLF.print();
GaussianFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0)); GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas(0));
CHECK(assert_equal(expectedLF,actualLF,1e-5)); CHECK(assert_equal(expectedLF,actualLF,1e-5));
} }
@ -655,16 +657,17 @@ TEST( GaussianFactor, alphaFactor )
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get alphafactor for first factor in fg at zero, in gradient direction // get alphafactor for first factor in fg at zero, in gradient direction
Symbol alphaKey(ALPHA, 1);
VectorConfig x = createZeroDelta(); VectorConfig x = createZeroDelta();
VectorConfig d = fg.gradient(x); VectorConfig d = fg.gradient(x);
GaussianFactor::shared_ptr factor = fg[0]; GaussianFactor::shared_ptr factor = fg[0];
GaussianFactor::shared_ptr actual = factor->alphaFactor(x,d); GaussianFactor::shared_ptr actual = factor->alphaFactor(alphaKey,x,d);
// calculate expected // calculate expected
Matrix A = Matrix_(2,1,30.0,5.0); Matrix A = Matrix_(2,1,30.0,5.0);
Vector b = Vector_(2,-0.1,-0.1); Vector b = Vector_(2,-0.1,-0.1);
Vector sigmas = Vector_(2,0.1,0.1); Vector sigmas = Vector_(2,0.1,0.1);
GaussianFactor expected("alpha",A,b,sigmas); GaussianFactor expected(alphaKey,A,b,sigmas);
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
} }

12
cpp/testGaussianFactorGraph.cpp
View File

@ -16,6 +16,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Matrix.h" #include "Matrix.h"
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"
@ -57,13 +59,13 @@ TEST( GaussianFactorGraph, find_separator )
{ {
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
set<string> separator = fg.find_separator("x2"); set<Symbol> separator = fg.find_separator("x2");
set<string> expected; set<Symbol> expected;
expected.insert("x1"); expected.insert("x1");
expected.insert("l1"); expected.insert("l1");
CHECK(separator.size()==expected.size()); CHECK(separator.size()==expected.size());
set<string>::iterator it1 = separator.begin(), it2 = expected.begin(); set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin();
for(; it1!=separator.end(); it1++, it2++) for(; it1!=separator.end(); it1++, it2++)
CHECK(*it1 == *it2); CHECK(*it1 == *it2);
} }
@ -120,7 +122,7 @@ TEST( GaussianFactorGraph, combine_factors_x1 )
b(4) = 0*sigma3; b(4) = 0*sigma3;
b(5) = 1*sigma3; b(5) = 1*sigma3;
vector<pair<string, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
@ -177,7 +179,7 @@ TEST( GaussianFactorGraph, combine_factors_x2 )
b(2) = -1 *sigma2; b(2) = -1 *sigma2;
b(3) = 1.5*sigma2; b(3) = 1.5*sigma2;
vector<pair<string, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));

2
cpp/testGaussianISAM.cpp
View File

@ -10,6 +10,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "GaussianBayesNet.h" #include "GaussianBayesNet.h"
#include "ISAM-inl.h" #include "ISAM-inl.h"

2
cpp/testGaussianISAM2.cpp
View File

@ -10,6 +10,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "GaussianBayesNet.h" #include "GaussianBayesNet.h"
#include "ISAM2-inl.h" #include "ISAM2-inl.h"

3
cpp/testGraph.cpp
View File

@ -13,8 +13,9 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "pose2SLAM.h" #include "pose2SLAM.h"
#include "LieConfig-inl.h" #include "TupleConfig-inl.h"
#include "graph-inl.h" #include "graph-inl.h"
#include "FactorGraph-inl.h"
using namespace std; using namespace std;
using namespace boost; using namespace boost;

2
cpp/testISAM.cpp
View File

@ -10,6 +10,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "SymbolicBayesNet.h" #include "SymbolicBayesNet.h"
#include "SymbolicFactorGraph.h" #include "SymbolicFactorGraph.h"
#include "Ordering.h" #include "Ordering.h"

2
cpp/testInference.cpp
View File

@ -6,6 +6,8 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"
#include "inference-inl.h" #include "inference-inl.h"

4
cpp/testIterative.cpp
View File

@ -9,10 +9,14 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "iterative.h" #include "iterative.h"
#include "smallExample.h" #include "smallExample.h"
#include "pose2SLAM.h" #include "pose2SLAM.h"
#include "FactorGraph-inl.h"
#include "NonlinearFactorGraph-inl.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

3
cpp/testLieConfig.cpp
View File

@ -7,6 +7,9 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <stdexcept> #include <stdexcept>
#define GTSAM_MAGIC_KEY
#include <Pose2.h> #include <Pose2.h>
#include "LieConfig-inl.h" #include "LieConfig-inl.h"

75
cpp/testNonlinearConstraint.cpp
View File

@ -8,6 +8,9 @@
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#define GTSAM_MAGIC_KEY
#include <VectorConfig.h> #include <VectorConfig.h>
#include <NonlinearConstraint.h> #include <NonlinearConstraint.h>
#include <NonlinearConstraint-inl.h> #include <NonlinearConstraint-inl.h>
@ -22,13 +25,13 @@ using namespace boost::assign;
namespace test1 { namespace test1 {
/** p = 1, g(x) = x^2-5 = 0 */ /** p = 1, g(x) = x^2-5 = 0 */
Vector g(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Vector_(1, x * x - 5); return Vector_(1, x * x - 5);
} }
/** p = 1, jacobianG(x) = 2x */ /** p = 1, jacobianG(x) = 2x */
Matrix G(const VectorConfig& config, const list<string>& keys) { Matrix G(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x); return Matrix_(1, 1, 2 * x);
} }
@ -41,10 +44,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
// the lagrange multipliers will be expected on L_x1 // the lagrange multipliers will be expected on L_x1
// and there is only one multiplier // and there is only one multiplier
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x"; list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys), NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys),
"x", boost::bind(test1::G, _1, keys), "x", boost::bind(test1::G, _1, keys),
p, "L_x1"); p, "L1");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;
@ -59,10 +62,10 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) { TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x"; list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys), NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys),
"x", boost::bind(test1::G, _1, keys), "x", boost::bind(test1::G, _1, keys),
p, "L_x1"); p, "L1");
// get a configuration to use for linearization // get a configuration to use for linearization
VectorConfig realconfig; VectorConfig realconfig;
@ -70,14 +73,14 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
// get a configuration of Lagrange multipliers // get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig; VectorConfig lagrangeConfig;
lagrangeConfig.insert("L_x1", Vector_(1, 3.0)); lagrangeConfig.insert("L1", Vector_(1, 3.0));
// linearize the system // linearize the system
GaussianFactor::shared_ptr actualFactor, actualConstraint; GaussianFactor::shared_ptr actualFactor, actualConstraint;
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify // verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0); GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
CHECK(assert_equal(*actualFactor, expectedFactor)); CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actualConstraint, expectedConstraint)); CHECK(assert_equal(*actualConstraint, expectedConstraint));
@ -85,7 +88,7 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) { TEST( NonlinearConstraint1, unary_scalar_equal ) {
list<string> keys1, keys2; keys1 += "x"; keys2 += "y"; list<Symbol> keys1, keys2; keys1 += "x"; keys2 += "y";
NonlinearConstraint1<VectorConfig> NonlinearConstraint1<VectorConfig>
c1(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1", true), c1(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1", true),
c2(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1"), c2(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1"),
@ -104,20 +107,20 @@ TEST( NonlinearConstraint1, unary_scalar_equal ) {
namespace test2 { namespace test2 {
/** p = 1, g(x) = x^2-5 -y = 0 */ /** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double y = config[keys.back()](0); double y = config[keys.back()](0);
return Vector_(1, x * x - 5.0 - y); return Vector_(1, x * x - 5.0 - y);
} }
/** jacobian for x, jacobianG(x,y) in x: 2x*/ /** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(const VectorConfig& config, const list<string>& keys) { Matrix G1(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2.0 * x); return Matrix_(1, 1, 2.0 * x);
} }
/** jacobian for y, jacobianG(x,y) in y: -1 */ /** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(const VectorConfig& config, const list<string>& keys) { Matrix G2(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.back()](0); double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0); return Matrix_(1, 1, -1.0);
} }
@ -130,12 +133,12 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
// the lagrange multipliers will be expected on L_xy // the lagrange multipliers will be expected on L_xy
// and there is only one multiplier // and there is only one multiplier
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x", "y"; list<Symbol> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
boost::bind(test2::g, _1, keys), boost::bind(test2::g, _1, keys),
"x", boost::bind(test2::G1, _1, keys), "x", boost::bind(test2::G1, _1, keys),
"y", boost::bind(test2::G1, _1, keys), "y", boost::bind(test2::G1, _1, keys),
p, "L_xy"); p, "L12");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;
@ -152,12 +155,12 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
TEST( NonlinearConstraint2, binary_scalar_linearize ) { TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint // create a constraint
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x", "y"; list<Symbol> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
boost::bind(test2::g, _1, keys), boost::bind(test2::g, _1, keys),
"x", boost::bind(test2::G1, _1, keys), "x", boost::bind(test2::G1, _1, keys),
"y", boost::bind(test2::G2, _1, keys), "y", boost::bind(test2::G2, _1, keys),
p, "L_xy"); p, "L12");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig realconfig; VectorConfig realconfig;
@ -166,7 +169,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// get a configuration of Lagrange multipliers // get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig; VectorConfig lagrangeConfig;
lagrangeConfig.insert("L_xy", Vector_(1, 3.0)); lagrangeConfig.insert("L12", Vector_(1, 3.0));
// linearize the system // linearize the system
GaussianFactor::shared_ptr actualFactor, actualConstraint; GaussianFactor::shared_ptr actualFactor, actualConstraint;
@ -175,7 +178,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// verify // verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0),
"y", Matrix_(1,1, -3.0), "y", Matrix_(1,1, -3.0),
"L_xy", eye(1), zero(1), 1.0); "L12", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0),
"y", Matrix_(1,1, -1.0), "y", Matrix_(1,1, -1.0),
Vector_(1, 6.0), 0.0); Vector_(1, 6.0), 0.0);
@ -185,7 +188,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) { TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<string> keys1, keys2, keys3; list<Symbol> keys1, keys2, keys3;
keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z"; keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z";
NonlinearConstraint2<VectorConfig> NonlinearConstraint2<VectorConfig>
c1(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"), c1(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
@ -205,14 +208,14 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
namespace inequality1 { namespace inequality1 {
/** p = 1, g(x) x^2 - 5 > 0 */ /** p = 1, g(x) x^2 - 5 > 0 */
Vector g(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double g = x * x - 5; double g = x * x - 5;
return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
} }
/** p = 1, jacobianG(x) = 2*x */ /** p = 1, jacobianG(x) = 2*x */
Matrix G(const VectorConfig& config, const list<string>& keys) { Matrix G(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, 2 * x); return Matrix_(1, 1, 2 * x);
} }
@ -222,10 +225,10 @@ namespace inequality1 {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) { TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x"; list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys), NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
"x", boost::bind(inequality1::G, _1, keys), "x", boost::bind(inequality1::G, _1, keys),
p, "L_x1", p, "L1",
false); // inequality constraint false); // inequality constraint
// get configurations to use for evaluation // get configurations to use for evaluation
@ -243,10 +246,10 @@ TEST( NonlinearConstraint1, unary_inequality ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality_linearize ) { TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1; size_t p = 1;
list<string> keys; keys += "x"; list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys), NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
"x", boost::bind(inequality1::G, _1, keys), "x", boost::bind(inequality1::G, _1, keys),
p, "L_x1", p, "L1",
false); // inequality constraint false); // inequality constraint
// get configurations to use for linearization // get configurations to use for linearization
@ -256,7 +259,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
// get a configuration of Lagrange multipliers // get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig; VectorConfig lagrangeConfig;
lagrangeConfig.insert("L_x1", Vector_(1, 3.0)); lagrangeConfig.insert("L1", Vector_(1, 3.0));
// linearize for inactive constraint // linearize for inactive constraint
GaussianFactor::shared_ptr actualFactor1, actualConstraint1; GaussianFactor::shared_ptr actualFactor1, actualConstraint1;
@ -271,7 +274,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
CHECK(c1.active(config2)); CHECK(c1.active(config2));
// verify // verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L_x1", eye(1), zero(1), 1.0); GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
CHECK(assert_equal(*actualFactor2, expectedFactor)); CHECK(assert_equal(*actualFactor2, expectedFactor));
CHECK(assert_equal(*actualConstraint2, expectedConstraint)); CHECK(assert_equal(*actualConstraint2, expectedConstraint));
@ -283,7 +286,7 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
namespace binding1 { namespace binding1 {
/** p = 1, g(x) x^2 - r > 0 */ /** p = 1, g(x) x^2 - r > 0 */
Vector g(double r, const VectorConfig& config, const list<string>& keys) { Vector g(double r, const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double g = x * x - r; double g = x * x - r;
return Vector_(1, g); // return the actual cost return Vector_(1, g); // return the actual cost
@ -291,7 +294,7 @@ namespace binding1 {
/** p = 1, jacobianG(x) = 2*x */ /** p = 1, jacobianG(x) = 2*x */
Matrix G(double coeff, const VectorConfig& config, Matrix G(double coeff, const VectorConfig& config,
const list<string>& keys) { const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, coeff * x); return Matrix_(1, 1, coeff * x);
} }
@ -303,11 +306,11 @@ TEST( NonlinearConstraint1, unary_binding ) {
size_t p = 1; size_t p = 1;
double coeff = 2; double coeff = 2;
double radius = 5; double radius = 5;
list<string> keys; keys += "x"; list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1( NonlinearConstraint1<VectorConfig> c1(
boost::bind(binding1::g, radius, _1, keys), boost::bind(binding1::g, radius, _1, keys),
"x", boost::bind(binding1::G, coeff, _1, keys), "x", boost::bind(binding1::G, coeff, _1, keys),
p, "L_x1", p, "L1",
false); // inequality constraint false); // inequality constraint
// get configurations to use for evaluation // get configurations to use for evaluation
@ -326,20 +329,20 @@ TEST( NonlinearConstraint1, unary_binding ) {
namespace binding2 { namespace binding2 {
/** p = 1, g(x) = x^2-5 -y = 0 */ /** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(double r, const VectorConfig& config, const list<string>& keys) { Vector g(double r, const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
double y = config[keys.back()](0); double y = config[keys.back()](0);
return Vector_(1, x * x - r - y); return Vector_(1, x * x - r - y);
} }
/** jacobian for x, jacobianG(x,y) in x: 2x*/ /** jacobian for x, jacobianG(x,y) in x: 2x*/
Matrix G1(double c, const VectorConfig& config, const list<string>& keys) { Matrix G1(double c, const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0); double x = config[keys.front()](0);
return Matrix_(1, 1, c * x); return Matrix_(1, 1, c * x);
} }
/** jacobian for y, jacobianG(x,y) in y: -1 */ /** jacobian for y, jacobianG(x,y) in y: -1 */
Matrix G2(double c, const VectorConfig& config, const list<string>& keys) { Matrix G2(double c, const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.back()](0); double x = config[keys.back()](0);
return Matrix_(1, 1, -1.0 * c); return Matrix_(1, 1, -1.0 * c);
} }
@ -355,12 +358,12 @@ TEST( NonlinearConstraint2, binary_binding ) {
double a = 2.0; double a = 2.0;
double b = 1.0; double b = 1.0;
double r = 5.0; double r = 5.0;
list<string> keys; keys += "x", "y"; list<Symbol> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1( NonlinearConstraint2<VectorConfig> c1(
boost::bind(binding2::g, r, _1, keys), boost::bind(binding2::g, r, _1, keys),
"x", boost::bind(binding2::G1, a, _1, keys), "x", boost::bind(binding2::G1, a, _1, keys),
"y", boost::bind(binding2::G2, b, _1, keys), "y", boost::bind(binding2::G2, b, _1, keys),
p, "L_xy"); p, "L1");
// get a configuration to use for finding the error // get a configuration to use for finding the error
VectorConfig config; VectorConfig config;

9
cpp/testNonlinearEquality.cpp
View File

@ -4,6 +4,9 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "VectorConfig.h" #include "VectorConfig.h"
#include "NonlinearEquality.h" #include "NonlinearEquality.h"
@ -19,7 +22,7 @@ bool vector_compare(const Vector& a, const Vector& b) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) { TEST ( NonlinearEquality, linearization ) {
string key = "x"; Symbol key = "x";
Vector value = Vector_(2, 1.0, 2.0); Vector value = Vector_(2, 1.0, 2.0);
VectorConfig linearize; VectorConfig linearize;
linearize.insert(key, value); linearize.insert(key, value);
@ -35,7 +38,7 @@ TEST ( NonlinearEquality, linearization ) {
/* ********************************************************************** */ /* ********************************************************************** */
TEST ( NonlinearEquality, linearization_fail ) { TEST ( NonlinearEquality, linearization_fail ) {
string key = "x"; Symbol key = "x";
Vector value = Vector_(2, 1.0, 2.0); Vector value = Vector_(2, 1.0, 2.0);
Vector wrong = Vector_(2, 3.0, 4.0); Vector wrong = Vector_(2, 3.0, 4.0);
VectorConfig bad_linearize; VectorConfig bad_linearize;
@ -55,7 +58,7 @@ TEST ( NonlinearEquality, linearization_fail ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( NonlinearEquality, error ) { TEST ( NonlinearEquality, error ) {
string key = "x"; Symbol key = "x";
Vector value = Vector_(2, 1.0, 2.0); Vector value = Vector_(2, 1.0, 2.0);
Vector wrong = Vector_(2, 3.0, 4.0); Vector wrong = Vector_(2, 3.0, 4.0);
VectorConfig feasible, bad_linearize; VectorConfig feasible, bad_linearize;

2
cpp/testNonlinearFactor.cpp
View File

@ -11,6 +11,8 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Matrix.h" #include "Matrix.h"
#include "smallExample.h" #include "smallExample.h"
#include "simulated2D.h" #include "simulated2D.h"

2
cpp/testNonlinearFactorGraph.cpp
View File

@ -15,6 +15,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Matrix.h" #include "Matrix.h"
#include "smallExample.h" #include "smallExample.h"
#include "FactorGraph-inl.h" #include "FactorGraph-inl.h"

2
cpp/testNonlinearOptimizer.cpp
View File

@ -15,6 +15,8 @@ using namespace boost::assign;
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
using namespace boost; using namespace boost;
#define GTSAM_MAGIC_KEY
#include "Matrix.h" #include "Matrix.h"
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"

5
cpp/testOrdering.cpp
View File

@ -5,6 +5,9 @@
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering-inl.h" #include "Ordering-inl.h"
#include "pose2SLAM.h" #include "pose2SLAM.h"
@ -18,7 +21,7 @@ using namespace boost::assign;
// -> x3 -> x4 // -> x3 -> x4
// -> x5 // -> x5
TEST ( Ordering, predecessorMap2Keys ) { TEST ( Ordering, predecessorMap2Keys ) {
typedef Symbol<Pose2,'x'> Key; typedef TypedSymbol<Pose2,'x'> Key;
PredecessorMap<Key> p_map; PredecessorMap<Key> p_map;
p_map.insert(1,1); p_map.insert(1,1);
p_map.insert(2,1); p_map.insert(2,1);

3
cpp/testPose2Factor.cpp
View File

@ -5,6 +5,9 @@
**/ **/
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "numericalDerivative.h" #include "numericalDerivative.h"
#include "pose2SLAM.h" #include "pose2SLAM.h"

3
cpp/testPose2Prior.cpp
View File

@ -5,6 +5,9 @@
**/ **/
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "numericalDerivative.h" #include "numericalDerivative.h"
#include "pose2SLAM.h" #include "pose2SLAM.h"

2
cpp/testPose2SLAM.cpp
View File

@ -11,6 +11,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "FactorGraph-inl.h" #include "FactorGraph-inl.h"
#include "Ordering.h" #include "Ordering.h"

2
cpp/testPose3SLAM.cpp
View File

@ -13,6 +13,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "pose3SLAM.h" #include "pose3SLAM.h"
#include "Ordering.h" #include "Ordering.h"

71
cpp/testSQP.cpp
View File

@ -11,6 +11,9 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <GaussianFactorGraph.h> #include <GaussianFactorGraph.h>
#include <NonlinearOptimizer.h> #include <NonlinearOptimizer.h>
#include <SQPOptimizer.h> #include <SQPOptimizer.h>
@ -52,7 +55,7 @@ TEST (SQP, problem1_cholesky ) {
VectorConfig init, state; VectorConfig init, state;
init.insert("x", Vector_(1, 1.0)); init.insert("x", Vector_(1, 1.0));
init.insert("y", Vector_(1, 1.0)); init.insert("y", Vector_(1, 1.0));
init.insert("lambda", Vector_(1, 1.0)); init.insert("L", Vector_(1, 1.0));
state = init; state = init;
if (verbose) init.print("Initial State"); if (verbose) init.print("Initial State");
@ -66,7 +69,7 @@ TEST (SQP, problem1_cholesky ) {
double x, y, lambda; double x, y, lambda;
x = state["x"](0); x = state["x"](0);
y = state["y"](0); y = state["y"](0);
lambda = state["lambda"](0); lambda = state["L"](0);
// calculate the components // calculate the components
Matrix H1, H2, gradG; Matrix H1, H2, gradG;
@ -96,7 +99,7 @@ TEST (SQP, problem1_cholesky ) {
// create a factor for the states // create a factor for the states
GaussianFactor::shared_ptr f1(new GaussianFactor::shared_ptr f1(new
GaussianFactor("x", H1, "y", H2, "lambda", gradG, gradL, 1.0)); GaussianFactor("x", H1, "y", H2, "L", gradG, gradL, 1.0));
// create a factor for the lagrange multiplier // create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new GaussianFactor::shared_ptr f2(new
@ -111,7 +114,7 @@ TEST (SQP, problem1_cholesky ) {
// solve // solve
Ordering ord; Ordering ord;
ord += "x", "y", "lambda"; ord += "x", "y", "L";
VectorConfig delta = fg.optimize(ord).scale(-1.0); VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
@ -124,7 +127,7 @@ TEST (SQP, problem1_cholesky ) {
// verify that it converges to the nearest optimal point // verify that it converges to the nearest optimal point
VectorConfig expected; VectorConfig expected;
expected.insert("lambda", Vector_(1, -1.0)); expected.insert("L", Vector_(1, -1.0));
expected.insert("x", Vector_(1, 2.12)); expected.insert("x", Vector_(1, 2.12));
expected.insert("y", Vector_(1, -0.5)); expected.insert("y", Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2)); CHECK(assert_equal(expected,state, 1e-2));
@ -148,7 +151,7 @@ TEST (SQP, problem1_sqp ) {
VectorConfig init, state; VectorConfig init, state;
init.insert("x", Vector_(1, 1.0)); init.insert("x", Vector_(1, 1.0));
init.insert("y", Vector_(1, 1.0)); init.insert("y", Vector_(1, 1.0));
init.insert("lambda", Vector_(1, 1.0)); init.insert("L", Vector_(1, 1.0));
state = init; state = init;
if (verbose) init.print("Initial State"); if (verbose) init.print("Initial State");
@ -162,7 +165,7 @@ TEST (SQP, problem1_sqp ) {
double x, y, lambda; double x, y, lambda;
x = state["x"](0); x = state["x"](0);
y = state["y"](0); y = state["y"](0);
lambda = state["lambda"](0); lambda = state["L"](0);
/** create the linear factor /** create the linear factor
* ||h(x)-z||^2 => ||Ax-b||^2 * ||h(x)-z||^2 => ||Ax-b||^2
@ -190,7 +193,7 @@ TEST (SQP, problem1_sqp ) {
GaussianFactor::shared_ptr f2( GaussianFactor::shared_ptr f2(
new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1) new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
"y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2) "y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
"lambda", eye(1), // dlambda term "L", eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero Vector_(1, 0.0), // rhs is zero
1.0)); // arbitrary sigma 1.0)); // arbitrary sigma
@ -212,7 +215,7 @@ TEST (SQP, problem1_sqp ) {
// solve // solve
Ordering ord; Ordering ord;
ord += "x", "y", "lambda"; ord += "x", "y", "L";
VectorConfig delta = fg.optimize(ord); VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta"); if (verbose) delta.print("Delta");
@ -243,7 +246,7 @@ typedef NonlinearFactorGraph<VectorConfig> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c; typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
typedef NonlinearEquality<VectorConfig,string,Vector> NLE; typedef NonlinearEquality<VectorConfig,Symbol,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_eq; typedef boost::shared_ptr<NLE> shared_eq;
typedef boost::shared_ptr<VectorConfig> shared_cfg; typedef boost::shared_ptr<VectorConfig> shared_cfg;
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer; typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
@ -314,17 +317,17 @@ namespace sqp_test1 {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()] - config[keys.back()]; return config[keys.front()] - config[keys.back()];
} }
/** jacobian at l1 */ /** jacobian at l1 */
Matrix G1(const VectorConfig& config, const list<string>& keys) { Matrix G1(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2); return eye(2);
} }
/** jacobian at l2 */ /** jacobian at l2 */
Matrix G2(const VectorConfig& config, const list<string>& keys) { Matrix G2(const VectorConfig& config, const list<Symbol>& keys) {
return -1 * eye(2); return -1 * eye(2);
} }
@ -334,12 +337,12 @@ namespace sqp_test2 {
// Unary Constraint on x1 // Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */ /** g(x) = x -[1;1] = 0 */
Vector g(const VectorConfig& config, const list<string>& keys) { Vector g(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()] - Vector_(2, 1.0, 1.0); return config[keys.front()] - Vector_(2, 1.0, 1.0);
} }
/** jacobian at x1 */ /** jacobian at x1 */
Matrix G(const VectorConfig& config, const list<string>& keys) { Matrix G(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2); return eye(2);
} }
@ -359,12 +362,12 @@ TEST (SQP, two_pose ) {
feas.insert("x1", Vector_(2, 1.0, 1.0)); feas.insert("x1", Vector_(2, 1.0, 1.0));
// constant constraint on x1 // constant constraint on x1
list<string> keys1; keys1 += "x1"; list<Symbol> keys1; keys1 += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
boost::bind(sqp_test2::g, _1, keys1), boost::bind(sqp_test2::g, _1, keys1),
"x1", boost::bind(sqp_test2::G, _1, keys1), "x1", boost::bind(sqp_test2::G, _1, keys1),
2, "L_x1")); 2, "L1"));
// measurement from x1 to l1 // measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0); Vector z1 = Vector_(2, 0.0, 5.0);
@ -377,13 +380,13 @@ TEST (SQP, two_pose ) {
shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2")); shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2"));
// equality constraint between l1 and l2 // equality constraint between l1 and l2
list<string> keys2; keys2 += "l1", "l2"; list<Symbol> keys2; keys2 += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_test1::g, _1, keys2), boost::bind(sqp_test1::g, _1, keys2),
"l1", boost::bind(sqp_test1::G1, _1, keys2), "l1", boost::bind(sqp_test1::G1, _1, keys2),
"l2", boost::bind(sqp_test1::G2, _1, keys2), "l2", boost::bind(sqp_test1::G2, _1, keys2),
2, "L_l1l2")); 2, "L12"));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -400,8 +403,8 @@ TEST (SQP, two_pose ) {
// create an initial estimate for the lagrange multiplier // create an initial estimate for the lagrange multiplier
shared_cfg initLagrange(new VectorConfig); shared_cfg initLagrange(new VectorConfig);
initLagrange->insert("L_l1l2", Vector_(2, 1.0, 1.0)); initLagrange->insert("L12", Vector_(2, 1.0, 1.0));
initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0)); initLagrange->insert("L1", Vector_(2, 1.0, 1.0));
// create state config variables and initialize them // create state config variables and initialize them
VectorConfig state(*initialEstimate), state_lambda(*initLagrange); VectorConfig state(*initialEstimate), state_lambda(*initLagrange);
@ -432,7 +435,7 @@ TEST (SQP, two_pose ) {
// create an ordering // create an ordering
Ordering ordering; Ordering ordering;
ordering += "x1", "x2", "l1", "l2", "L_l1l2", "L_x1"; ordering += "x1", "x2", "l1", "l2", "L12", "L1";
// optimize linear graph to get full delta config // optimize linear graph to get full delta config
VectorConfig delta = fg.optimize(ordering); VectorConfig delta = fg.optimize(ordering);
@ -616,27 +619,27 @@ TEST (SQP, stereo_truth_noisy ) {
/* ********************************************************************* */ /* ********************************************************************* */
// Utility function to strip out a landmark number from a string key // Utility function to strip out a landmark number from a string key
int getNum(const string& key) { //int getNum(const string& key) {
return atoi(key.substr(1, key.size()-1).c_str()); // return atoi(key.substr(1, key.size()-1).c_str());
} //}
/* ********************************************************************* */ /* ********************************************************************* */
namespace sqp_stereo { namespace sqp_stereo {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g(const Config& config, const list<string>& keys) { Vector g(const Config& config, const list<Symbol>& keys) {
return config[PointKey(getNum(keys.front()))].vector() return config[PointKey(keys.front().index())].vector()
- config[PointKey(getNum(keys.back()))].vector(); - config[PointKey(keys.back().index())].vector();
} }
/** jacobian at l1 */ /** jacobian at l1 */
Matrix G1(const Config& config, const list<string>& keys) { Matrix G1(const Config& config, const list<Symbol>& keys) {
return eye(3); return eye(3);
} }
/** jacobian at l2 */ /** jacobian at l2 */
Matrix G2(const Config& config, const list<string>& keys) { Matrix G2(const Config& config, const list<Symbol>& keys) {
return -1.0 * eye(3); return -1.0 * eye(3);
} }
@ -674,13 +677,13 @@ Graph stereoExampleGraph() {
// create the binary equality constraint between the landmarks // create the binary equality constraint between the landmarks
// NOTE: this is really just a linear constraint that is exactly the same // NOTE: this is really just a linear constraint that is exactly the same
// as the previous examples // as the previous examples
list<string> keys; keys += "l1", "l2"; list<Symbol> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<Config> > c2( boost::shared_ptr<NonlinearConstraint2<Config> > c2(
new NonlinearConstraint2<Config>( new NonlinearConstraint2<Config>(
boost::bind(sqp_stereo::g, _1, keys), boost::bind(sqp_stereo::g, _1, keys),
"l1", boost::bind(sqp_stereo::G1, _1, keys), "l1", boost::bind(sqp_stereo::G1, _1, keys),
"l2", boost::bind(sqp_stereo::G2, _1, keys), "l2", boost::bind(sqp_stereo::G2, _1, keys),
3, "L_l1l2")); 3, "L12"));
graph.push_back(c2); graph.push_back(c2);
return graph; return graph;
@ -832,11 +835,11 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
// create ordering with lagrange multiplier included // create ordering with lagrange multiplier included
Ordering ord; Ordering ord;
ord += "x1", "x2", "l1", "l2", "L_l1l2"; ord += "x1", "x2", "l1", "l2", "L12";
// create lagrange multipliers // create lagrange multipliers
SOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig); SOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig);
initLagrangeConfig->insert("L_l1l2", Vector_(3, 0.0, 0.0, 0.0)); initLagrangeConfig->insert("L12", Vector_(3, 0.0, 0.0, 0.0));
// create optimizer // create optimizer
SOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig); SOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig);

67
cpp/testSQPOptimizer.cpp
View File

@ -8,6 +8,9 @@
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/map.hpp> // for insert #include <boost/assign/std/map.hpp> // for insert
#include <boost/bind.hpp> #include <boost/bind.hpp>
#define GTSAM_MAGIC_KEY
#include <simulated2D.h> #include <simulated2D.h>
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "NonlinearConstraint.h" #include "NonlinearConstraint.h"
@ -54,17 +57,17 @@ TEST ( SQPOptimizer, basic ) {
namespace sqp_LinearMapWarp2 { namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) { Vector g_func(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()]-config[keys.back()]; return config[keys.front()]-config[keys.back()];
} }
/** jacobian at l1 */ /** jacobian at l1 */
Matrix jac_g1(const VectorConfig& config, const list<string>& keys) { Matrix jac_g1(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2); return eye(2);
} }
/** jacobian at l2 */ /** jacobian at l2 */
Matrix jac_g2(const VectorConfig& config, const list<string>& keys) { Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
return -1*eye(2); return -1*eye(2);
} }
} // \namespace sqp_LinearMapWarp2 } // \namespace sqp_LinearMapWarp2
@ -72,12 +75,12 @@ Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
namespace sqp_LinearMapWarp1 { namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1 // Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */ /** g(x) = x -[1;1] = 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) { Vector g_func(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()]-Vector_(2, 1.0, 1.0); return config[keys.front()]-Vector_(2, 1.0, 1.0);
} }
/** jacobian at x1 */ /** jacobian at x1 */
Matrix jac_g(const VectorConfig& config, const list<string>& keys) { Matrix jac_g(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2); return eye(2);
} }
} // \namespace sqp_LinearMapWarp12 } // \namespace sqp_LinearMapWarp12
@ -90,12 +93,12 @@ typedef SQPOptimizer<NLGraph, VectorConfig> Optimizer;
*/ */
NLGraph linearMapWarpGraph() { NLGraph linearMapWarpGraph() {
// constant constraint on x1 // constant constraint on x1
list<string> keyx; keyx += "x1"; list<Symbol> keyx; keyx += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1( boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>( new NonlinearConstraint1<VectorConfig>(
boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx), boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx),
"x1", boost::bind(sqp_LinearMapWarp1::jac_g, _1, keyx), "x1", boost::bind(sqp_LinearMapWarp1::jac_g, _1, keyx),
2, "L_x1")); 2, "L1"));
// measurement from x1 to l1 // measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0); Vector z1 = Vector_(2, 0.0, 5.0);
@ -108,13 +111,13 @@ NLGraph linearMapWarpGraph() {
shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2")); shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2"));
// equality constraint between l1 and l2 // equality constraint between l1 and l2
list<string> keys; keys += "l1", "l2"; list<Symbol> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2( boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>( new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_LinearMapWarp2::g_func, _1, keys), boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
"l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys), "l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys),
"l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys), "l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys),
2, "L_l1l2")); 2, "L12"));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -141,12 +144,12 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
// create an initial estimate for the lagrange multiplier // create an initial estimate for the lagrange multiplier
shared_config initLagrange(new VectorConfig); shared_config initLagrange(new VectorConfig);
initLagrange->insert("L_l1l2", Vector_(2, 1.0, 1.0)); initLagrange->insert("L12", Vector_(2, 1.0, 1.0));
initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0)); initLagrange->insert("L1", Vector_(2, 1.0, 1.0));
// create an ordering // create an ordering
Ordering ordering; Ordering ordering;
ordering += "x1", "x2", "l1", "l2", "L_l1l2", "L_x1"; ordering += "x1", "x2", "l1", "l2", "L12", "L1";
// create an optimizer // create an optimizer
Optimizer optimizer(graph, ordering, initialEstimate, initLagrange); Optimizer optimizer(graph, ordering, initialEstimate, initLagrange);
@ -214,7 +217,7 @@ typedef NonlinearConstraint1<VectorConfig> NLC1;
typedef boost::shared_ptr<NLC1> shared_NLC1; typedef boost::shared_ptr<NLC1> shared_NLC1;
typedef NonlinearConstraint2<VectorConfig> NLC2; typedef NonlinearConstraint2<VectorConfig> NLC2;
typedef boost::shared_ptr<NLC2> shared_NLC2; typedef boost::shared_ptr<NLC2> shared_NLC2;
typedef NonlinearEquality<VectorConfig,string,Vector> NLE; typedef NonlinearEquality<VectorConfig,Symbol,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_NLE; typedef boost::shared_ptr<NLE> shared_NLE;
namespace sqp_avoid1 { namespace sqp_avoid1 {
@ -223,7 +226,7 @@ double radius = 1.0;
// binary avoidance constraint // binary avoidance constraint
/** g(x) = ||x2-obs||^2 - radius^2 > 0 */ /** g(x) = ||x2-obs||^2 - radius^2 > 0 */
Vector g_func(const VectorConfig& config, const list<string>& keys) { Vector g_func(const VectorConfig& config, const list<Symbol>& keys) {
Vector delta = config[keys.front()]-config[keys.back()]; Vector delta = config[keys.front()]-config[keys.back()];
double dist2 = sum(emul(delta, delta)); double dist2 = sum(emul(delta, delta));
double thresh = radius*radius; double thresh = radius*radius;
@ -231,14 +234,14 @@ Vector g_func(const VectorConfig& config, const list<string>& keys) {
} }
/** jacobian at pose */ /** jacobian at pose */
Matrix jac_g1(const VectorConfig& config, const list<string>& keys) { Matrix jac_g1(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs); Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
} }
/** jacobian at obstacle */ /** jacobian at obstacle */
Matrix jac_g2(const VectorConfig& config, const list<string>& keys) { Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs); Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
@ -252,10 +255,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
Vector_(2, 5.0, -0.5); Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1); feasible.insert("x1", feas1);
feasible.insert("x3", feas2); feasible.insert("x3", feas2);
feasible.insert("obs", feas3); feasible.insert("o", feas3);
shared_NLE e1(new NLE("x1", feas1, vector_compare)); shared_NLE e1(new NLE("x1", feas1, vector_compare));
shared_NLE e2(new NLE("x3", feas2, vector_compare)); shared_NLE e2(new NLE("x3", feas2, vector_compare));
shared_NLE e3(new NLE("obs", feas3, vector_compare)); shared_NLE e3(new NLE("o", feas3, vector_compare));
// measurement from x1 to x2 // measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0); Vector x1x2 = Vector_(2, 5.0, 0.0);
@ -269,11 +272,11 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// create a binary inequality constraint that forces the middle point away from // create a binary inequality constraint that forces the middle point away from
// the obstacle // the obstacle
list<string> keys; keys += "x2", "obs"; list<Symbol> keys; keys += "x2", "o";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys), shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys),
"x2", boost::bind(sqp_avoid1::jac_g1, _1, keys), "x2", boost::bind(sqp_avoid1::jac_g1, _1, keys),
"obs",boost::bind(sqp_avoid1::jac_g2, _1, keys), "o",boost::bind(sqp_avoid1::jac_g2, _1, keys),
1, "L_x2obs", false)); 1, "L20", false));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -299,7 +302,7 @@ TEST ( SQPOptimizer, inequality_avoid ) {
// create an ordering // create an ordering
Ordering ord; Ordering ord;
ord += "x1", "x2", "x3", "obs"; ord += "x1", "x2", "x3", "o";
// create an optimizer // create an optimizer
Optimizer optimizer(graph, ord, init); Optimizer optimizer(graph, ord, init);
@ -334,7 +337,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// create an ordering // create an ordering
Ordering ord; Ordering ord;
ord += "x1", "x2", "x3", "obs"; ord += "x1", "x2", "x3", "o";
// create an optimizer // create an optimizer
Optimizer optimizer(graph, ord, init); Optimizer optimizer(graph, ord, init);
@ -355,7 +358,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
namespace sqp_avoid2 { namespace sqp_avoid2 {
// binary avoidance constraint // binary avoidance constraint
/** g(x) = ||x2-obs||^2 - radius^2 > 0 */ /** g(x) = ||x2-obs||^2 - radius^2 > 0 */
Vector g_func(double radius, const VectorConfig& config, const list<string>& keys) { Vector g_func(double radius, const VectorConfig& config, const list<Symbol>& keys) {
Vector delta = config[keys.front()]-config[keys.back()]; Vector delta = config[keys.front()]-config[keys.back()];
double dist2 = sum(emul(delta, delta)); double dist2 = sum(emul(delta, delta));
double thresh = radius*radius; double thresh = radius*radius;
@ -363,14 +366,14 @@ Vector g_func(double radius, const VectorConfig& config, const list<string>& key
} }
/** jacobian at pose */ /** jacobian at pose */
Matrix jac_g1(const VectorConfig& config, const list<string>& keys) { Matrix jac_g1(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs); Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
} }
/** jacobian at obstacle */ /** jacobian at obstacle */
Matrix jac_g2(const VectorConfig& config, const list<string>& keys) { Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()]; Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs); Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1)); return Matrix_(1,2, grad(0), grad(1));
@ -384,10 +387,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
Vector_(2, 5.0, -0.5); Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1); feasible.insert("x1", feas1);
feasible.insert("x3", feas2); feasible.insert("x3", feas2);
feasible.insert("obs", feas3); feasible.insert("o", feas3);
shared_NLE e1(new NLE("x1", feas1,vector_compare)); shared_NLE e1(new NLE("x1", feas1,vector_compare));
shared_NLE e2(new NLE("x3", feas2, vector_compare)); shared_NLE e2(new NLE("x3", feas2, vector_compare));
shared_NLE e3(new NLE("obs", feas3, vector_compare)); shared_NLE e3(new NLE("o", feas3, vector_compare));
// measurement from x1 to x2 // measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0); Vector x1x2 = Vector_(2, 5.0, 0.0);
@ -403,11 +406,11 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
// create a binary inequality constraint that forces the middle point away from // create a binary inequality constraint that forces the middle point away from
// the obstacle // the obstacle
list<string> keys; keys += "x2", "obs"; list<Symbol> keys; keys += "x2", "o";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys), shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys),
"x2", boost::bind(sqp_avoid2::jac_g1, _1, keys), "x2", boost::bind(sqp_avoid2::jac_g1, _1, keys),
"obs", boost::bind(sqp_avoid2::jac_g2, _1, keys), "o", boost::bind(sqp_avoid2::jac_g2, _1, keys),
1, "L_x2obs", false)); 1, "L20", false));
// construct the graph // construct the graph
NLGraph graph; NLGraph graph;
@ -433,7 +436,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative_bind ) {
// create an ordering // create an ordering
Ordering ord; Ordering ord;
ord += "x1", "x2", "x3", "obs"; ord += "x1", "x2", "x3", "o";
// create an optimizer // create an optimizer
Optimizer optimizer(graph, ord, init); Optimizer optimizer(graph, ord, init);

4
cpp/testSubgraphPreconditioner.cpp
View File

@ -11,6 +11,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "numericalDerivative.h" #include "numericalDerivative.h"
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"
@ -108,7 +110,7 @@ TEST( SubgraphPreconditioner, system )
// Create zero config // Create zero config
VectorConfig zeros; VectorConfig zeros;
Vector z2 = zero(2); Vector z2 = zero(2);
BOOST_FOREACH(const string& j, ordering) zeros.insert(j,z2); BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,z2);
// Set up y0 as all zeros // Set up y0 as all zeros
VectorConfig y0 = zeros; VectorConfig y0 = zeros;

2
cpp/testSymbolicBayesNet.cpp
View File

@ -10,6 +10,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"
#include "SymbolicBayesNet.h" #include "SymbolicBayesNet.h"

2
cpp/testSymbolicFactorGraph.cpp
View File

@ -9,6 +9,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
#include "smallExample.h" #include "smallExample.h"
#include "SymbolicFactorGraph.h" #include "SymbolicFactorGraph.h"

7
cpp/testTupleConfig.cpp
View File

@ -7,6 +7,9 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <stdexcept> #include <stdexcept>
#define GTSAM_MAGIC_KEY
#include <Pose2.h> #include <Pose2.h>
#include <Point2.h> #include <Point2.h>
@ -18,8 +21,8 @@
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
typedef Symbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Symbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config; typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
/* ************************************************************************* */ /* ************************************************************************* */

3
cpp/testVSLAMConfig.cpp
View File

@ -5,6 +5,9 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "VectorConfig.h" #include "VectorConfig.h"
#include "visualSLAM.h" #include "visualSLAM.h"

2
cpp/testVSLAMFactor.cpp
View File

@ -4,6 +4,8 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "visualSLAM.h" #include "visualSLAM.h"
#include "Point3.h" #include "Point3.h"
#include "Pose3.h" #include "Pose3.h"

6
cpp/testVSLAMGraph.cpp
View File

@ -11,6 +11,8 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
using namespace boost; using namespace boost;
#define GTSAM_MAGIC_KEY
#include "NonlinearFactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "Ordering-inl.h" #include "Ordering-inl.h"
@ -87,7 +89,7 @@ TEST( Graph, optimizeLM)
initialEstimate->insert(4, landmark4); initialEstimate->insert(4, landmark4);
// Create an ordering of the variables // Create an ordering of the variables
list<string> keys; list<Symbol> keys;
keys.push_back("l1"); keys.push_back("l1");
keys.push_back("l2"); keys.push_back("l2");
keys.push_back("l3"); keys.push_back("l3");
@ -130,7 +132,7 @@ TEST( Graph, optimizeLM2)
initialEstimate->insert(4, landmark4); initialEstimate->insert(4, landmark4);
// Create an ordering of the variables // Create an ordering of the variables
list<string> keys; list<Symbol> keys;
keys.push_back("l1"); keys.push_back("l1");
keys.push_back("l2"); keys.push_back("l2");

10
cpp/testVectorConfig.cpp
View File

@ -15,6 +15,8 @@
#include <boost/archive/text_iarchive.hpp> #include <boost/archive/text_iarchive.hpp>
#endif //HAVE_BOOST_SERIALIZATION #endif //HAVE_BOOST_SERIALIZATION
#define GTSAM_MAGIC_KEY
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Matrix.h" #include "Matrix.h"
#include "VectorConfig.h" #include "VectorConfig.h"
@ -67,9 +69,9 @@ TEST( VectorConfig, contains)
{ {
VectorConfig fg; VectorConfig fg;
Vector v = Vector_(3, 5.0, 6.0, 7.0); Vector v = Vector_(3, 5.0, 6.0, 7.0);
fg.insert("ali", v); fg.insert("a", v);
CHECK(fg.contains("ali")); CHECK(fg.contains("a"));
CHECK(!fg.contains("gholi")); CHECK(!fg.contains("g"));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -125,7 +127,7 @@ TEST( VectorConfig, update_with_large_delta) {
init.insert("y", Vector_(2, 3.0, 4.0)); init.insert("y", Vector_(2, 3.0, 4.0));
delta.insert("x", Vector_(2, 0.1, 0.1)); delta.insert("x", Vector_(2, 0.1, 0.1));
delta.insert("y", Vector_(2, 0.1, 0.1)); delta.insert("y", Vector_(2, 0.1, 0.1));
delta.insert("penguin", Vector_(2, 0.1, 0.1)); delta.insert("p", Vector_(2, 0.1, 0.1));
VectorConfig actual = expmap(init,delta); VectorConfig actual = expmap(init,delta);
VectorConfig expected; VectorConfig expected;

2
cpp/timeGaussianFactor.cpp
View File

@ -4,6 +4,8 @@
* @author Alireza Fathi * @author Alireza Fathi
*/ */
#define GTSAM_MAGIC_KEY
#include <time.h> #include <time.h>
/*STL/C++*/ /*STL/C++*/

2
cpp/timeGaussianFactorGraph.cpp
View File

@ -4,6 +4,8 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#define GTSAM_MAGIC_KEY
#include <time.h> #include <time.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "smallExample.h" #include "smallExample.h"

4
cpp/visualSLAM.h
View File

@ -22,8 +22,8 @@ namespace gtsam { namespace visualSLAM {
/** /**
* Typedefs that make up the visualSLAM namespace. * Typedefs that make up the visualSLAM namespace.
*/ */
typedef Symbol<Pose3,'x'> PoseKey; typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef Symbol<Point3,'l'> PointKey; typedef TypedSymbol<Point3,'l'> PointKey;
typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config; typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config;
typedef NonlinearFactorGraph<Config> Graph; typedef NonlinearFactorGraph<Config> Graph;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint; typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;