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

Added FactorGraph::replace(index, factor) to replace one factor with another (used in FrameSLAM).

release/4.3a0
Richard Roberts 2009-12-18 00:13:38 +00:00
parent 3c0ae0ec1c
commit ff817551de
2 changed files with 55 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
cpp/FactorGraph-inl.h
View File

@ -17,6 +17,7 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <colamd/colamd.h>
#include "Ordering.h"
#include "FactorGraph.h"
@ -86,19 +87,7 @@ void FactorGraph<Factor>::push_back(sharedFactor factor) {
if (factor==NULL) return;
int i = factors_.size() - 1; // index of factor
list<string> keys = factor->keys(); // get keys for factor
BOOST_FOREACH(string key, keys){ // for each key push i onto list
Indices::iterator it = indices_.find(key); // old list for that key (if exists)
if (it==indices_.end()){ // there's no list yet
list<int> indices(1,i); // so make one
indices_.insert(make_pair(key,indices)); // insert new indices into factorMap
}
else {
list<int> *indices_ptr = &(it->second); // get the list
indices_ptr->push_back(i); // add the index i to it
}
}
associateFactor(i, factor);
}
/* ************************************************************************* */
@ -109,6 +98,36 @@ void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) {
push_back(*factor);
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::replace(int index, sharedFactor factor) {
if(index >= factors_.size())
throw invalid_argument(boost::str(boost::format(
"Factor graph does not contain a factor with index %d.") % index));
//if(factors_[index] == NULL)
// throw invalid_argument(boost::str(boost::format(
// "Factor with index %d is NULL." % index)));
if(factors_[index] != NULL) {
// Remove this factor from its variables' index lists
list<string> keys(factor->keys());
BOOST_FOREACH(string key, 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));
}
}
}
// Replace the factor
factors_[index] = factor;
associateFactor(index, factor);
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::keys() const {
@ -231,6 +250,24 @@ FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
return found;
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
list<string> keys = factor->keys(); // get keys for factor
BOOST_FOREACH(string key, keys){ // for each key push i onto list
Indices::iterator it = indices_.find(key); // old list for that key (if exists)
if (it==indices_.end()){ // there's no list yet
list<int> indices(1,index); // so make one
indices_.insert(make_pair(key,indices)); // insert new indices into factorMap
}
else {
list<int> *indices_ptr = &(it->second); // get the list
indices_ptr->push_back(index); // add the index i to it
}
}
}
/* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */
@ -253,6 +290,4 @@ FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Fa
return fg;
}
/* ************************************************************************* */
}

5
cpp/FactorGraph.h
View File

@ -79,6 +79,9 @@ namespace gtsam {
/** push back many factors */
void push_back(const FactorGraph<Factor>& factors);
/** replace a factor by index */
void replace(int index, sharedFactor factor);
/** return keys in some random order */
Ordering keys() const;
@ -111,6 +114,8 @@ namespace gtsam {
std::vector<sharedFactor> findAndRemoveFactors(const std::string& key);
private:
/** Associate factor index with the variables connected to the factor */
void associateFactor(int index, sharedFactor factor);
/** Serialization function */
friend class boost::serialization::access;