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

Added accessor for NonlinearISAM, comments and whitespace

release/4.3a0
Alex Cunningham 2011-01-29 00:53:15 +00:00
parent d87ffe7ece
commit e4cda1b360
6 changed files with 63 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/inference/BayesTree-inl.h
View File

@ -118,15 +118,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesTree<CONDITIONAL>::Clique::print(const string& s) const {
cout << s << "Clique ";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { cout << conditional->key() << " "; }
cout << "| ";
BOOST_FOREACH(const Index sep, separator_) { cout << sep << " "; }
cout << "\n";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) {
conditional->print(" " + s + "conditional");
}
}
cout << s << "Clique ";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
cout << conditional->key() << " ";
cout << "| ";
BOOST_FOREACH(const Index sep, separator_)
cout << sep << " ";
cout << "\n";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_)
conditional->print(" " + s + "conditional");
}
/* ************************************************************************* */
template<class CONDITIONAL>

21
gtsam/inference/BayesTree.h
View File

@ -43,13 +43,14 @@ namespace gtsam {
public:
typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
/** A Clique in the tree is an incomplete Bayes net: the variables
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on is the separator. We also have pointers up and down the tree.
* on are the separator. We also have pointers up and down the tree.
*/
struct Clique: public BayesNet<CONDITIONAL> {
@ -62,7 +63,7 @@ namespace gtsam {
weak_ptr parent_;
std::list<shared_ptr> children_;
std::list<Index> separator_; /** separator keys */
typename CONDITIONAL::Factor::shared_ptr cachedFactor_;
typename CONDITIONAL::Factor::shared_ptr cachedFactor_;
friend class BayesTree<CONDITIONAL>;
@ -119,7 +120,8 @@ namespace gtsam {
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<typename CONDITIONAL::Factor> joint(shared_ptr C2, shared_ptr root);
};
}; // \struct Clique
// typedef for shared pointers to cliques
typedef boost::shared_ptr<Clique> sharedClique;
@ -174,7 +176,8 @@ namespace gtsam {
sharedClique addClique(const sharedConditional& conditional,
std::list<sharedClique>& child_cliques);
/** Add a conditional to the front of a clique, i.e. a conditional whose
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
@ -198,8 +201,7 @@ namespace gtsam {
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees);
/** Destructor */
virtual ~BayesTree() {
}
virtual ~BayesTree() {}
/**
* Constructing Bayes trees
@ -208,7 +210,8 @@ namespace gtsam {
/** Insert a new conditional */
void insert(const sharedConditional& conditional);
/** Insert a new clique corresponding to the given Bayes net.
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/

9
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -45,11 +45,10 @@ void NonlinearISAM<Values>::update(const Factors& newFactors, const Values& init
linPoint_.insert(initialValues);
// Augment ordering
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
ordering_.tryInsert(key, ordering_.nVars());
}
}
// FIXME: should just loop over new values
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors)
BOOST_FOREACH(const Symbol& key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
newFactors.linearize(linPoint_, ordering_)->template dynamicCastFactors<GaussianFactorGraph>());

16
gtsam/nonlinear/NonlinearISAM.h
View File

@ -53,11 +53,14 @@ protected:
public:
/** default constructor will disable periodic reordering */
NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {}
/** Periodically reorder and relinearize */
NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
/**
* Periodically reorder and relinearize
* @param reorderInterval is the number of updates between reorderings,
* 0 never reorders (and is dangerous for memory consumption)
* 1 (default) reorders every time, in worse case is batch every update
* typical values are 50 or 100
*/
NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
/** Add new factors along with their initial linearization points */
void update(const Factors& newFactors, const Values& initialValues);
@ -70,6 +73,9 @@ public:
// access
/** access the underlying bayes tree */
const GaussianISAM& bayesTree() const { return isam_; }
/** Return the current linearization point */
const Values& getLinearizationPoint() const { return linPoint_; }

34
gtsam/nonlinear/Ordering.h
View File

@ -33,6 +33,9 @@
namespace gtsam {
/**
* An ordering is a map from symbols (non-typed keys) to integer indices
*/
class Ordering : Testable<Ordering> {
protected:
typedef boost::fast_pool_allocator<std::pair<const Symbol, Index> > Allocator;
@ -62,6 +65,8 @@ public:
iterator end() { return order_.end(); }
const_iterator end() const { return order_.end(); }
// access to integer indices
Index& at(const Symbol& key) { return operator[](key); }
Index at(const Symbol& key) const { return operator[](key); }
bool tryAt(const Symbol& key, Index& index) const {
@ -77,18 +82,29 @@ public:
Index operator[](const Symbol& key) const {
const_iterator i=order_.find(key); assert(i != order_.end()); return i->second; }
iterator insert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(tryInsert(key_order));
assert(it_ok.second);
return it_ok.first; }
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
// adding symbols
/**
* Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
* i.e., returns a pair of iterator and success (false if already present)
*/
std::pair<iterator,bool> tryInsert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(order_.insert(key_order));
if(it_ok.second == true && key_order.second+1 > nVars_)
nVars_ = key_order.second+1;
return it_ok; }
std::pair<iterator,bool> it_ok(order_.insert(key_order));
if(it_ok.second == true && key_order.second+1 > nVars_)
nVars_ = key_order.second+1;
return it_ok;
}
std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
/** Try insert, but will fail if the key is already present */
iterator insert(const value_type& key_order) {
std::pair<iterator,bool> it_ok(tryInsert(key_order));
assert(it_ok.second);
return it_ok.first;
}
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
bool exists(const Symbol& key) const { return order_.count(key); }
Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }

2
tests/testNonlinearISAM.cpp
View File

@ -19,7 +19,7 @@ const double tol=1e-5;
/* ************************************************************************* */
TEST(testNonlinearISAM, markov_chain ) {
int reorder_interval = 2;
PlanarISAM isam(reorder_interval);
PlanarISAM isam(reorder_interval); // create an ISAM object
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5));
Sampler sampler(model, 42u);