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

fluid relinearization works

release/4.3a0
Michael Kaess 2010-01-21 17:41:54 +00:00
parent 16a9ba1fdc
commit 2183e09c67
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/ISAM2-inl.h
View File

@ -151,6 +151,15 @@ namespace gtsam {
// remember the new factors for later relinearization // remember the new factors for later relinearization
nonlinearFactors_.push_back(newFactors); nonlinearFactors_.push_back(newFactors);
// 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...
// the goal is to relinearize all variables directly affected by new factors
boost::shared_ptr<FactorGraph<NonlinearFactor<Config> > > allAffected = getAffectedFactors(marked_);
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());