diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index b73be9b39..f09e54e47 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -67,7 +67,7 @@ namespace gtsam { /** Create a Bayes Tree from a nonlinear factor graph */ template ISAM2::ISAM2(const NonlinearFactorGraph& nlfg, const Ordering& ordering, const Config& config) - : BayesTree(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config) { + : BayesTree(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), theta_(config) { // todo: repeats calculation above, just to set "cached" _eliminate_const(nlfg.linearize(config), cached_, ordering); } @@ -113,7 +113,7 @@ namespace gtsam { nonlinearAffectedFactors.push_back(*it); } - return nonlinearAffectedFactors.linearize(linPoint_); + return nonlinearAffectedFactors.linearize(theta_); } /* ************************************************************************* */ @@ -135,31 +135,27 @@ namespace gtsam { /* ************************************************************************* */ template void ISAM2::update_internal(const NonlinearFactorGraph& 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 -// marked_ = nonlinearFactors_.keys(); + // marked_ = nonlinearFactors_.keys(); - - //// 1 - add in new information - - // add new variables - linPoint_.insert(config); - - // remember the new factors for later relinearization + //// 1 - Remember the new factors for later relinearization 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... // the goal is to relinearize all variables directly affected by new factors boost::shared_ptr > > allAffected = getAffectedFactors(marked_); marked_ = allAffected->keys(); - // merge keys of new factors with mask const list newKeys = newFactors.keys(); marked_.insert(marked_.begin(), newKeys.begin(), newKeys.end()); @@ -167,7 +163,7 @@ namespace gtsam { marked_.sort(); marked_.unique(); - //// 2 - invalidate all cliques involving marked variables + //// 4 - removeTop invalidate all cliques involving marked variables // remove affected factors BayesNet affectedBayesNet; @@ -232,7 +228,7 @@ namespace gtsam { //// 8 - relinearize selected variables - linPoint_ = expmap(linPoint_, deltaMarked); + theta_ = expmap(theta_, deltaMarked); } diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index bb8f3b255..112c0056a 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -32,7 +32,7 @@ namespace gtsam { protected: // current linearization point - Config linPoint_; + Config theta_; // for keeping all original nonlinear factors NonlinearFactorGraph nonlinearFactors_; @@ -68,7 +68,7 @@ namespace gtsam { void update(const NonlinearFactorGraph& newFactors, const Config& config, double wildfire_threshold = 0., double relinearize_threshold = 0.); - const Config estimate() const {return linPoint_;} + const Config estimate() const {return theta_;} private: