From df436f32d187892b084b9d5d96eab376746524fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Tue, 19 Sep 2023 17:55:45 +0200 Subject: [PATCH 1/2] Mark current clique affected. --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 52e56260d6..560e48fa05 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -118,6 +118,11 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( std::set additionalKeys; for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; + // Mark all frontal keys of the current clique. + for(Key i: clique->conditional()->frontals()) { + additionalKeys.insert(i); + } + // Recursively mark all of the children key that contain the marginal key. for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys); } From aceac22526c04ba021f010d04c912a93963ed6d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Tue, 19 Sep 2023 19:24:30 +0200 Subject: [PATCH 2/2] simplify --- gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 560e48fa05..ec543e45fc 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -119,9 +119,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; // Mark all frontal keys of the current clique. - for(Key i: clique->conditional()->frontals()) { - additionalKeys.insert(i); - } + additionalKeys.insert(clique->conditional()->frontals().begin(), + clique->conditional()->frontals().end()); // Recursively mark all of the children key that contain the marginal key. for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys);