Fix marginalization of non-stamped variables in fixed-lag smoother

Old stamped variables (e.g. expired poses) were incorrectly shielded
from marginalization when they shared a cross-window constraint with a
recent stamped variable (e.g. an odometry factor linking kf_N to
kf_N+1). This prevented prompt marginalization of expired poses and,
more importantly, kept non-stamped variables like landmarks alive in the
graph indefinitely even after all their associated poses left the
sliding window.

The fix skips old stamped variables when building the connected
(protected) set in VariableStampIndex::query(), so they are returned as
marginalization candidates alongside any exclusively-attached landmarks.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Author: jakemclaughlin6

vesta_optimizers/include/vesta_optimizers/variable_stamp_index.h

@@ -142,7 +142,12 @@ class VariableStampIndex
       }
     }
 
-    // Now find all of the variables connected to the recent variables
+    // Now find all of the variables connected to the recent variables.
+    // Old stamped variables (stamp < threshold) are NOT added to the connected
+    // set even if they share a constraint with a recent variable (e.g. via an
+    // odometry factor). This ensures that expired poses—and the non-stamped
+    // variables attached exclusively to them—are marginalized promptly instead
+    // of being kept alive by cross-window constraints.
     std::unordered_set<vesta_core::UUID> connected_variable_uuids;
     for (const auto& recent_variable_uuid : recent_variable_uuids)
     {
@@ -160,6 +165,14 @@ class VariableStampIndex
           {
             for (const auto& connected_variable_uuid : constraints_iter->second)
             {
+              // Skip old stamped variables: they are candidates for
+              // marginalization and should not be protected by cross-window
+              // constraints (e.g. odometry linking an old pose to a recent one)
+              const auto stamped_iter = stamped_index_.find(connected_variable_uuid);
+              if (stamped_iter != stamped_index_.end() && stamped_iter->second < stamp)
+              {
+                continue;
+              }
               connected_variable_uuids.insert(connected_variable_uuid);
             }
           }

vesta_optimizers/src/fixed_lag_smoother.cpp

@@ -164,9 +164,6 @@ ceres::Solver::Summary FixedLagSmoother::optimize()
   marginal_transaction_ = vesta_constraints::marginalizeVariables(
       "FixedLagSmoother", computeVariablesToMarginalize(lag_expiration_), *graph_);
 
-  // TODO: marginalize non stamped variables that are connected to the
-  // marginalized subgraph and not the window subgraph
-
   // Perform any post-marginal cleanup
   postprocessMarginalization(marginal_transaction_);
   // Note: The marginal transaction will not be applied until the next

vesta_optimizers/test/test_variable_stamp_index.cpp

@@ -310,12 +310,19 @@ TEST(VariableStampIndex, Query)
   transaction.addConstraint(c5);
   index.addNewTransaction(transaction);
 
-  auto expected1 = std::vector<vesta_core::UUID>{};
+  // x1 (stamp=1) is old at threshold 1.5, and its only cross-window link is
+  // to x2 via c1.  Old stamped variables are no longer shielded by
+  // cross-window constraints, so x1 is now returned for marginalization.
+  auto expected1 = std::vector<vesta_core::UUID>{ x1->uuid() };
+  std::sort(expected1.begin(), expected1.end());
   auto actual1 = std::vector<vesta_core::UUID>();
   index.query(vesta_core::Timestamp(1, 500000), std::back_inserter(actual1));
+  std::sort(actual1.begin(), actual1.end());
   EXPECT_EQ(expected1, actual1);
 
-  auto expected2 = std::vector<vesta_core::UUID>{ x1->uuid(), l1->uuid() };
+  // At threshold 2.5, x1 (stamp=1) and x2 (stamp=2) are both old.
+  // l1 is connected only to old variables.  All three are returned.
+  auto expected2 = std::vector<vesta_core::UUID>{ x1->uuid(), x2->uuid(), l1->uuid() };
   std::sort(expected2.begin(), expected2.end());
   auto actual2 = std::vector<vesta_core::UUID>();
   index.query(vesta_core::Timestamp(2, 500000), std::back_inserter(actual2));
@@ -367,8 +374,10 @@ TEST(VariableStampIndex, MarginalTransaction)
   // The x1 variable should be removed
   EXPECT_EQ(4u, index.size());
 
-  // And the marginal constraint x3->l1 should not affect future queries
-  auto expected = std::vector<vesta_core::UUID>{ l1->uuid() };
+  // At threshold 2.5 only x3 (stamp=3) is recent.  x2 (stamp=2) is old and
+  // no longer shielded by the cross-window constraint c2(x2,x3).  l1 is
+  // connected only to x2 (old).  Both x2 and l1 are returned.
+  auto expected = std::vector<vesta_core::UUID>{ x2->uuid(), l1->uuid() };
   std::sort(expected.begin(), expected.end());
   auto actual = std::vector<vesta_core::UUID>();
   index.query(vesta_core::Timestamp(2, 500000), std::back_inserter(actual));