Added accessor for NonlinearISAM, comments and whitespace
parent
d87ffe7ece
commit
e4cda1b360
|
@ -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>
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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>());
|
||||
|
|
|
@ -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_; }
|
||||
|
||||
|
|
|
@ -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; }
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue