From a45893a434e10d0dc8ab63a5991f7996be917d14 Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Tue, 19 Jan 2010 01:18:28 +0000 Subject: [PATCH] selective incremental linearization works; bug fix in landmark initialization (frank01 again much faster) --- cpp/ISAM2-inl.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 77d1eb685..e03d37e85 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -124,6 +124,8 @@ namespace gtsam { FactorGraph affectedFactors; list newFactorsKeys = newFactors.keys(); +#if 1 + // relinearize all keys that are in newFactors, and already exist (not new variables!) list keysToRelinearize; list oldKeys = nonlinearFactors_.keys(); @@ -132,7 +134,6 @@ namespace gtsam { keysToRelinearize.push_back(key); } -#if 1 // 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 FactorGraph > allAffected; @@ -147,10 +148,14 @@ namespace gtsam { allAffected.push_back(*it); } list keysToBeRemoved = allAffected.keys(); + #else - list keysToBeRemoved = nonlinearFactors_.keys(); // todo/debug - relinearize all (a cumbersome way to do batch) + // debug only: full relinearization in each step + list keysToRelinearize = nonlinearFactors_.keys(); + list keysToBeRemoved = nonlinearFactors_.keys(); #endif + // remove affected factors this->removeTop(keysToBeRemoved, affectedFactors, orphans); // selectively update the linearization point