diff --git a/cpp/BayesNet-inl.h b/cpp/BayesNet-inl.h index 5a708531d..9d07baf1a 100644 --- a/cpp/BayesNet-inl.h +++ b/cpp/BayesNet-inl.h @@ -24,9 +24,9 @@ namespace gtsam { template void BayesNet::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 typename BayesNet::sharedConditional - BayesNet::operator[](const std::string& key) const { + BayesNet::operator[](const Symbol& key) const { const_iterator it = find_if(conditionals_.begin(),conditionals_.end(),onKey(key)); if (it == conditionals_.end()) throw(invalid_argument( - "BayesNet::operator['"+key+"']: not found")); + "BayesNet::operator['"+(std::string)key+"']: not found")); return *it; } diff --git a/cpp/BayesNet.h b/cpp/BayesNet.h index 75d9a1f5d..2a20fa0fa 100644 --- a/cpp/BayesNet.h +++ b/cpp/BayesNet.h @@ -14,6 +14,7 @@ #include #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(); } diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 516bf93c2..3ae4c9b64 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -38,11 +38,11 @@ namespace gtsam { void BayesTree::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 p_S_R = eliminate(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 bool check_pair( - const pair::sharedClique >& v1, - const pair::sharedClique >& v2 + const pair::sharedClique >& v1, + const pair::sharedClique >& v2 ) { return v1.first == v2.first && v1.second->equals(*(v2.second)); } @@ -267,8 +267,8 @@ namespace gtsam { void BayesTree::insert(const sharedConditional& conditional) { // get key and parents - string key = conditional->key(); - list parents = conditional->parents(); + const Symbol& key = conditional->key(); + list 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 template FactorGraph - BayesTree::marginal(const string& key) const { + BayesTree::marginal(const Symbol& key) const { // get clique containing key sharedClique clique = (*this)[key]; @@ -318,7 +318,7 @@ namespace gtsam { template template BayesNet - BayesTree::marginalBayesNet(const string& key) const { + BayesTree::marginalBayesNet(const Symbol& key) const { // calculate marginal as a factor graph FactorGraph fg = this->marginal(key); @@ -333,7 +333,7 @@ namespace gtsam { template template FactorGraph - BayesTree::joint(const string& key1, const string& key2) const { + BayesTree::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 template BayesNet - BayesTree::jointBayesNet(const string& key1, const string& key2) const { + BayesTree::jointBayesNet(const Symbol& key1, const Symbol& key2) const { // calculate marginal as a factor graph FactorGraph fg = this->joint(key1,key2); @@ -406,7 +406,7 @@ namespace gtsam { FactorGraph &factors, typename BayesTree::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]; diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 2e52689f7..999a649f8 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -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 shared_ptr; shared_ptr parent_; std::list children_; - std::list separator_; /** separator keys */ + std::list separator_; /** separator keys */ //* Constructor */ Clique(const sharedConditional& conditional); @@ -94,7 +95,7 @@ namespace gtsam { private: /** Map from keys to Clique */ - typedef std::map Nodes; + typedef std::map 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 - FactorGraph marginal(const std::string& key) const; + FactorGraph marginal(const Symbol& key) const; /** return marginal on any variable, as a Bayes Net */ template - BayesNet marginalBayesNet(const std::string& key) const; + BayesNet marginalBayesNet(const Symbol& key) const; /** return joint on two variables */ template - FactorGraph joint(const std::string& key1, const std::string& key2) const; + FactorGraph joint(const Symbol& key1, const Symbol& key2) const; /** return joint on two variables as a BayesNet */ template - BayesNet jointBayesNet(const std::string& key1, const std::string& key2) const; + BayesNet jointBayesNet(const Symbol& key1, const Symbol& key2) const; /** * Remove path from clique to root and return that path as factors diff --git a/cpp/BinaryConditional.h b/cpp/BinaryConditional.h index 064587e9a..37848a774 100644 --- a/cpp/BinaryConditional.h +++ b/cpp/BinaryConditional.h @@ -16,6 +16,7 @@ #include #include #include "Conditional.h" +#include "Key.h" namespace gtsam { @@ -26,7 +27,7 @@ namespace gtsam { private: - std::list parents_; + std::list parents_; std::vector 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& cpt) : + BinaryConditional(const Symbol& key, const Symbol& parent, const std::vector& 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 config) { + double probability( std::map 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 parents() const { return parents_;} + std::list parents() const { return parents_;} /** return Conditional probability table*/ std::vector cpt() const { return cpt_;} diff --git a/cpp/Conditional.h b/cpp/Conditional.h index 55812f4cb..b6641888e 100644 --- a/cpp/Conditional.h +++ b/cpp/Conditional.h @@ -13,6 +13,7 @@ #include #include #include "Testable.h" +#include "Key.h" namespace gtsam { @@ -27,19 +28,15 @@ class Conditional: boost::noncopyable, public Testable { 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 parents() const = 0; + virtual std::list 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 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) { diff --git a/cpp/Factor.h b/cpp/Factor.h index 8576bf259..cdce15a3b 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -15,11 +15,12 @@ #include #include // for noncopyable #include "Testable.h" +#include "Key.h" namespace gtsam { /** A map from key to dimension, useful in various contexts */ - typedef std::map Dimensions; + typedef std::map 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 keys() const = 0; + virtual std::list keys() const = 0; /** * @return the number of nodes the factor connects diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index f4fc93aa2..a556169a3 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -118,15 +118,14 @@ void FactorGraph::replace(int index, sharedFactor factor) { if(factors_[index] != NULL) { // Remove this factor from its variables' index lists - list 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 FactorGraph::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 > columns; // map from keys to a sparse column of non-zero row indices + map > 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 keys = factors_[i]->keys(); - BOOST_FOREACH(Key key, keys) columns[key].push_back(i); + list 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::getOrdering() const { /** O(1) */ /* ************************************************************************* */ template -list FactorGraph::factors(const string& key) const { +list FactorGraph::factors(const Symbol& key) const { Indices::const_iterator it = indices_.find(key); return it->second; } @@ -239,13 +237,13 @@ list FactorGraph::factors(const string& key) const { /* ************************************************************************* */ template vector > -FactorGraph::findAndRemoveFactors(const string& key) { +FactorGraph::findAndRemoveFactors(const Symbol& key) { vector 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 *indices_ptr; // pointer to indices list in indices_ map indices_ptr = &(it->second); @@ -261,15 +259,17 @@ FactorGraph::findAndRemoveFactors(const string& key) { /* ************************************************************************* */ template void FactorGraph::associateFactor(int index, sharedFactor factor) { - list keys = factor->keys(); // get keys for factor + list 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 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 *indices_ptr = &(it->second); // get the list indices_ptr->push_back(index); // add the index i to it } @@ -326,12 +326,11 @@ void FactorGraph::split(const PredecessorMap& tree, FactorGraph boost::shared_ptr -removeAndCombineFactors(FactorGraph& factorGraph, const string& key) +removeAndCombineFactors(FactorGraph& factorGraph, const Symbol& key) { vector > found = factorGraph.findAndRemoveFactors(key); boost::shared_ptr new_factor(new Factor(found)); diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 432814236..d1ad4625a 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -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::const_iterator const_iterator; protected: - /** Collection of factors */ + + /** Collection of factors */ std::vector factors_; /** For each variable a list of factor indices connected to it */ - typedef std::map > Indices; + typedef std::map > 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 factors(const std::string& key) const; + std::list 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 findAndRemoveFactors(const std::string& key); + std::vector findAndRemoveFactors(const Symbol& key); /** * find the minimum spanning tree. @@ -149,7 +151,7 @@ namespace gtsam { * @return the combined linear factor */ template boost::shared_ptr - removeAndCombineFactors(FactorGraph& factorGraph, const std::string& key); + removeAndCombineFactors(FactorGraph& factorGraph, const Symbol& key); /** * static function that combines two factor graphs diff --git a/cpp/GaussianBayesNet.cpp b/cpp/GaussianBayesNet.cpp index 364e8ced3..49cddfa5d 100644 --- a/cpp/GaussianBayesNet.cpp +++ b/cpp/GaussianBayesNet.cpp @@ -25,7 +25,7 @@ template class BayesNet; 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(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 mapping; + size_t N=0; map mapping; BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) { mapping.insert(make_pair(cg->key(),N)); N += cg->dim(); @@ -147,7 +147,7 @@ pair 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]; diff --git a/cpp/GaussianBayesNet.h b/cpp/GaussianBayesNet.h index 482dfb80e..23d4b1d3c 100644 --- a/cpp/GaussianBayesNet.h +++ b/cpp/GaussianBayesNet.h @@ -13,6 +13,7 @@ #include "GaussianConditional.h" #include "BayesNet.h" +#include "Key.h" namespace gtsam { @@ -20,24 +21,24 @@ namespace gtsam { typedef BayesNet 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 diff --git a/cpp/GaussianConditional.cpp b/cpp/GaussianConditional.cpp index efad7d0d0..c2fd1603b 100644 --- a/cpp/GaussianConditional.cpp +++ b/cpp/GaussianConditional.cpp @@ -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& parents, Vector sigmas) : +GaussianConditional::GaussianConditional(const Symbol& key, + const Vector& d, const Matrix& R, const map& 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 GaussianConditional::parents() const { - list result; +list GaussianConditional::parents() const { + list 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]; } diff --git a/cpp/GaussianConditional.h b/cpp/GaussianConditional.h index ed6036ab1..de1ebf555 100644 --- a/cpp/GaussianConditional.h +++ b/cpp/GaussianConditional.h @@ -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 Parents; + typedef std::map Parents; typedef Parents::const_iterator const_iterator; typedef boost::shared_ptr 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 parents() const; + std::list 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)); } diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index fe7e41b19..535b44a05 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -23,15 +23,16 @@ namespace ublas = boost::numeric::ublas; using namespace gtsam; -typedef pair& mypair; +// richard: commented out this typedef because appears to be unused? +//typedef pair& mypair; /* ************************************************************************* */ GaussianFactor::GaussianFactor(const boost::shared_ptr& cg) : b_(cg->get_d()) { As_.insert(make_pair(cg->key(), cg->get_R())); - std::map::const_iterator it = cg->parentsBegin(); + std::map::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& 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& 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 GaussianFactor::keys() const { - list result; - string j; Matrix A; +list GaussianFactor::keys() const { + list 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 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& separator) const { +void GaussianFactor::tally_separator(const Symbol& key, set& 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 GaussianFactor::matrix(const Ordering& ordering, bool weight) const { + // rtodo: this is called in eliminate, potential function to optimize? // get pointers to the matrices vector 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 GaussianFactor::matrix(const Ordering& ordering, bool weight Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const { // get pointers to the matrices vector 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 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 -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; } diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 5a4a74396..45712c9c7 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -12,6 +12,7 @@ #include #include #include +#include #include "Factor.h" #include "Matrix.h" @@ -31,12 +32,12 @@ class GaussianFactor: boost::noncopyable, public Factor { public: typedef boost::shared_ptr shared_ptr; - typedef std::map::iterator iterator; - typedef std::map::const_iterator const_iterator; + typedef std::map::iterator iterator; + typedef std::map::const_iterator const_iterator; protected: - std::map As_; // linear matrices + std::map 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 > &terms, + GaussianFactor(const std::vector > &terms, const Vector &b, double sigma) : b_(b), sigmas_(repeat(b.size(),sigma)) { for(unsigned int i=0; i > &terms, + GaussianFactor(const std::vector > &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 keys() const; + std::list 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& separator) const; + void tally_separator(const Symbol& key, + std::set& 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, 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. diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 215a12977..c033272a8 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -73,9 +73,9 @@ VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const { } /* ************************************************************************* */ -set GaussianFactorGraph::find_separator(const string& key) const +set GaussianFactorGraph::find_separator(const Symbol& key) const { - set separator; + set separator; BOOST_FOREACH(sharedFactor factor,factors_) factor->tally_separator(key,separator); @@ -84,7 +84,7 @@ set GaussianFactorGraph::find_separator(const string& key) const /* ************************************************************************* */ GaussianConditional::shared_ptr -GaussianFactorGraph::eliminateOne(const std::string& key) { +GaussianFactorGraph::eliminateOne(const Symbol& key) { return gtsam::eliminateOne(*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 GaussianFactorGraph::eliminate_(const Ordering& ordering) { boost::shared_ptr 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; diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 9f99ee201..c319588c7 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -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 > &terms, + inline void add(const std::vector > &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 find_separator(const std::string& key) const; + std::set 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 diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h index 8bd24612c..7bd75702e 100644 --- a/cpp/ISAM-inl.h +++ b/cpp/ISAM-inl.h @@ -41,7 +41,7 @@ namespace gtsam { if (true) { ordering = factors.getOrdering(); } else { - list keys = factors.keys(); + list 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; diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 896329beb..3306ddec0 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -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 _eliminateOne(FactorGraph& graph, cachedFactors& cached, const string& key) { + boost::shared_ptr _eliminateOne(FactorGraph& 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& 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::sharedFactor fac, affectedFactors) { // retrieve correspondent factor from nonlinearFactors_ Ordering keys = fac->keys(); - BOOST_FOREACH(string key, keys) { + BOOST_FOREACH(const Symbol& key, keys) { list 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 affectedKeys = affectedFactors.keys(); + list affectedKeys = affectedFactors.keys(); typename FactorGraph >::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 keys = orphan->keys(); - list::iterator it = find(keys.begin(), keys.end(), oneTooFar); + list keys = orphan->keys(); + list::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 keys = factors.keys(); + list 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; diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index 22da93f78..214f5963e 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -20,10 +20,11 @@ #include "NonlinearFactorGraph.h" #include "BayesNet.h" #include "BayesTree.h" +#include "Key.h" namespace gtsam { - typedef std::map cachedFactors; + typedef std::map cachedFactors; template class ISAM2: public BayesTree { diff --git a/cpp/Key.h b/cpp/Key.h index eb52eeeee..b8483aea2 100644 --- a/cpp/Key.h +++ b/cpp/Key.h @@ -10,17 +10,21 @@ #include #include +#include +#include + +#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 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::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_::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 + Symbol(const TypedSymbol& 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(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(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 + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(c_); + ar & BOOST_SERIALIZATION_NVP(j_); + } }; } // namespace gtsam diff --git a/cpp/LieConfig-inl.h b/cpp/LieConfig-inl.h index ff0407fae..267d20e67 100644 --- a/cpp/LieConfig-inl.h +++ b/cpp/LieConfig-inl.h @@ -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); diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 2625266e8..825e4030d 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -46,10 +46,10 @@ namespace gtsam { protected: - double sigma_; // noise standard deviation - std::list keys_; // keys + typedef NonlinearFactor This; - typedef NonlinearFactor This; + double sigma_; // noise standard deviation + std::list keys_; // keys public: @@ -96,7 +96,7 @@ namespace gtsam { ; /** return keys */ - std::list keys() const { + std::list 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())); } diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 2a0cae1b7..02ac56cd8 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -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; } diff --git a/cpp/Ordering.h b/cpp/Ordering.h index 9682c54fd..2eafba1dc 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -11,6 +11,7 @@ #include #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, public Testable { + class Ordering: public std::list, public Testable { 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& strings_in) : - std::list(strings_in) { + Ordering(const std::list& keys_in) : + std::list(keys_in) { } /** diff --git a/cpp/Simulated3D.h b/cpp/Simulated3D.h index fd9d75035..cc20c71dd 100644 --- a/cpp/Simulated3D.h +++ b/cpp/Simulated3D.h @@ -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 diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp index c42d80a61..062a35b2c 100644 --- a/cpp/SubgraphPreconditioner.cpp +++ b/cpp/SubgraphPreconditioner.cpp @@ -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); } diff --git a/cpp/SymbolicBayesNet.h b/cpp/SymbolicBayesNet.h index 89988530f..d36448b39 100644 --- a/cpp/SymbolicBayesNet.h +++ b/cpp/SymbolicBayesNet.h @@ -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::const_iterator it = bn.begin(); for (; it != bn.end(); it++) { boost::shared_ptr conditional = *it; - std::string key = conditional->key(); - std::list parents = conditional->parents(); + Symbol key = conditional->key(); + std::list parents = conditional->parents(); SymbolicConditional::shared_ptr c(new SymbolicConditional(key, parents)); result.push_back(c); } diff --git a/cpp/SymbolicConditional.h b/cpp/SymbolicConditional.h index 0c9a5d96b..e9781cb6a 100644 --- a/cpp/SymbolicConditional.h +++ b/cpp/SymbolicConditional.h @@ -15,6 +15,7 @@ #include // TODO: make cpp file #include #include "Conditional.h" +#include "Key.h" namespace gtsam { @@ -24,8 +25,7 @@ namespace gtsam { class SymbolicConditional: public Conditional { private: - - std::list parents_; + std::list 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& parents) : + SymbolicConditional(const Symbol& key, + const std::list& 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 parents() const { return parents_;} + std::list parents() const { return parents_;} /** find the number of parents */ size_t nrParents() const { diff --git a/cpp/SymbolicFactor.cpp b/cpp/SymbolicFactor.cpp index 4bda97df7..adc89bb29 100644 --- a/cpp/SymbolicFactor.cpp +++ b/cpp/SymbolicFactor.cpp @@ -31,13 +31,13 @@ namespace gtsam { SymbolicFactor::SymbolicFactor(const vector & factors) { // store keys in a map to make them unique (set is not portable) - map map; + map 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 - SymbolicFactor::eliminate(const string& key) const + SymbolicFactor::eliminate(const Symbol& key) const { // get keys from input factor - list separator; - BOOST_FOREACH(string j,keys_) + list separator; + BOOST_FOREACH(const Symbol& j,keys_) if (j!=key) separator.push_back(j); // start empty remaining factor to be returned diff --git a/cpp/SymbolicFactor.h b/cpp/SymbolicFactor.h index 921ccbe9c..ac3cfebbf 100644 --- a/cpp/SymbolicFactor.h +++ b/cpp/SymbolicFactor.h @@ -14,6 +14,7 @@ #include #include "Testable.h" #include "Ordering.h" +#include "Key.h" namespace gtsam { @@ -23,7 +24,7 @@ namespace gtsam { class SymbolicFactor: public Testable { private: - std::list keys_; + std::list 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 keys() const { + std::list keys() const { return keys_; } @@ -81,7 +82,7 @@ namespace gtsam { * @return a new factor and a symbolic conditional on the eliminated variable */ std::pair, shared_ptr> - eliminate(const std::string& key) const; + eliminate(const Symbol& key) const; /** * Check if empty factor diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index fc01479ef..0e1bf8826 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -20,7 +20,7 @@ namespace gtsam { /* ************************************************************************* */ boost::shared_ptr - SymbolicFactorGraph::eliminateOne(const std::string& key){ + SymbolicFactorGraph::eliminateOne(const Symbol& key){ return gtsam::eliminateOne(*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(*this,key); bayesNet.push_back(conditional); diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h index 4f760a107..76f3f0e3e 100644 --- a/cpp/SymbolicFactorGraph.h +++ b/cpp/SymbolicFactorGraph.h @@ -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 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 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 factor(new SymbolicFactor(key1,key2,key3)); push_back(factor); } @@ -50,7 +51,7 @@ namespace gtsam { SymbolicFactorGraph(const FactorGraph& fg) { for (size_t i = 0; i < fg.size(); i++) { boost::shared_ptr f = fg[i]; - std::list keys = f->keys(); + std::list 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 eliminateOne(const std::string& key); + boost::shared_ptr eliminateOne(const Symbol& key); /** * eliminate factor graph in place(!) in the given order, yielding diff --git a/cpp/VectorConfig.cpp b/cpp/VectorConfig.cpp index 04e5807bd..e05d98d6a 100644 --- a/cpp/VectorConfig.cpp +++ b/cpp/VectorConfig.cpp @@ -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 VectorConfig::get_names() const { - std::vector names; +std::vector VectorConfig::get_names() const { + std::vector 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; } diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h index f5ab02719..bc70a510b 100644 --- a/cpp/VectorConfig.h +++ b/cpp/VectorConfig.h @@ -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 values; + std::map values; public: - typedef std::map::iterator iterator; - typedef std::map::const_iterator const_iterator; + typedef std::map::iterator iterator; + typedef std::map::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 get_names() const; + std::vector 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()); } diff --git a/cpp/graph.h b/cpp/graph.h index 2d8a66923..45e30a6a3 100644 --- a/cpp/graph.h +++ b/cpp/graph.h @@ -43,7 +43,7 @@ namespace gtsam { template class PredecessorMap: public std::map { 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::insert(std::make_pair(key, parent)); } diff --git a/cpp/inference-inl.h b/cpp/inference-inl.h index ee41e87b4..ac086d6fb 100644 --- a/cpp/inference-inl.h +++ b/cpp/inference-inl.h @@ -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 - boost::shared_ptr eliminateOne(FactorGraph& graph, const string& key) { + boost::shared_ptr eliminateOne(FactorGraph& 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 bayesNet; // empty - BOOST_FOREACH(string key, ordering) { + BOOST_FOREACH(Symbol key, ordering) { boost::shared_ptr cg = eliminateOne(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 = eliminate(factorGraph,ord); diff --git a/cpp/inference.h b/cpp/inference.h index e3321ba47..cc17c9df1 100644 --- a/cpp/inference.h +++ b/cpp/inference.h @@ -9,6 +9,7 @@ #include "FactorGraph.h" #include "BayesNet.h" +#include "Key.h" namespace gtsam { @@ -23,7 +24,7 @@ namespace gtsam { */ template boost::shared_ptr - eliminateOne(FactorGraph& factorGraph, const std::string& key); + eliminateOne(FactorGraph& factorGraph, const Symbol& key); /** * eliminate factor graph using the given (not necessarily complete) diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h index 9678dee22..67e9d22bb 100644 --- a/cpp/planarSLAM.h +++ b/cpp/planarSLAM.h @@ -79,8 +79,8 @@ namespace gtsam { namespace planarSLAM { // Keys and Config - typedef Symbol PoseKey; - typedef Symbol PointKey; + typedef TypedSymbol PoseKey; + typedef TypedSymbol PointKey; typedef PairConfig Config; // Factors diff --git a/cpp/pose2SLAM.h b/cpp/pose2SLAM.h index 9498cde4d..055f70019 100644 --- a/cpp/pose2SLAM.h +++ b/cpp/pose2SLAM.h @@ -21,7 +21,7 @@ namespace gtsam { namespace pose2SLAM { // Keys and Config - typedef Symbol Key; + typedef TypedSymbol Key; typedef LieConfig Config; /** diff --git a/cpp/pose3SLAM.h b/cpp/pose3SLAM.h index b74a7cf65..9d9dfc5a4 100644 --- a/cpp/pose3SLAM.h +++ b/cpp/pose3SLAM.h @@ -21,7 +21,7 @@ namespace gtsam { namespace pose3SLAM { // Keys and Config - typedef Symbol Key; + typedef TypedSymbol Key; typedef LieConfig Config; /** diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index f2c1f778c..d30031fd5 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -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 { Vector z_; - Prior(const Vector& z, double sigma, const std::string& key) : - gtsam::NonlinearFactor1(sigma, key), + Prior(const Vector& z, double sigma, const PoseKey& key) : + gtsam::NonlinearFactor1(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(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(sigma, j1, j2) { } diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 6aed23bf6..42c17ef8c 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -12,6 +12,8 @@ using namespace std; +#define GTSAM_MAGIC_KEY + #include "Ordering.h" #include "Matrix.h" #include "NonlinearFactor.h" diff --git a/cpp/testBayesNetPreconditioner.cpp b/cpp/testBayesNetPreconditioner.cpp index 7d378abe6..f2918a89a 100644 --- a/cpp/testBayesNetPreconditioner.cpp +++ b/cpp/testBayesNetPreconditioner.cpp @@ -8,6 +8,8 @@ #include #include +#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); diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 515258831..628e8d6fb 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -11,6 +11,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "SymbolicBayesNet.h" #include "SymbolicFactorGraph.h" #include "Ordering.h" diff --git a/cpp/testBinaryBayesNet.cpp b/cpp/testBinaryBayesNet.cpp index fecfe772f..1f3d1f5f7 100644 --- a/cpp/testBinaryBayesNet.cpp +++ b/cpp/testBinaryBayesNet.cpp @@ -20,6 +20,8 @@ using namespace boost::assign; #include #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 BinaryBayesNet; -double probability( BinaryBayesNet & bbn, map & config) +double probability( BinaryBayesNet & bbn, map & 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 config; + map config; config["y"] = false; config["x"] = false; // unary conditional for y diff --git a/cpp/testGaussianBayesNet.cpp b/cpp/testGaussianBayesNet.cpp index 2cba16787..dca43d8c1 100644 --- a/cpp/testGaussianBayesNet.cpp +++ b/cpp/testGaussianBayesNet.cpp @@ -19,6 +19,8 @@ using namespace boost::assign; #include #endif //HAVE_BOOST_SERIALIZATION +#define GTSAM_MAGIC_KEY + #include "GaussianBayesNet.h" #include "BayesNet.h" #include "smallExample.h" diff --git a/cpp/testGaussianConditional.cpp b/cpp/testGaussianConditional.cpp index 9b8839e3a..53e2f61be 100644 --- a/cpp/testGaussianConditional.cpp +++ b/cpp/testGaussianConditional.cpp @@ -14,6 +14,8 @@ #include #endif //HAVE_BOOST_SERIALIZATION +#define GTSAM_MAGIC_KEY + #include "Matrix.h" #include "GaussianConditional.h" diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index 8f09e643e..773ddf976 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -15,6 +15,8 @@ using namespace boost::assign; #include +#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 expected; + list 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 > meas; + vector > 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 > meas; + vector > 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 > combinedMeasurement; + vector > 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 > meas; + vector > 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)); } diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index b579a4591..99726cddb 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -16,6 +16,8 @@ using namespace boost::assign; #include +#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 separator = fg.find_separator("x2"); - set expected; + set separator = fg.find_separator("x2"); + set expected; expected.insert("x1"); expected.insert("l1"); CHECK(separator.size()==expected.size()); - set::iterator it1 = separator.begin(), it2 = expected.begin(); + set::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 > meas; + vector > 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 > meas; + vector > meas; meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x2", Ax2)); diff --git a/cpp/testGaussianISAM.cpp b/cpp/testGaussianISAM.cpp index 689dc7206..1573b7efb 100644 --- a/cpp/testGaussianISAM.cpp +++ b/cpp/testGaussianISAM.cpp @@ -10,6 +10,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "Ordering.h" #include "GaussianBayesNet.h" #include "ISAM-inl.h" diff --git a/cpp/testGaussianISAM2.cpp b/cpp/testGaussianISAM2.cpp index 0ddf6c14c..e1cdf255d 100644 --- a/cpp/testGaussianISAM2.cpp +++ b/cpp/testGaussianISAM2.cpp @@ -10,6 +10,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "Ordering.h" #include "GaussianBayesNet.h" #include "ISAM2-inl.h" diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp index edda723e1..66d7a4c8f 100644 --- a/cpp/testGraph.cpp +++ b/cpp/testGraph.cpp @@ -13,8 +13,9 @@ #include #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; diff --git a/cpp/testISAM.cpp b/cpp/testISAM.cpp index 93410abf1..9476c2cf2 100644 --- a/cpp/testISAM.cpp +++ b/cpp/testISAM.cpp @@ -10,6 +10,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "SymbolicBayesNet.h" #include "SymbolicFactorGraph.h" #include "Ordering.h" diff --git a/cpp/testInference.cpp b/cpp/testInference.cpp index c6893e768..58ab84668 100644 --- a/cpp/testInference.cpp +++ b/cpp/testInference.cpp @@ -6,6 +6,8 @@ #include +#define GTSAM_MAGIC_KEY + #include "Ordering.h" #include "smallExample.h" #include "inference-inl.h" diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 04468bfd4..ad6391fe1 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -9,10 +9,14 @@ using namespace boost::assign; #include +#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; diff --git a/cpp/testLieConfig.cpp b/cpp/testLieConfig.cpp index 535234872..b75ee685f 100644 --- a/cpp/testLieConfig.cpp +++ b/cpp/testLieConfig.cpp @@ -7,6 +7,9 @@ #include #include + +#define GTSAM_MAGIC_KEY + #include #include "LieConfig-inl.h" diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index afa636c3e..6fe119195 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -8,6 +8,9 @@ #include #include #include // for operator += + +#define GTSAM_MAGIC_KEY + #include #include #include @@ -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& keys) { + Vector g(const VectorConfig& config, const list& 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& keys) { + Matrix G(const VectorConfig& config, const list& 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 keys; keys += "x"; + list keys; keys += "x"; NonlinearConstraint1 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 keys; keys += "x"; + list keys; keys += "x"; NonlinearConstraint1 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 keys1, keys2; keys1 += "x"; keys2 += "y"; + list keys1, keys2; keys1 += "x"; keys2 += "y"; NonlinearConstraint1 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& keys) { + Vector g(const VectorConfig& config, const list& 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& keys) { + Matrix G1(const VectorConfig& config, const list& 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& keys) { + Matrix G2(const VectorConfig& config, const list& 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 keys; keys += "x", "y"; + list keys; keys += "x", "y"; NonlinearConstraint2 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 keys; keys += "x", "y"; + list keys; keys += "x", "y"; NonlinearConstraint2 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 keys1, keys2, keys3; + list keys1, keys2, keys3; keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z"; NonlinearConstraint2 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& keys) { + Vector g(const VectorConfig& config, const list& 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& keys) { + Matrix G(const VectorConfig& config, const list& 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 keys; keys += "x"; + list keys; keys += "x"; NonlinearConstraint1 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 keys; keys += "x"; + list keys; keys += "x"; NonlinearConstraint1 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& keys) { + Vector g(double r, const VectorConfig& config, const list& 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& keys) { + const list& 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 keys; keys += "x"; + list keys; keys += "x"; NonlinearConstraint1 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& keys) { + Vector g(double r, const VectorConfig& config, const list& 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& keys) { + Matrix G1(double c, const VectorConfig& config, const list& 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& keys) { + Matrix G2(double c, const VectorConfig& config, const list& 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 keys; keys += "x", "y"; + list keys; keys += "x", "y"; NonlinearConstraint2 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; diff --git a/cpp/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp index 91a990aef..eb74508bb 100644 --- a/cpp/testNonlinearEquality.cpp +++ b/cpp/testNonlinearEquality.cpp @@ -4,6 +4,9 @@ */ #include + +#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; diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 837fd719a..8d3ea57b7 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -11,6 +11,8 @@ #include +#define GTSAM_MAGIC_KEY + #include "Matrix.h" #include "smallExample.h" #include "simulated2D.h" diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp index 7e27cb39e..114b05689 100644 --- a/cpp/testNonlinearFactorGraph.cpp +++ b/cpp/testNonlinearFactorGraph.cpp @@ -15,6 +15,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "Matrix.h" #include "smallExample.h" #include "FactorGraph-inl.h" diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index e7c7afc6c..7e599996a 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -15,6 +15,8 @@ using namespace boost::assign; #include using namespace boost; +#define GTSAM_MAGIC_KEY + #include "Matrix.h" #include "Ordering.h" #include "smallExample.h" diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index 57b6158df..9998e206b 100644 --- a/cpp/testOrdering.cpp +++ b/cpp/testOrdering.cpp @@ -5,6 +5,9 @@ #include // for operator += #include + +#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 Key; + typedef TypedSymbol Key; PredecessorMap p_map; p_map.insert(1,1); p_map.insert(2,1); diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index ad6ab5829..da9e17a38 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -5,6 +5,9 @@ **/ #include + +#define GTSAM_MAGIC_KEY + #include "numericalDerivative.h" #include "pose2SLAM.h" diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp index a0e9c6140..dd7801e6d 100644 --- a/cpp/testPose2Prior.cpp +++ b/cpp/testPose2Prior.cpp @@ -5,6 +5,9 @@ **/ #include + +#define GTSAM_MAGIC_KEY + #include "numericalDerivative.h" #include "pose2SLAM.h" diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index 6db1e58ae..951011743 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -11,6 +11,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "NonlinearOptimizer-inl.h" #include "FactorGraph-inl.h" #include "Ordering.h" diff --git a/cpp/testPose3SLAM.cpp b/cpp/testPose3SLAM.cpp index 13f45c4ca..7a4271edc 100644 --- a/cpp/testPose3SLAM.cpp +++ b/cpp/testPose3SLAM.cpp @@ -13,6 +13,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "pose3SLAM.h" #include "Ordering.h" diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 600829cb1..0784d9233 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -11,6 +11,9 @@ #include #include #include + +#define GTSAM_MAGIC_KEY + #include #include #include @@ -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 NLGraph; typedef boost::shared_ptr > shared; typedef boost::shared_ptr > shared_c; -typedef NonlinearEquality NLE; +typedef NonlinearEquality NLE; typedef boost::shared_ptr shared_eq; typedef boost::shared_ptr shared_cfg; typedef NonlinearOptimizer Optimizer; @@ -314,17 +317,17 @@ namespace sqp_test1 { // binary constraint between landmarks /** g(x) = x-y = 0 */ - Vector g(const VectorConfig& config, const list& keys) { + Vector g(const VectorConfig& config, const list& keys) { return config[keys.front()] - config[keys.back()]; } /** jacobian at l1 */ - Matrix G1(const VectorConfig& config, const list& keys) { + Matrix G1(const VectorConfig& config, const list& keys) { return eye(2); } /** jacobian at l2 */ - Matrix G2(const VectorConfig& config, const list& keys) { + Matrix G2(const VectorConfig& config, const list& 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& keys) { + Vector g(const VectorConfig& config, const list& keys) { return config[keys.front()] - Vector_(2, 1.0, 1.0); } /** jacobian at x1 */ - Matrix G(const VectorConfig& config, const list& keys) { + Matrix G(const VectorConfig& config, const list& 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 keys1; keys1 += "x1"; + list keys1; keys1 += "x1"; boost::shared_ptr > c1( new NonlinearConstraint1( 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 keys2; keys2 += "l1", "l2"; + list keys2; keys2 += "l1", "l2"; boost::shared_ptr > c2( new NonlinearConstraint2( 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& keys) { - return config[PointKey(getNum(keys.front()))].vector() - - config[PointKey(getNum(keys.back()))].vector(); + Vector g(const Config& config, const list& keys) { + return config[PointKey(keys.front().index())].vector() + - config[PointKey(keys.back().index())].vector(); } /** jacobian at l1 */ - Matrix G1(const Config& config, const list& keys) { + Matrix G1(const Config& config, const list& keys) { return eye(3); } /** jacobian at l2 */ - Matrix G2(const Config& config, const list& keys) { + Matrix G2(const Config& config, const list& 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 keys; keys += "l1", "l2"; + list keys; keys += "l1", "l2"; boost::shared_ptr > c2( new NonlinearConstraint2( 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); diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 078894300..7eebf21e5 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -8,6 +8,9 @@ #include // for operator += #include // for insert #include + +#define GTSAM_MAGIC_KEY + #include #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& keys) { +Vector g_func(const VectorConfig& config, const list& keys) { return config[keys.front()]-config[keys.back()]; } /** jacobian at l1 */ -Matrix jac_g1(const VectorConfig& config, const list& keys) { +Matrix jac_g1(const VectorConfig& config, const list& keys) { return eye(2); } /** jacobian at l2 */ -Matrix jac_g2(const VectorConfig& config, const list& keys) { +Matrix jac_g2(const VectorConfig& config, const list& keys) { return -1*eye(2); } } // \namespace sqp_LinearMapWarp2 @@ -72,12 +75,12 @@ Matrix jac_g2(const VectorConfig& config, const list& keys) { namespace sqp_LinearMapWarp1 { // Unary Constraint on x1 /** g(x) = x -[1;1] = 0 */ -Vector g_func(const VectorConfig& config, const list& keys) { +Vector g_func(const VectorConfig& config, const list& keys) { return config[keys.front()]-Vector_(2, 1.0, 1.0); } /** jacobian at x1 */ -Matrix jac_g(const VectorConfig& config, const list& keys) { +Matrix jac_g(const VectorConfig& config, const list& keys) { return eye(2); } } // \namespace sqp_LinearMapWarp12 @@ -90,12 +93,12 @@ typedef SQPOptimizer Optimizer; */ NLGraph linearMapWarpGraph() { // constant constraint on x1 - list keyx; keyx += "x1"; + list keyx; keyx += "x1"; boost::shared_ptr > c1( new NonlinearConstraint1( 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 keys; keys += "l1", "l2"; + list keys; keys += "l1", "l2"; boost::shared_ptr > c2( new NonlinearConstraint2( 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 NLC1; typedef boost::shared_ptr shared_NLC1; typedef NonlinearConstraint2 NLC2; typedef boost::shared_ptr shared_NLC2; -typedef NonlinearEquality NLE; +typedef NonlinearEquality NLE; typedef boost::shared_ptr 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& keys) { +Vector g_func(const VectorConfig& config, const list& 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& keys) { } /** jacobian at pose */ -Matrix jac_g1(const VectorConfig& config, const list& keys) { +Matrix jac_g1(const VectorConfig& config, const list& 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& keys) { +Matrix jac_g2(const VectorConfig& config, const list& 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 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 obstacleAvoidGraph() { // create a binary inequality constraint that forces the middle point away from // the obstacle - list keys; keys += "x2", "obs"; + list 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& keys) { +Vector g_func(double radius, const VectorConfig& config, const list& 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& key } /** jacobian at pose */ -Matrix jac_g1(const VectorConfig& config, const list& keys) { +Matrix jac_g1(const VectorConfig& config, const list& 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& keys) { +Matrix jac_g2(const VectorConfig& config, const list& 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 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 obstacleAvoidGraphGeneral() { // create a binary inequality constraint that forces the middle point away from // the obstacle - list keys; keys += "x2", "obs"; + list 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); diff --git a/cpp/testSubgraphPreconditioner.cpp b/cpp/testSubgraphPreconditioner.cpp index 9236f6e13..491369248 100644 --- a/cpp/testSubgraphPreconditioner.cpp +++ b/cpp/testSubgraphPreconditioner.cpp @@ -11,6 +11,8 @@ using namespace boost::assign; #include +#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; diff --git a/cpp/testSymbolicBayesNet.cpp b/cpp/testSymbolicBayesNet.cpp index ee74c8d78..b26fa1816 100644 --- a/cpp/testSymbolicBayesNet.cpp +++ b/cpp/testSymbolicBayesNet.cpp @@ -10,6 +10,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "Ordering.h" #include "smallExample.h" #include "SymbolicBayesNet.h" diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index d5dd4cd5d..f81d13aa8 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -9,6 +9,8 @@ using namespace boost::assign; #include +#define GTSAM_MAGIC_KEY + #include "Ordering.h" #include "smallExample.h" #include "SymbolicFactorGraph.h" diff --git a/cpp/testTupleConfig.cpp b/cpp/testTupleConfig.cpp index 12730cd07..8598fab4e 100644 --- a/cpp/testTupleConfig.cpp +++ b/cpp/testTupleConfig.cpp @@ -7,6 +7,9 @@ #include #include + +#define GTSAM_MAGIC_KEY + #include #include @@ -18,8 +21,8 @@ using namespace gtsam; using namespace std; -typedef Symbol PoseKey; -typedef Symbol PointKey; +typedef TypedSymbol PoseKey; +typedef TypedSymbol PointKey; typedef PairConfig Config; /* ************************************************************************* */ diff --git a/cpp/testVSLAMConfig.cpp b/cpp/testVSLAMConfig.cpp index 1f9cbe38b..c08edc647 100644 --- a/cpp/testVSLAMConfig.cpp +++ b/cpp/testVSLAMConfig.cpp @@ -5,6 +5,9 @@ */ #include + +#define GTSAM_MAGIC_KEY + #include "VectorConfig.h" #include "visualSLAM.h" diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index 863f621ee..aec5658ea 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -4,6 +4,8 @@ #include +#define GTSAM_MAGIC_KEY + #include "visualSLAM.h" #include "Point3.h" #include "Pose3.h" diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index ef548feea..6c5b18789 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -11,6 +11,8 @@ #include 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 keys; + list 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 keys; + list keys; keys.push_back("l1"); keys.push_back("l2"); diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp index a80b298e1..0c1cf23bc 100644 --- a/cpp/testVectorConfig.cpp +++ b/cpp/testVectorConfig.cpp @@ -15,6 +15,8 @@ #include #endif //HAVE_BOOST_SERIALIZATION +#define GTSAM_MAGIC_KEY + #include #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; diff --git a/cpp/timeGaussianFactor.cpp b/cpp/timeGaussianFactor.cpp index 9a55ecd12..4c43250df 100644 --- a/cpp/timeGaussianFactor.cpp +++ b/cpp/timeGaussianFactor.cpp @@ -4,6 +4,8 @@ * @author Alireza Fathi */ +#define GTSAM_MAGIC_KEY + #include /*STL/C++*/ diff --git a/cpp/timeGaussianFactorGraph.cpp b/cpp/timeGaussianFactorGraph.cpp index a5231c7a7..372c8f5e1 100644 --- a/cpp/timeGaussianFactorGraph.cpp +++ b/cpp/timeGaussianFactorGraph.cpp @@ -4,6 +4,8 @@ * @author Frank Dellaert */ +#define GTSAM_MAGIC_KEY + #include #include #include "smallExample.h" diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h index 362b07d04..1fc27ab75 100644 --- a/cpp/visualSLAM.h +++ b/cpp/visualSLAM.h @@ -22,8 +22,8 @@ namespace gtsam { namespace visualSLAM { /** * Typedefs that make up the visualSLAM namespace. */ - typedef Symbol PoseKey; - typedef Symbol PointKey; + typedef TypedSymbol PoseKey; + typedef TypedSymbol PointKey; typedef PairConfig Config; typedef NonlinearFactorGraph Graph; typedef NonlinearEquality PoseConstraint;