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>
void BayesNet<Conditional>::print(const string& s) const {
cout << s << ":\n";
std::string key;
Symbol key;
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>
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));
if (it == conditionals_.end()) throw(invalid_argument(
"BayesNet::operator['"+key+"']: not found"));
"BayesNet::operator['"+(std::string)key+"']: not found"));
return *it;
}

3
cpp/BayesNet.h
View File

@ -14,6 +14,7 @@
#include <boost/serialization/shared_ptr.hpp>
#include "Testable.h"
#include "Key.h"
namespace gtsam {
@ -85,7 +86,7 @@ namespace gtsam {
Ordering ordering() const;
/** 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 */
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 {
cout << s;
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
cout << " " << conditional->key();
cout << " " << (string)(conditional->key());
if (!separator_.empty()) {
cout << " :";
BOOST_FOREACH(string key, separator_)
cout << " " << key;
BOOST_FOREACH(const Symbol& key, separator_)
cout << " " << (std::string)key;
}
cout << endl;
}
@ -105,22 +105,22 @@ namespace gtsam {
Ordering ordering = separator_;
// 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);
ordering.remove(key);
}
// 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]
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
BayesNet<Conditional> p_S_R = eliminate<Factor,Conditional>(p_Cp_R,ordering);
// 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 p_S_R;
@ -167,7 +167,7 @@ namespace gtsam {
// Find the keys of both C1 and C2
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();
// Calculate the marginal
@ -214,7 +214,7 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_.reset();
BOOST_FOREACH(string key, clique->ordering()) {
BOOST_FOREACH(const Symbol& key, clique->ordering()) {
nodes_.erase(key);
}
}
@ -248,8 +248,8 @@ namespace gtsam {
// binary predicate to test equality of a pair for use in equals
template<class Conditional>
bool check_pair(
const pair<string,typename BayesTree<Conditional>::sharedClique >& v1,
const pair<string,typename BayesTree<Conditional>::sharedClique >& v2
const pair<Symbol,typename BayesTree<Conditional>::sharedClique >& v1,
const pair<Symbol,typename BayesTree<Conditional>::sharedClique >& v2
) {
return v1.first == v2.first && v1.second->equals(*(v2.second));
}
@ -267,8 +267,8 @@ namespace gtsam {
void BayesTree<Conditional>::insert(const sharedConditional& conditional)
{
// get key and parents
string key = conditional->key();
list<string> parents = conditional->parents();
const Symbol& key = conditional->key();
list<Symbol> parents = conditional->parents(); // rtodo: const reference?
// if no parents, start a new root clique
if (parents.empty()) {
@ -277,7 +277,7 @@ namespace gtsam {
}
// otherwise, find the parent clique
string parent = parents.front();
Symbol parent = parents.front();
sharedClique parent_clique = (*this)[parent];
// 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 Factor>
FactorGraph<Factor>
BayesTree<Conditional>::marginal(const string& key) const {
BayesTree<Conditional>::marginal(const Symbol& key) const {
// get clique containing key
sharedClique clique = (*this)[key];
@ -318,7 +318,7 @@ namespace gtsam {
template<class Conditional>
template<class Factor>
BayesNet<Conditional>
BayesTree<Conditional>::marginalBayesNet(const string& key) const {
BayesTree<Conditional>::marginalBayesNet(const Symbol& key) const {
// calculate marginal as a factor graph
FactorGraph<Factor> fg = this->marginal<Factor>(key);
@ -333,7 +333,7 @@ namespace gtsam {
template<class Conditional>
template<class 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
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
@ -356,7 +356,7 @@ namespace gtsam {
template<class Conditional>
template<class Factor>
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
FactorGraph<Factor> fg = this->joint<Factor>(key1,key2);
@ -406,7 +406,7 @@ namespace gtsam {
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
// process each key of the new factor
BOOST_FOREACH(string key, newFactor->keys())
BOOST_FOREACH(const Symbol& key, newFactor->keys())
try {
// get the clique and remove it from orphans (if it exists)
sharedClique clique = (*this)[key];

17
cpp/BayesTree.h
View File

@ -18,6 +18,7 @@
#include "Testable.h"
#include "FactorGraph.h"
#include "BayesNet.h"
#include "Key.h"
namespace gtsam {
@ -43,7 +44,7 @@ namespace gtsam {
typedef typename boost::shared_ptr<Clique> shared_ptr;
shared_ptr parent_;
std::list<shared_ptr> children_;
std::list<std::string> separator_; /** separator keys */
std::list<Symbol> separator_; /** separator keys */
//* Constructor */
Clique(const sharedConditional& conditional);
@ -94,7 +95,7 @@ namespace gtsam {
private:
/** Map from keys to Clique */
typedef std::map<std::string, sharedClique> Nodes;
typedef std::map<Symbol, sharedClique> Nodes;
Nodes nodes_;
/** Root clique */
@ -141,29 +142,29 @@ namespace gtsam {
}
/** 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);
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;
return clique;
}
/** return marginal on any variable */
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 */
template<class Factor>
BayesNet<Conditional> marginalBayesNet(const std::string& key) const;
BayesNet<Conditional> marginalBayesNet(const Symbol& key) const;
/** return joint on two variables */
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 */
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

15
cpp/BinaryConditional.h
View File

@ -16,6 +16,7 @@
#include <boost/serialization/list.hpp>
#include <boost/serialization/vector.hpp>
#include "Conditional.h"
#include "Key.h"
namespace gtsam {
@ -26,7 +27,7 @@ namespace gtsam {
private:
std::list<std::string> parents_;
std::list<Symbol> parents_;
std::vector<double> cpt_;
public:
@ -42,7 +43,7 @@ namespace gtsam {
/**
* No parents
*/
BinaryConditional(const std::string& key, double p) :
BinaryConditional(const Symbol& key, double p) :
Conditional(key) {
cpt_.push_back(1-p);
cpt_.push_back(p);
@ -51,7 +52,7 @@ namespace gtsam {
/**
* 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) {
parents_.push_back(parent);
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)
}
double probability( std::map<std::string,bool> config) {
double probability( std::map<Symbol,bool> config) {
int index = 0, count = 1;
BOOST_FOREACH( std::string parent, parents_){
BOOST_FOREACH(const Symbol& parent, parents_){
index += count*(int)(config[parent]);
count = count << 1;
}
@ -72,7 +73,7 @@ namespace gtsam {
/** print */
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 << " |";
BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
std::cout << ")" << std::endl;
@ -90,7 +91,7 @@ namespace gtsam {
}
/** return parents */
std::list<std::string> parents() const { return parents_;}
std::list<Symbol> parents() const { return parents_;}
/** return Conditional probability table*/
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/nvp.hpp>
#include "Testable.h"
#include "Key.h"
namespace gtsam {
@ -27,19 +28,15 @@ class Conditional: boost::noncopyable, public Testable<Conditional> {
protected:
/** key of random variable */
std::string key_;
Symbol key_;
public:
/** empty constructor for serialization */
Conditional() :
key_("__unitialized__") {
}
Conditional() {}
/** constructor */
Conditional(const std::string& key) :
key_(key) {
}
Conditional(const Symbol& key) : key_(key) {}
/* destructor */
virtual ~Conditional() {
@ -51,12 +48,12 @@ public:
}
/** return key */
inline const std::string& key() const {
inline const Symbol& key() const {
return key_;
}
/** return parent keys */
virtual std::list<std::string> parents() const = 0;
virtual std::list<Symbol> parents() const = 0;
/** return the number of parents */
virtual std::size_t nrParents() const = 0;
@ -73,9 +70,9 @@ private:
// predicate to check whether a conditional has the sought key
template<class Conditional>
class onKey {
const std::string& key_;
const Symbol& key_;
public:
onKey(const std::string& key) :
onKey(const Symbol& key) :
key_(key) {
}
bool operator()(const typename Conditional::shared_ptr& conditional) {

5
cpp/Factor.h
View File

@ -15,11 +15,12 @@
#include <list>
#include <boost/utility.hpp> // for noncopyable
#include "Testable.h"
#include "Key.h"
namespace gtsam {
/** 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.
@ -50,7 +51,7 @@ namespace gtsam {
/**
* 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

27
cpp/FactorGraph-inl.h
View File

@ -118,15 +118,14 @@ void FactorGraph<Factor>::replace(int index, sharedFactor factor) {
if(factors_[index] != NULL) {
// Remove this factor from its variables' index lists
list<string> keys(factor->keys());
BOOST_FOREACH(string key, keys) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
Indices::iterator indices = indices_.find(key);
if(indices != indices_.end()) {
indices->second.remove(index);
} else {
throw invalid_argument(boost::str(boost::format(
"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
// Below, we compute a symbolic matrix stored in sparse columns.
typedef string Key; // default case with string keys
map<Key, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
map<Symbol, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
int nrNonZeros = 0; // number of non-zero entries
int n_row = factors_.size(); /* colamd arg 1: number of rows in A */
// loop over all factors = rows
for (int i = 0; i < n_row; i++) {
if (factors_[i]==NULL) continue;
list<Key> keys = factors_[i]->keys();
BOOST_FOREACH(Key key, keys) columns[key].push_back(i);
list<Symbol> keys = factors_[i]->keys();
BOOST_FOREACH(const Symbol& key, keys) columns[key].push_back(i);
nrNonZeros+= keys.size();
}
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) */
/* ************************************************************************* */
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);
return it->second;
}
@ -239,13 +237,13 @@ list<int> FactorGraph<Factor>::factors(const string& key) const {
/* ************************************************************************* */
template<class Factor>
vector<boost::shared_ptr<Factor> >
FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
vector<sharedFactor> found;
Indices::iterator it = indices_.find(key);
if (it == indices_.end())
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
indices_ptr = &(it->second);
@ -261,15 +259,17 @@ FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
/* ************************************************************************* */
template<class 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)
if (it==indices_.end()){ // there's no list yet
list<int> indices(1,index); // so make one
indices_.insert(make_pair(key,indices)); // insert new indices into factorMap
}
else {
// rtodo: what is going on with this pointer?
list<int> *indices_ptr = &(it->second); // get the list
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) */
/* ************************************************************************* */
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);
boost::shared_ptr<Factor> new_factor(new Factor(found));

14
cpp/FactorGraph.h
View File

@ -18,6 +18,7 @@
#include "Testable.h"
#include "BayesNet.h"
#include "graph.h"
#include "Key.h"
namespace gtsam {
@ -36,11 +37,12 @@ namespace gtsam {
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
protected:
/** Collection of factors */
/** Collection of factors */
std::vector<sharedFactor> factors_;
/** 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_;
public:
@ -90,7 +92,7 @@ namespace gtsam {
Ordering keys() const;
/** 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());
}
@ -108,14 +110,14 @@ namespace gtsam {
* Return indices for all factors that involve 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
* from the factor graph
* @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.
@ -149,7 +151,7 @@ namespace gtsam {
* @return the combined linear 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

28
cpp/GaussianBayesNet.cpp
View File

@ -25,7 +25,7 @@ template class BayesNet<GaussianConditional>;
namespace gtsam {
/* ************************************************************************* */
GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) {
GaussianBayesNet scalarGaussian(const Symbol& key, double mu, double sigma) {
GaussianBayesNet bn;
GaussianConditional::shared_ptr
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;
size_t n = mu.size();
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,
const string& name1, Matrix S, Vector sigmas) {
void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const Symbol& name1, Matrix S, Vector sigmas) {
GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas));
bn.push_front(cg);
}
/* ************************************************************************* */
void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R,
const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) {
void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
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));
bn.push_front(cg);
}
@ -78,11 +78,11 @@ VectorConfig backSubstitute(const GaussianBayesNet& bn, const VectorConfig& y) {
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) {
// 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:))
const string& i = cg->key();
const Symbol& i = cg->key();
Vector zi = emul(y[i],cg->get_sigmas());
GaussianConditional::const_iterator it;
for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) {
const string& j = it->first;
const Symbol& j = it->first;
const Matrix& Rij = it->second;
zi -= Rij * x[j];
}
@ -102,7 +102,7 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
// Initialize gy from gx
VectorConfig gy;
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());
gy.insert(j,gyj); // initialize result
}
@ -110,12 +110,12 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
// we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L
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
gyj = gtsam::backSubstituteUpper(gyj,cg->get_R());
GaussianConditional::const_iterator it;
for (it = cg->parentsBegin(); it!= cg->parentsEnd(); it++) {
const string& i = it->first;
const Symbol& i = it->first;
const Matrix& Rij = it->second;
Vector& gyi = gy.getReference(i); // should never fail
Matrix Lji = trans(Rij); // TODO avoid transpose of matrix ?
@ -125,7 +125,7 @@ VectorConfig backSubstituteTranspose(const GaussianBayesNet& bn,
// Scale gy
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
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
// 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) {
mapping.insert(make_pair(cg->key(),N));
N += cg->dim();
@ -147,7 +147,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// create matrix and copy in values
Matrix R = zeros(N,N);
Vector d(N);
string key; size_t I;
Symbol key; size_t I;
FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional
GaussianConditional::shared_ptr cg = bn[key];

13
cpp/GaussianBayesNet.h
View File

@ -13,6 +13,7 @@
#include "GaussianConditional.h"
#include "BayesNet.h"
#include "Key.h"
namespace gtsam {
@ -20,24 +21,24 @@ namespace gtsam {
typedef BayesNet<GaussianConditional> GaussianBayesNet;
/** 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 */
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
* |Rx+Sy-d|
*/
void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, Vector sigmas);
void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const Symbol& name1, Matrix S, Vector sigmas);
/**
* Add a conditional node with two parents
* |Rx+Sy+Tz-d|
*/
void push_front(GaussianBayesNet& bn, const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas);
void push_front(GaussianBayesNet& bn, const Symbol& key, Vector d, Matrix R,
const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas);
/**
* optimize, i.e. return x = inv(R)*d

28
cpp/GaussianConditional.cpp
View File

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

23
cpp/GaussianConditional.h
View File

@ -18,6 +18,7 @@
#include "Conditional.h"
#include "VectorConfig.h"
#include "Matrix.h"
#include "Key.h"
namespace gtsam {
@ -31,7 +32,7 @@ class Ordering;
class GaussianConditional : public Conditional {
public:
typedef std::map<std::string, Matrix> Parents;
typedef std::map<Symbol, Matrix> Parents;
typedef Parents::const_iterator const_iterator;
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
@ -55,31 +56,31 @@ public:
GaussianConditional(){}
/** constructor */
GaussianConditional(const std::string& key) :
GaussianConditional(const Symbol& key) :
Conditional (key) {}
/** constructor with no parents
* |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
* |Rx+Sy-d|
*/
GaussianConditional(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, Vector sigmas);
GaussianConditional(const Symbol& key, Vector d, Matrix R,
const Symbol& name1, Matrix S, Vector sigmas);
/** constructor with two parents
* |Rx+Sy+Tz-d|
*/
GaussianConditional(const std::string& key, Vector d, Matrix R,
const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas);
GaussianConditional(const Symbol& key, Vector d, Matrix R,
const Symbol& name1, Matrix S, const Symbol& name2, Matrix T, Vector sigmas);
/**
* constructor with number of arbitrary parents
* |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);
/** deconstructor */
@ -95,7 +96,7 @@ public:
size_t dim() const { return R_.size2();}
/** return all parents */
std::list<std::string> parents() const;
std::list<Symbol> parents() const;
/** return stuff contained in GaussianConditional */
const Vector& get_d() const {return d_;}
@ -118,7 +119,7 @@ public:
}
/** 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);
}
/**
@ -131,7 +132,7 @@ public:
/**
* adds a parent
*/
void add(const std::string key, Matrix S){
void add(const Symbol& key, Matrix 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;
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) :
b_(cg->get_d()) {
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++) {
const std::string& j = it->first;
const Symbol& j = it->first;
const Matrix& Aj = it->second;
As_.insert(make_pair(j, Aj));
}
@ -80,15 +81,15 @@ void GaussianFactor::print(const string& s) const {
cout << s << endl;
if (empty()) cout << " empty" << endl;
else {
string j; Matrix A;
FOREACH_PAIR(j,A,As_) gtsam::print(A, "A["+j+"]=\n");
Symbol j; Matrix A;
FOREACH_PAIR(j,A,As_) gtsam::print(A, "A["+(string)j+"]=\n");
gtsam::print(b_,"b=");
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);
if (it != As_.end())
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;
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;
if (j1 != j2) return false;
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 e = -b_;
if (empty()) return e;
string j; Matrix Aj;
Symbol j; Matrix Aj; // rtodo: copying matrix here?
FOREACH_PAIR(j, Aj, As_)
e += (Aj * c[j]);
return e;
@ -144,14 +145,14 @@ Vector GaussianFactor::error_vector(const VectorConfig& c) const {
/* ************************************************************************* */
double GaussianFactor::error(const VectorConfig& c) const {
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);
}
/* ************************************************************************* */
list<string> GaussianFactor::keys() const {
list<string> result;
string j; Matrix A;
list<Symbol> GaussianFactor::keys() const {
list<Symbol> result;
Symbol j; Matrix A; // rtodo: copying matrix here?
FOREACH_PAIR(j,A,As_)
result.push_back(j);
return result;
@ -160,16 +161,16 @@ list<string> GaussianFactor::keys() const {
/* ************************************************************************* */
Dimensions GaussianFactor::dimensions() const {
Dimensions result;
string j; Matrix A;
Symbol j; Matrix A; // rtodo: copying matrix here?
FOREACH_PAIR(j,A,As_)
result.insert(make_pair(j,A.size2()));
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)) {
string j; Matrix A;
Symbol j; Matrix A; // rtodo: copying matrix here?
FOREACH_PAIR(j,A,As_)
if(j != key) separator.insert(j);
}
@ -181,7 +182,7 @@ Vector GaussianFactor::operator*(const VectorConfig& x) const {
if (empty()) return Ax;
// 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_)
Ax += (Aj * x[j]);
@ -193,7 +194,7 @@ VectorConfig GaussianFactor::operator^(const Vector& e) const {
Vector E = ediv_(e,sigmas_);
VectorConfig x;
// 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_)
x.insert(j,Aj^E);
return x;
@ -202,9 +203,10 @@ VectorConfig GaussianFactor::operator^(const Vector& e) 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
vector<const Matrix *> matrices;
BOOST_FOREACH(string j, ordering) {
BOOST_FOREACH(const Symbol& j, ordering) {
const Matrix& Aj = get_A(j);
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 {
// get pointers to the matrices
vector<const Matrix *> matrices;
BOOST_FOREACH(string j, ordering) {
BOOST_FOREACH(const Symbol& j, ordering) {
const Matrix& Aj = get_A(j);
matrices.push_back(&Aj);
}
@ -250,7 +252,7 @@ GaussianFactor::sparse(const Dimensions& columnIndices) const {
list<double> S;
// iterate over all matrices in the factor
string key; Matrix Aj;
Symbol key; Matrix Aj; // rtodo: copying matrix?
FOREACH_PAIR( key, Aj, As_) {
// find first column index for this key
// 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) {
// 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_) {
// 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>
GaussianFactor::eliminate(const string& key) const
GaussianFactor::eliminate(const Symbol& key) const
{
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
const_iterator it = As_.find(key);
@ -323,7 +325,7 @@ GaussianFactor::eliminate(const string& key) const
// create an internal ordering that eliminates key first
Ordering ordering;
ordering += key;
BOOST_FOREACH(string k, keys())
BOOST_FOREACH(const Symbol& k, keys())
if (k != key) ordering += k;
// 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
GaussianFactor::shared_ptr factor(new GaussianFactor);
size_t j = n1;
BOOST_FOREACH(string cur_key, ordering)
BOOST_FOREACH(Symbol& cur_key, ordering)
if (cur_key!=key) {
size_t dim = getDim(cur_key);
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
// -> |(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 {
// Calculate A matrix
size_t m = b_.size();
Vector A = zero(m);
string j; Matrix Aj;
Symbol j; Matrix Aj; // rtodo: copying matrix?
FOREACH_PAIR(j, Aj, As_)
A += Aj * d[j];
@ -405,7 +407,7 @@ GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x,
Vector b = - unweighted_error(x);
// construct factor
shared_ptr factor(new GaussianFactor("alpha",Matrix_(A),b,sigmas_));
shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,sigmas_));
return factor;
}

53
cpp/GaussianFactor.h
View File

@ -12,6 +12,7 @@
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/serialization/map.hpp>
#include <list>
#include "Factor.h"
#include "Matrix.h"
@ -31,12 +32,12 @@ class GaussianFactor: boost::noncopyable, public Factor<VectorConfig> {
public:
typedef boost::shared_ptr<GaussianFactor> shared_ptr;
typedef std::map<std::string, Matrix>::iterator iterator;
typedef std::map<std::string, Matrix>::const_iterator const_iterator;
typedef std::map<Symbol, Matrix>::iterator iterator;
typedef std::map<Symbol, Matrix>::const_iterator const_iterator;
protected:
std::map<std::string, Matrix> As_; // linear matrices
std::map<Symbol, Matrix> As_; // linear matrices
Vector b_; // right-hand-side
Vector sigmas_; // vector of standard deviations for each row in the factor
@ -52,22 +53,22 @@ public:
}
/** Construct unary factor */
GaussianFactor(const std::string& key1, const Matrix& A1,
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) {
As_.insert(make_pair(key1, A1));
}
/** 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) :
b_(b), sigmas_(sigmas) {
As_.insert(make_pair(key1, A1));
}
/** Construct binary factor */
GaussianFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2,
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) {
As_.insert(make_pair(key1, A1));
@ -75,9 +76,9 @@ public:
}
/** Construct ternary factor */
GaussianFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3,
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) :
b_(b), sigmas_(repeat(b.size(),sigma)) {
As_.insert(make_pair(key1, A1));
@ -86,7 +87,7 @@ public:
}
/** 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) :
b_(b), sigmas_(repeat(b.size(),sigma)) {
for(unsigned int i=0; i<terms.size(); i++)
@ -94,7 +95,7 @@ public:
}
/** 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) :
b_(b), sigmas_(sigmas) {
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
* 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);
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;
}
/** 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);
}
/** 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);
return (it != As_.end());
}
@ -169,19 +170,19 @@ public:
* Find all variables
* @return The set of all variable keys
*/
std::list<std::string> keys() const;
std::list<Symbol> keys() const;
/**
* return the first key
* @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 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!");
return (++(As_.begin()))->first;
}
@ -198,15 +199,15 @@ public:
* @param key is the name 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
* @param key
* @param separator set to add to
*/
void tally_separator(const std::string& key,
std::set<std::string>& separator) const;
void tally_separator(const Symbol& key,
std::set<Symbol>& separator) const;
/**
* Return A*x
@ -247,14 +248,14 @@ public:
* @param x: starting point for search
* @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
/* ************************************************************************* */
/** 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));
}
@ -264,7 +265,7 @@ public:
}
// 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);
}
@ -275,7 +276,7 @@ public:
* @return a new factor and a conditional gaussian on the eliminated variable
*/
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.

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

18
cpp/GaussianFactorGraph.h
View File

@ -47,28 +47,28 @@ namespace gtsam {
}
/** 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) {
push_back(sharedFactor(new GaussianFactor(key1,A1,b,sigma)));
}
/** Add a binary factor */
inline void add(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2,
inline void add(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,sigma)));
}
/** Add a ternary factor */
inline void add(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3,
inline void add(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,sigma)));
}
/** 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) {
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
* 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
* Eliminates the factors from the factor graph through findAndRemoveFactors
* 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

4
cpp/ISAM-inl.h
View File

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

20
cpp/ISAM2-inl.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
using namespace std;
// 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
// if no factors are connected to key, returns an empty factor
@ -47,7 +47,7 @@ namespace gtsam {
// from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) {
BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
chordalBayesNet.push_back(cg);
}
@ -98,12 +98,12 @@ namespace gtsam {
BOOST_FOREACH(FactorGraph<GaussianFactor>::sharedFactor fac, affectedFactors) {
// retrieve correspondent factor from nonlinearFactors_
Ordering keys = fac->keys();
BOOST_FOREACH(string key, keys) {
BOOST_FOREACH(const Symbol& key, keys) {
list<int> indices = nonlinearFactors_.factors(key);
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?
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 (subset) {
@ -124,7 +124,7 @@ namespace gtsam {
// retrieve all factors that ONLY contain the affected variables
// (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;
for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) {
bool inside = true;
@ -143,10 +143,10 @@ namespace gtsam {
BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that is not part of the separator
string oneTooFar = orphan->separator_.front();
list<string> keys = orphan->keys();
list<string>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
list<Symbol> keys = orphan->keys();
list<Symbol>::iterator it = find(keys.begin(), keys.end(), oneTooFar);
it--;
string key = *it;
const Symbol& key = *it;
// retrieve the cached factor and add to boundary
cachedBoundary.push_back(cached[key]);
}
@ -176,7 +176,7 @@ namespace gtsam {
if (true) {
ordering = factors.getOrdering();
} else {
list<string> keys = factors.keys();
list<Symbol> keys = factors.keys();
keys.sort(); // todo: correct sorting order?
ordering = keys;
}
@ -220,7 +220,7 @@ namespace gtsam {
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
string key = orphan->separator_.front();
Symbol key = orphan->separator_.front();
sharedClique parent = (*this)[key];
parent->children_ += orphan;

3
cpp/ISAM2.h
View File

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

99
cpp/Key.h
View File

@ -10,17 +10,21 @@
#include <boost/format.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/integer_traits.hpp>
#define ALPHA '\224'
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
* 2) the character constant used for its string representation
* TODO: make Testable
*/
template <class T, char C>
class Symbol {
class TypedSymbol {
private:
size_t j_;
@ -29,8 +33,8 @@ namespace gtsam {
// Constructors:
Symbol():j_(999999) {}
Symbol(size_t j):j_(j) {}
TypedSymbol():j_(boost::integer_traits<size_t>::const_max) {}
TypedSymbol(size_t j):j_(j) {}
// Get stuff:
@ -41,9 +45,9 @@ namespace gtsam {
// logic:
bool operator< (const Symbol& compare) const { return j_<compare.j_;}
bool operator== (const Symbol& compare) const { return j_==compare.j_;}
int compare(const Symbol& compare) const {return j_-compare.j_;}
bool operator< (const TypedSymbol& compare) const { return j_<compare.j_;}
bool operator== (const TypedSymbol& compare) const { return j_==compare.j_;}
int compare(const TypedSymbol& compare) const {return j_-compare.j_;}
private:
@ -53,7 +57,88 @@ namespace gtsam {
void serialize(Archive & ar, const unsigned int version) {
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

6
cpp/LieConfig-inl.h
View File

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

15
cpp/NonlinearFactor.h
View File

@ -46,10 +46,10 @@ namespace gtsam {
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:
@ -96,7 +96,7 @@ namespace gtsam {
;
/** return keys */
std::list<std::string> keys() const {
std::list<Symbol> keys() const {
return keys_;
}
@ -194,8 +194,8 @@ namespace gtsam {
const X& xj = x[key_];
Matrix A;
Vector b = -evaluateError(xj, A);
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
this->sigma()));
return GaussianFactor::shared_ptr(new GaussianFactor(
key_, A, b, this->sigma()));
}
/*
@ -288,7 +288,8 @@ namespace gtsam {
const X2& x2 = c[key2_];
Matrix 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()));
}

6
cpp/Ordering.cpp
View File

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

9
cpp/Ordering.h
View File

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

7
cpp/Simulated3D.h
View File

@ -11,6 +11,7 @@
#include "Matrix.h"
#include "VectorConfig.h"
#include "NonlinearFactor.h"
#include "Key.h"
// \namespace
@ -18,10 +19,8 @@ namespace simulated3D {
typedef gtsam::VectorConfig VectorConfig;
struct PoseKey: public std::string {
};
struct PointKey: public std::string {
};
typedef gtsam::Symbol PoseKey;
typedef gtsam::Symbol PointKey;
/**
* 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
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
const string& j = cg->key();
const Symbol& j = cg->key();
e.push_back(y[j]); // append y
}
@ -59,7 +59,7 @@ namespace gtsam {
// Use BayesNet order to add y contributions in order
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
const string& j = cg->key();
const Symbol& j = cg->key();
e.push_back(y[j]); // append y
}
@ -80,7 +80,7 @@ namespace gtsam {
// Use BayesNet order to remove y contributions in order
Errors::const_iterator it = e.begin();
BOOST_FOREACH(GaussianConditional::shared_ptr cg, Rc1_) {
const string& j = cg->key();
const Symbol& j = cg->key();
const Vector& ej = *(it++);
y1.insert(j,ej);
}

5
cpp/SymbolicBayesNet.h
View File

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

26
cpp/SymbolicConditional.h
View File

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

14
cpp/SymbolicFactor.cpp
View File

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

13
cpp/SymbolicFactor.h
View File

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

4
cpp/SymbolicFactorGraph.cpp
View File

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

11
cpp/SymbolicFactorGraph.h
View File

@ -13,6 +13,7 @@
#include "FactorGraph.h"
#include "SymbolicFactor.h"
#include "SymbolicBayesNet.h"
#include "Key.h"
namespace gtsam {
@ -26,19 +27,19 @@ namespace gtsam {
SymbolicFactorGraph() {}
/** 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));
push_back(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));
push_back(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));
push_back(factor);
}
@ -50,7 +51,7 @@ namespace gtsam {
SymbolicFactorGraph(const FactorGraph<Factor>& fg) {
for (size_t i = 0; i < fg.size(); 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));
push_back(factor);
}
@ -61,7 +62,7 @@ namespace gtsam {
* Eliminates the factors from the factor graph through findAndRemoveFactors
* 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

44
cpp/VectorConfig.cpp
View File

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

23
cpp/VectorConfig.h
View File

@ -14,6 +14,7 @@
#include "Testable.h"
#include "Vector.h"
#include "Key.h"
namespace gtsam {
@ -22,42 +23,42 @@ namespace gtsam {
protected:
/** Map from string indices to values */
std::map<std::string, Vector> values;
std::map<Symbol, Vector> values;
public:
typedef std::map<std::string, Vector>::iterator iterator;
typedef std::map<std::string, Vector>::const_iterator const_iterator;
typedef std::map<Symbol, Vector>::iterator iterator;
typedef std::map<Symbol, Vector>::const_iterator const_iterator;
VectorConfig() {}
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() {}
/** 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 */
VectorConfig& insert(const std::string& name, const Vector& val);
VectorConfig& insert(const Symbol& name, const Vector& val);
/** 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 end() const {return values.end();}
/** 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 */
Vector& getReference(const std::string& name);
Vector& getReference(const Symbol& name);
/** operator[] syntax for get */
inline const Vector& operator[](const std::string& name) const {
inline const Vector& operator[](const Symbol& name) const {
return get(name);
}
bool contains(const std::string& name) const {
bool contains(const Symbol& name) const {
const_iterator it = values.find(name);
return (it!=values.end());
}

2
cpp/graph.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
template<class Key>
class PredecessorMap: public std::map<Key, Key> {
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) {
std::map<Key, Key>::insert(std::make_pair(key, parent));
}

9
cpp/inference-inl.h
View File

@ -4,9 +4,10 @@
* @author Frank Dellaert
*/
//#include "inference.h"
#include "inference.h"
#include "FactorGraph-inl.h"
#include "BayesNet-inl.h"
#include "Key.h"
using namespace std;
@ -16,7 +17,7 @@ namespace gtsam {
/* eliminate one node from the factor graph */
/* ************************************************************************* */
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
// if no factors are connected to key, returns an empty factor
@ -44,7 +45,7 @@ namespace gtsam {
{
BayesNet<Conditional> bayesNet; // empty
BOOST_FOREACH(string key, ordering) {
BOOST_FOREACH(Symbol key, ordering) {
boost::shared_ptr<Conditional> cg = eliminateOne<Factor,Conditional>(factorGraph,key);
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
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,
BayesNet<Conditional> conditional = eliminate<Factor,Conditional>(factorGraph,ord);

3
cpp/inference.h
View File

@ -9,6 +9,7 @@
#include "FactorGraph.h"
#include "BayesNet.h"
#include "Key.h"
namespace gtsam {
@ -23,7 +24,7 @@ namespace gtsam {
*/
template<class Factor, class 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)

4
cpp/planarSLAM.h
View File

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

2
cpp/pose2SLAM.h
View File

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

2
cpp/pose3SLAM.h
View File

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

19
cpp/simulated2D.h
View File

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

2
cpp/smallExample.cpp
View File

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

4
cpp/testBayesNetPreconditioner.cpp
View File

@ -8,6 +8,8 @@
#include <boost/tuple/tuple.hpp>
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Ordering.h"
#include "smallExample.h"
#include "BayesNetPreconditioner.h"
@ -83,7 +85,7 @@ TEST( BayesNetPreconditioner, conjugateGradients )
// Create zero config y0 and perturbed config y1
VectorConfig y0;
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;
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>
#define GTSAM_MAGIC_KEY
#include "SymbolicBayesNet.h"
#include "SymbolicFactorGraph.h"
#include "Ordering.h"

6
cpp/testBinaryBayesNet.cpp
View File

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

2
cpp/testGaussianBayesNet.cpp
View File

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

2
cpp/testGaussianConditional.cpp
View File

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

27
cpp/testGaussianFactor.cpp
View File

@ -15,6 +15,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Matrix.h"
#include "Ordering.h"
#include "GaussianConditional.h"
@ -70,7 +72,7 @@ TEST( GaussianFactor, keys )
// get the factor "f2" from the small linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph();
GaussianFactor::shared_ptr lf = fg[1];
list<string> expected;
list<Symbol> expected;
expected.push_back("x1");
expected.push_back("x2");
CHECK(lf->keys() == expected);
@ -151,7 +153,7 @@ TEST( GaussianFactor, combine )
b2(3) = -0.1;
// 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("l1", Al1));
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(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));
GaussianFactor expected(meas, exb, sigmas);
CHECK(assert_equal(expected,combined));
@ -250,7 +252,7 @@ TEST( GaussianFactor, linearFactorN){
GaussianFactor combinedFactor(f);
vector<pair<string, Matrix> > combinedMeasurement;
vector<pair<Symbol, Matrix> > combinedMeasurement;
combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2,
1.0, 0.0,
0.0, 1.0,
@ -410,9 +412,9 @@ TEST( GaussianFactor, eliminate2 )
b2(2) = 0.2;
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("l1x1", Al1x1));
meas.push_back(make_pair("l11", Al1x1));
GaussianFactor combined(meas, b2, sigmas);
// eliminate the combined factor
@ -435,7 +437,7 @@ TEST( GaussianFactor, eliminate2 )
x2Sigmas(0) = 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
double sigma = 0.2236;
@ -448,7 +450,7 @@ TEST( GaussianFactor, eliminate2 )
// the RHS
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(assert_equal(expectedCG,*actualCG,1e-4));
@ -641,10 +643,10 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
sigmas(0) = 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);
// 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));
}
@ -655,16 +657,17 @@ TEST( GaussianFactor, alphaFactor )
GaussianFactorGraph fg = createGaussianFactorGraph();
// get alphafactor for first factor in fg at zero, in gradient direction
Symbol alphaKey(ALPHA, 1);
VectorConfig x = createZeroDelta();
VectorConfig d = fg.gradient(x);
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
Matrix A = Matrix_(2,1,30.0,5.0);
Vector b = 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));
}

12
cpp/testGaussianFactorGraph.cpp
View File

@ -16,6 +16,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "Matrix.h"
#include "Ordering.h"
#include "smallExample.h"
@ -57,13 +59,13 @@ TEST( GaussianFactorGraph, find_separator )
{
GaussianFactorGraph fg = createGaussianFactorGraph();
set<string> separator = fg.find_separator("x2");
set<string> expected;
set<Symbol> separator = fg.find_separator("x2");
set<Symbol> expected;
expected.insert("x1");
expected.insert("l1");
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++)
CHECK(*it1 == *it2);
}
@ -120,7 +122,7 @@ TEST( GaussianFactorGraph, combine_factors_x1 )
b(4) = 0*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("x1", Ax1));
meas.push_back(make_pair("x2", Ax2));
@ -177,7 +179,7 @@ TEST( GaussianFactorGraph, combine_factors_x2 )
b(2) = -1 *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("x1", Ax1));
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>
#define GTSAM_MAGIC_KEY
#include "Ordering.h"
#include "GaussianBayesNet.h"
#include "ISAM-inl.h"

2
cpp/testGaussianISAM2.cpp
View File

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

3
cpp/testGraph.cpp
View File

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

2
cpp/testISAM.cpp
View File

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

2
cpp/testInference.cpp
View File

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

4
cpp/testIterative.cpp
View File

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

3
cpp/testLieConfig.cpp
View File

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

75
cpp/testNonlinearConstraint.cpp
View File

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

9
cpp/testNonlinearEquality.cpp
View File

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

2
cpp/testNonlinearFactor.cpp
View File

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

2
cpp/testNonlinearFactorGraph.cpp
View File

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

2
cpp/testNonlinearOptimizer.cpp
View File

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

5
cpp/testOrdering.cpp
View File

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

3
cpp/testPose2Factor.cpp
View File

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

3
cpp/testPose2Prior.cpp
View File

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

2
cpp/testPose2SLAM.cpp
View File

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

2
cpp/testPose3SLAM.cpp
View File

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

71
cpp/testSQP.cpp
View File

@ -11,6 +11,9 @@
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include <GaussianFactorGraph.h>
#include <NonlinearOptimizer.h>
#include <SQPOptimizer.h>
@ -52,7 +55,7 @@ TEST (SQP, problem1_cholesky ) {
VectorConfig init, state;
init.insert("x", 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;
if (verbose) init.print("Initial State");
@ -66,7 +69,7 @@ TEST (SQP, problem1_cholesky ) {
double x, y, lambda;
x = state["x"](0);
y = state["y"](0);
lambda = state["lambda"](0);
lambda = state["L"](0);
// calculate the components
Matrix H1, H2, gradG;
@ -96,7 +99,7 @@ TEST (SQP, problem1_cholesky ) {
// create a factor for the states
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
GaussianFactor::shared_ptr f2(new
@ -111,7 +114,7 @@ TEST (SQP, problem1_cholesky ) {
// solve
Ordering ord;
ord += "x", "y", "lambda";
ord += "x", "y", "L";
VectorConfig delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta");
@ -124,7 +127,7 @@ TEST (SQP, problem1_cholesky ) {
// verify that it converges to the nearest optimal point
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("y", Vector_(1, -0.5));
CHECK(assert_equal(expected,state, 1e-2));
@ -148,7 +151,7 @@ TEST (SQP, problem1_sqp ) {
VectorConfig init, state;
init.insert("x", 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;
if (verbose) init.print("Initial State");
@ -162,7 +165,7 @@ TEST (SQP, problem1_sqp ) {
double x, y, lambda;
x = state["x"](0);
y = state["y"](0);
lambda = state["lambda"](0);
lambda = state["L"](0);
/** create the linear factor
* ||h(x)-z||^2 => ||Ax-b||^2
@ -190,7 +193,7 @@ TEST (SQP, problem1_sqp ) {
GaussianFactor::shared_ptr f2(
new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
"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
1.0)); // arbitrary sigma
@ -212,7 +215,7 @@ TEST (SQP, problem1_sqp ) {
// solve
Ordering ord;
ord += "x", "y", "lambda";
ord += "x", "y", "L";
VectorConfig delta = fg.optimize(ord);
if (verbose) delta.print("Delta");
@ -243,7 +246,7 @@ typedef NonlinearFactorGraph<VectorConfig> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
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<VectorConfig> shared_cfg;
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
@ -314,17 +317,17 @@ namespace sqp_test1 {
// binary constraint between landmarks
/** 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()];
}
/** jacobian at l1 */
Matrix G1(const VectorConfig& config, const list<string>& keys) {
Matrix G1(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2);
}
/** 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);
}
@ -334,12 +337,12 @@ namespace sqp_test2 {
// Unary Constraint on x1
/** 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);
}
/** jacobian at x1 */
Matrix G(const VectorConfig& config, const list<string>& keys) {
Matrix G(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2);
}
@ -359,12 +362,12 @@ TEST (SQP, two_pose ) {
feas.insert("x1", Vector_(2, 1.0, 1.0));
// constant constraint on x1
list<string> keys1; keys1 += "x1";
list<Symbol> keys1; keys1 += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>(
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
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"));
// equality constraint between l1 and l2
list<string> keys2; keys2 += "l1", "l2";
list<Symbol> keys2; keys2 += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_test1::g, _1, keys2),
"l1", boost::bind(sqp_test1::G1, _1, keys2),
"l2", boost::bind(sqp_test1::G2, _1, keys2),
2, "L_l1l2"));
2, "L12"));
// construct the graph
NLGraph graph;
@ -400,8 +403,8 @@ TEST (SQP, two_pose ) {
// create an initial estimate for the lagrange multiplier
shared_cfg initLagrange(new VectorConfig);
initLagrange->insert("L_l1l2", Vector_(2, 1.0, 1.0));
initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0));
initLagrange->insert("L12", Vector_(2, 1.0, 1.0));
initLagrange->insert("L1", Vector_(2, 1.0, 1.0));
// create state config variables and initialize them
VectorConfig state(*initialEstimate), state_lambda(*initLagrange);
@ -432,7 +435,7 @@ TEST (SQP, two_pose ) {
// create an 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
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
int getNum(const string& key) {
return atoi(key.substr(1, key.size()-1).c_str());
}
//int getNum(const string& key) {
// return atoi(key.substr(1, key.size()-1).c_str());
//}
/* ********************************************************************* */
namespace sqp_stereo {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g(const Config& config, const list<string>& keys) {
return config[PointKey(getNum(keys.front()))].vector()
- config[PointKey(getNum(keys.back()))].vector();
Vector g(const Config& config, const list<Symbol>& keys) {
return config[PointKey(keys.front().index())].vector()
- config[PointKey(keys.back().index())].vector();
}
/** jacobian at l1 */
Matrix G1(const Config& config, const list<string>& keys) {
Matrix G1(const Config& config, const list<Symbol>& keys) {
return eye(3);
}
/** 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);
}
@ -674,13 +677,13 @@ Graph stereoExampleGraph() {
// create the binary equality constraint between the landmarks
// NOTE: this is really just a linear constraint that is exactly the same
// as the previous examples
list<string> keys; keys += "l1", "l2";
list<Symbol> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<Config> > c2(
new NonlinearConstraint2<Config>(
boost::bind(sqp_stereo::g, _1, keys),
"l1", boost::bind(sqp_stereo::G1, _1, keys),
"l2", boost::bind(sqp_stereo::G2, _1, keys),
3, "L_l1l2"));
3, "L12"));
graph.push_back(c2);
return graph;
@ -832,11 +835,11 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
// create ordering with lagrange multiplier included
Ordering ord;
ord += "x1", "x2", "l1", "l2", "L_l1l2";
ord += "x1", "x2", "l1", "l2", "L12";
// create lagrange multipliers
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
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/map.hpp> // for insert
#include <boost/bind.hpp>
#define GTSAM_MAGIC_KEY
#include <simulated2D.h>
#include "NonlinearFactorGraph.h"
#include "NonlinearConstraint.h"
@ -54,17 +57,17 @@ TEST ( SQPOptimizer, basic ) {
namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks
/** 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()];
}
/** 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);
}
/** 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);
}
} // \namespace sqp_LinearMapWarp2
@ -72,12 +75,12 @@ Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1
/** 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);
}
/** 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);
}
} // \namespace sqp_LinearMapWarp12
@ -90,12 +93,12 @@ typedef SQPOptimizer<NLGraph, VectorConfig> Optimizer;
*/
NLGraph linearMapWarpGraph() {
// constant constraint on x1
list<string> keyx; keyx += "x1";
list<Symbol> keyx; keyx += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>(
boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx),
"x1", boost::bind(sqp_LinearMapWarp1::jac_g, _1, keyx),
2, "L_x1"));
2, "L1"));
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
@ -108,13 +111,13 @@ NLGraph linearMapWarpGraph() {
shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "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(
new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
"l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys),
"l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys),
2, "L_l1l2"));
2, "L12"));
// construct the graph
NLGraph graph;
@ -141,12 +144,12 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
// create an initial estimate for the lagrange multiplier
shared_config initLagrange(new VectorConfig);
initLagrange->insert("L_l1l2", Vector_(2, 1.0, 1.0));
initLagrange->insert("L_x1", Vector_(2, 1.0, 1.0));
initLagrange->insert("L12", Vector_(2, 1.0, 1.0));
initLagrange->insert("L1", Vector_(2, 1.0, 1.0));
// create an ordering
Ordering ordering;
ordering += "x1", "x2", "l1", "l2", "L_l1l2", "L_x1";
ordering += "x1", "x2", "l1", "l2", "L12", "L1";
// create an optimizer
Optimizer optimizer(graph, ordering, initialEstimate, initLagrange);
@ -214,7 +217,7 @@ typedef NonlinearConstraint1<VectorConfig> NLC1;
typedef boost::shared_ptr<NLC1> shared_NLC1;
typedef NonlinearConstraint2<VectorConfig> 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;
namespace sqp_avoid1 {
@ -223,7 +226,7 @@ double radius = 1.0;
// binary avoidance constraint
/** 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()];
double dist2 = sum(emul(delta, delta));
double thresh = radius*radius;
@ -231,14 +234,14 @@ Vector g_func(const VectorConfig& config, const list<string>& keys) {
}
/** 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 grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
}
/** 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 grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
@ -252,10 +255,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1);
feasible.insert("x3", feas2);
feasible.insert("obs", feas3);
feasible.insert("o", feas3);
shared_NLE e1(new NLE("x1", feas1, 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
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
// 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),
"x2", boost::bind(sqp_avoid1::jac_g1, _1, keys),
"obs",boost::bind(sqp_avoid1::jac_g2, _1, keys),
1, "L_x2obs", false));
"o",boost::bind(sqp_avoid1::jac_g2, _1, keys),
1, "L20", false));
// construct the graph
NLGraph graph;
@ -299,7 +302,7 @@ TEST ( SQPOptimizer, inequality_avoid ) {
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "obs";
ord += "x1", "x2", "x3", "o";
// create an optimizer
Optimizer optimizer(graph, ord, init);
@ -334,7 +337,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "obs";
ord += "x1", "x2", "x3", "o";
// create an optimizer
Optimizer optimizer(graph, ord, init);
@ -355,7 +358,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
namespace sqp_avoid2 {
// binary avoidance constraint
/** 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()];
double dist2 = sum(emul(delta, delta));
double thresh = radius*radius;
@ -363,14 +366,14 @@ Vector g_func(double radius, const VectorConfig& config, const list<string>& key
}
/** 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 grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
}
/** 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 grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
@ -384,10 +387,10 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1);
feasible.insert("x3", feas2);
feasible.insert("obs", feas3);
feasible.insert("o", feas3);
shared_NLE e1(new NLE("x1", feas1,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
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
// 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),
"x2", boost::bind(sqp_avoid2::jac_g1, _1, keys),
"obs", boost::bind(sqp_avoid2::jac_g2, _1, keys),
1, "L_x2obs", false));
"o", boost::bind(sqp_avoid2::jac_g2, _1, keys),
1, "L20", false));
// construct the graph
NLGraph graph;
@ -433,7 +436,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative_bind ) {
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "obs";
ord += "x1", "x2", "x3", "o";
// create an optimizer
Optimizer optimizer(graph, ord, init);

4
cpp/testSubgraphPreconditioner.cpp
View File

@ -11,6 +11,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "numericalDerivative.h"
#include "Ordering.h"
#include "smallExample.h"
@ -108,7 +110,7 @@ TEST( SubgraphPreconditioner, system )
// Create zero config
VectorConfig zeros;
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
VectorConfig y0 = zeros;

2
cpp/testSymbolicBayesNet.cpp
View File

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

2
cpp/testSymbolicFactorGraph.cpp
View File

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

7
cpp/testTupleConfig.cpp
View File

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

3
cpp/testVSLAMConfig.cpp
View File

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

2
cpp/testVSLAMFactor.cpp
View File

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

6
cpp/testVSLAMGraph.cpp
View File

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

10
cpp/testVectorConfig.cpp
View File

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

2
cpp/timeGaussianFactor.cpp
View File

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

2
cpp/timeGaussianFactorGraph.cpp
View File

@ -4,6 +4,8 @@
* @author Frank Dellaert
*/
#define GTSAM_MAGIC_KEY
#include <time.h>
#include <CppUnitLite/TestHarness.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.
*/
typedef Symbol<Pose3,'x'> PoseKey;
typedef Symbol<Point3,'l'> PointKey;
typedef TypedSymbol<Pose3,'x'> PoseKey;
typedef TypedSymbol<Point3,'l'> PointKey;
typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config;
typedef NonlinearFactorGraph<Config> Graph;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;