From 3ab9a1e3cc3e7382d987a79b58a79ea2b4b0284d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jun 2019 13:53:37 -0400 Subject: [PATCH] Made detail handling more explicit in update --- gtsam/nonlinear/ISAM2-impl.h | 36 +++++++++++++++++++---------------- gtsam/nonlinear/ISAM2.cpp | 19 +++++++++--------- gtsam/nonlinear/ISAM2.h | 5 ++--- gtsam/nonlinear/ISAM2Params.h | 6 +++--- tests/testGaussianISAM2.cpp | 6 ++++-- 5 files changed, 39 insertions(+), 33 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 0fb70558e..f668e12c7 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -218,7 +218,7 @@ struct GTSAM_EXPORT UpdateImpl { void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const { gttic(updateKeys); // Observed keys for detailed results - if (params_.enableDetailedResults) { + if (result->detail && params_.enableDetailedResults) { for (Key key : markedKeys) { result->detail->variableStatus[key].isObserved = true; } @@ -359,8 +359,8 @@ struct GTSAM_EXPORT UpdateImpl { // Mark keys in \Delta above threshold \beta: KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots, const VectorValues& delta, - const KeySet& fixedVariables, KeySet* markedKeys, - ISAM2Result* result) const { + const KeySet& fixedVariables, + KeySet* markedKeys) const { gttic(gatherRelinearizeKeys); // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. KeySet relinKeys = @@ -381,37 +381,41 @@ struct GTSAM_EXPORT UpdateImpl { } } - // Record above relinerization threshold keys in detailed results - if (params_.enableDetailedResults) { - for (Key key : relinKeys) { - result->detail->variableStatus[key].isAboveRelinThreshold = true; - result->detail->variableStatus[key].isRelinearized = true; - } - } - // Add the variables being relinearized to the marked keys markedKeys->insert(relinKeys.begin(), relinKeys.end()); return relinKeys; } + // Record relinerization threshold keys in detailed results + void recordRelinearizeDetail(const KeySet& relinKeys, + ISAM2Result::DetailedResults* detail) const { + if (detail && params_.enableDetailedResults) { + for (Key key : relinKeys) { + detail->variableStatus[key].isAboveRelinThreshold = true; + detail->variableStatus[key].isRelinearized = true; + } + } + } + // Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. void fluidFindAll(const ISAM2::Roots& roots, const KeySet& relinKeys, - KeySet* markedKeys, ISAM2Result* result) const { + KeySet* markedKeys, + ISAM2Result::DetailedResults* detail) const { gttic(fluidFindAll); for (const auto& root : roots) // add other cliques that have the marked ones in the separator root->findAll(relinKeys, markedKeys); // Relinearization-involved keys for detailed results - if (params_.enableDetailedResults) { + if (detail && params_.enableDetailedResults) { KeySet involvedRelinKeys; for (const auto& root : roots) root->findAll(relinKeys, &involvedRelinKeys); for (Key key : involvedRelinKeys) { - if (!result->detail->variableStatus[key].isAboveRelinThreshold) { - result->detail->variableStatus[key].isRelinearizeInvolved = true; - result->detail->variableStatus[key].isRelinearized = true; + if (!detail->variableStatus[key].isAboveRelinThreshold) { + detail->variableStatus[key].isRelinearizeInvolved = true; + detail->variableStatus[key].isRelinearized = true; } } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 1d1447e92..4e4a507bf 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -337,7 +337,7 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams, } // Root clique variables for detailed results - if (params_.enableDetailedResults) { + if (result->detail && params_.enableDetailedResults) { for (const auto& root : roots_) for (Key var : *root->conditional()) result->detail->variableStatus[var].inRootClique = true; @@ -346,9 +346,8 @@ KeySet ISAM2::recalculate(const ISAM2UpdateParams& updateParams, return affectedKeysSet; } /* ************************************************************************* */ -void ISAM2::addVariables( - const Values& newTheta, - ISAM2Result::DetailedResults::StatusMap* variableStatus) { +void ISAM2::addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail) { gttic(addNewVariables); theta_.insert(newTheta); @@ -359,9 +358,9 @@ void ISAM2::addVariables( RgProd_.insert(newTheta.zeroVectors()); // New keys for detailed results - if (variableStatus && params_.enableDetailedResults) { + if (detail && params_.enableDetailedResults) { for (Key key : newTheta.keys()) { - (*variableStatus)[key].isNew = true; + detail->variableStatus[key].isNew = true; } } } @@ -430,7 +429,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.detail ? &result.detail->variableStatus : 0); + addVariables(newTheta, result.detail.get_ptr()); gttic(evaluate_error_before); if (params_.evaluateNonlinearError) @@ -448,11 +447,13 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, if (relinearizeThisStep) { // 4. Mark keys in \Delta above threshold \beta: relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_, - &result.markedKeys, &result); + &result.markedKeys); + update.recordRelinearizeDetail(relinKeys, result.detail.get_ptr()); if (!relinKeys.empty()) { // 5. Mark all cliques that involve marked variables \Theta_{J} and all // their ancestors. - update.fluidFindAll(roots_, relinKeys, &result.markedKeys, &result); + update.fluidFindAll(roots_, relinKeys, &result.markedKeys, + result.detail.get_ptr()); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 19199b22d..bd1477072 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -307,9 +307,8 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @param newTheta Initial values for new variables * @param variableStatus optional detailed result structure */ - void addVariables( - const Values& newTheta, - ISAM2Result::DetailedResults::StatusMap* variableStatus = 0); + void addVariables(const Values& newTheta, + ISAM2Result::DetailedResults* detail = 0); /** * Remove variables from the ISAM2 system. diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index afddd1f8e..5cf962e43 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -234,8 +234,8 @@ struct GTSAM_EXPORT ISAM2Params { Factorization _factorization = ISAM2Params::CHOLESKY, bool _cacheLinearizedFactors = true, const KeyFormatter& _keyFormatter = - DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) + DefaultKeyFormatter, ///< see ISAM2::Params::keyFormatter, + bool _enableDetailedResults = false) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), @@ -244,7 +244,7 @@ struct GTSAM_EXPORT ISAM2Params { factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), + enableDetailedResults(_enableDetailedResults), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 871e4bc18..68d10bb7b 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -47,9 +47,11 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, boost::optional full_graph = boost::none, - const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), + const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, + 0, false, true, + ISAM2Params::CHOLESKY, true, + DefaultKeyFormatter, true), size_t maxPoses = 10) { - // These variables will be reused and accumulate factors and values ISAM2 isam(params); Values fullinit;