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

coments and variables only

release/4.3a0
Frank Dellaert 2010-01-21 20:15:52 +00:00
parent 7575996d52
commit 2cc777228b
2 changed files with 14 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cpp/ISAM2-inl.h
View File

@ -67,7 +67,7 @@ namespace gtsam {
/** Create a Bayes Tree from a nonlinear factor graph */ /** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class Config> template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& 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) { : BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), theta_(config) {
// todo: repeats calculation above, just to set "cached" // todo: repeats calculation above, just to set "cached"
_eliminate_const(nlfg.linearize(config), cached_, ordering); _eliminate_const(nlfg.linearize(config), cached_, ordering);
} }
@ -113,7 +113,7 @@ namespace gtsam {
nonlinearAffectedFactors.push_back(*it); nonlinearAffectedFactors.push_back(*it);
} }
return nonlinearAffectedFactors.linearize(linPoint_); return nonlinearAffectedFactors.linearize(theta_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -135,31 +135,27 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors, void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) { const Config& theta_new, Cliques& orphans, double wildfire_threshold, double relinearize_threshold) {
// todo - debug only // todo - debug only
// marked_ = nonlinearFactors_.keys(); // marked_ = nonlinearFactors_.keys();
//// 1 - Remember the new factors for later relinearization
//// 1 - add in new information
// add new variables
linPoint_.insert(config);
// remember the new factors for later relinearization
nonlinearFactors_.push_back(newFactors); nonlinearFactors_.push_back(newFactors);
//// 2 - add in new information
// add new variables
theta_.insert(theta_new);
// todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!! It's about factors!!! // todo - not in lyx yet: relin requires more than just removing the cliques corresponding to the variables!!!
// It's about factors!!!
// basically calculate all the keys contained in the factors that contain any of the keys... // basically calculate all the keys contained in the factors that contain any of the keys...
// the goal is to relinearize all variables directly affected by new factors // the goal is to relinearize all variables directly affected by new factors
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > allAffected = getAffectedFactors(marked_); boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > allAffected = getAffectedFactors(marked_);
marked_ = allAffected->keys(); marked_ = allAffected->keys();
// merge keys of new factors with mask // merge keys of new factors with mask
const list<Symbol> newKeys = newFactors.keys(); const list<Symbol> newKeys = newFactors.keys();
marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end()); marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end());
@ -167,7 +163,7 @@ namespace gtsam {
marked_.sort(); marked_.sort();
marked_.unique(); marked_.unique();
//// 2 - invalidate all cliques involving marked variables //// 4 - removeTop invalidate all cliques involving marked variables
// remove affected factors // remove affected factors
BayesNet<GaussianConditional> affectedBayesNet; BayesNet<GaussianConditional> affectedBayesNet;
@ -232,7 +228,7 @@ namespace gtsam {
//// 8 - relinearize selected variables //// 8 - relinearize selected variables
linPoint_ = expmap(linPoint_, deltaMarked); theta_ = expmap(theta_, deltaMarked);
} }

4
cpp/ISAM2.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
protected: protected:
// current linearization point // current linearization point
Config linPoint_; Config theta_;
// for keeping all original nonlinear factors // for keeping all original nonlinear factors
NonlinearFactorGraph<Config> nonlinearFactors_; NonlinearFactorGraph<Config> nonlinearFactors_;
@ -68,7 +68,7 @@ namespace gtsam {
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config, void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config,
double wildfire_threshold = 0., double relinearize_threshold = 0.); double wildfire_threshold = 0., double relinearize_threshold = 0.);
const Config estimate() const {return linPoint_;} const Config estimate() const {return theta_;}
private: private: