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

VectorConfig/Config confusion resolved; planarSLAM integrated

release/4.3a0
Michael Kaess 2010-01-18 08:05:33 +00:00
parent eb3e0067ef
commit 18414b1286
4 changed files with 54 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cpp/GaussianISAM2.cpp
View File

@ -11,7 +11,9 @@ using namespace gtsam;
// Explicitly instantiate so we don't have to include everywhere
#include "ISAM2-inl.h"
//template class ISAM2<GaussianConditional, VectorConfig>;
//template class ISAM2<GaussianConditional, planarSLAM::Config>;
namespace gtsam {
@ -25,8 +27,6 @@ void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result)
result.insert(cg->key(), x); // store result in partial solution
}
BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
// list<GaussianISAM2::Clique::shared_ptr>::const_iterator child;
// for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
optimize2(child, result);
}
}
@ -39,4 +39,27 @@ VectorConfig optimize2(const GaussianISAM2& bayesTree) {
return result;
}
#if 0
/* ************************************************************************* */
void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result) {
// parents are assumed to already be solved and available in result
GaussianISAM2_P::Clique::const_reverse_iterator it;
for (it = clique->rbegin(); it!=clique->rend(); it++) {
GaussianConditional::shared_ptr cg = *it;
result.insert(cg->key(), cg->solve(result)); // store result in partial solution
}
BOOST_FOREACH(GaussianISAM2_P::sharedClique child, clique->children_) {
optimize2(child, result);
}
}
#endif
/* ************************************************************************* */
VectorConfig optimize2(const GaussianISAM2_P& bayesTree) {
VectorConfig result;
// starting from the root, call optimize on each conditional
optimize2(bayesTree.root(), result);
return result;
}
} /// namespace gtsam

12
cpp/GaussianISAM2.h
View File

@ -11,6 +11,7 @@
#include "ISAM2.h"
#include "GaussianConditional.h"
#include "GaussianFactor.h"
#include "planarSLAM.h"
namespace gtsam {
@ -22,4 +23,15 @@ namespace gtsam {
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2& bayesTree);
// todo: copy'n'paste to avoid template hell
typedef ISAM2<GaussianConditional, planarSLAM::Config> GaussianISAM2_P;
// recursively optimize this conditional and all subtrees
// void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result);
// optimize the BayesTree, starting from the root
VectorConfig optimize2(const GaussianISAM2_P& bayesTree);
}/// namespace gtsam

23
cpp/ISAM2-inl.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
using namespace std;
// from inference-inl.h - need to additionally return the newly created factor for caching
boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Symbol& key) {
boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Symbol& key) {
// combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor
@ -45,7 +45,7 @@ namespace gtsam {
}
// from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
@ -54,7 +54,7 @@ namespace gtsam {
return chordalBayesNet;
}
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, cachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
// make a copy that can be modified locally
FactorGraph<GaussianFactor> graph_ignored = graph;
return _eliminate(graph_ignored, cached, ordering);
@ -67,9 +67,9 @@ namespace gtsam {
/** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config), estimate_(config) {
: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config) {
// todo: repeats calculation above, just to set "cached"
_eliminate_const(nlfg.linearize(config), cached, ordering);
_eliminate_const(nlfg.linearize(config), cached_, ordering);
}
/* ************************************************************************* */
@ -108,7 +108,7 @@ namespace gtsam {
it--;
const Symbol& key = *it;
// retrieve the cached factor and add to boundary
cachedBoundary.push_back(cached[key]);
cachedBoundary.push_back(cached_[key]);
}
return cachedBoundary;
@ -119,13 +119,16 @@ namespace gtsam {
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans) {
#if 0 // todo - temporarily disabled --------------------------------------------------------------------------------------------------
// copy variables into config_, but don't overwrite existing entries (current linearization point!)
for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) {
if (!linPoint_.contains(it->first)) {
linPoint_.insert(it->first, it->second);
estimate_.insert(it->first, it->second);
}
}
#else
linPoint_ = config;
#endif
FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
@ -155,7 +158,7 @@ namespace gtsam {
}
// eliminate into a Bayes net
BayesNet<Conditional> bayesNet = _eliminate(factors, cached, ordering);
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
// remember the new factors for later relinearization
nonlinearFactors_.push_back(newFactors);
@ -176,9 +179,7 @@ namespace gtsam {
}
// update solution - todo: potentially expensive
VectorConfig delta = optimize2(*this);
// delta.print();
estimate_ += delta;
delta_ = optimize2(*this);
}

10
cpp/ISAM2.h
View File

@ -24,7 +24,7 @@
namespace gtsam {
typedef std::map<Symbol, GaussianFactor::shared_ptr> cachedFactors;
typedef std::map<Symbol, GaussianFactor::shared_ptr> CachedFactors;
template<class Conditional, class Config>
class ISAM2: public BayesTree<Conditional> {
@ -34,14 +34,14 @@ namespace gtsam {
// current linearization point
Config linPoint_;
// most recent estimate
Config estimate_;
// most recent solution
VectorConfig delta_;
// for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_;
// cached intermediate results for restarting computation in the middle
cachedFactors cached;
CachedFactors cached_;
public:
@ -65,7 +65,7 @@ namespace gtsam {
void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
const Config estimate() {return estimate_;}
const Config estimate() {return expmap(linPoint_, delta_);}
private:
FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys);