Fix marginalization bug

Author: jakemclaughlin6

fuse_optimizers/CMakeLists.txt

@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.0.2)
 project(fuse_optimizers)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
 
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
+
 set(build_depends
   diagnostic_updater
   fuse_constraints

fuse_optimizers/src/windowed_optimizer.cpp

@@ -173,6 +173,40 @@ void WindowedOptimizer::optimizationLoop()
       preprocessMarginalization(*new_transaction);
       // Combine the new transactions with any marginal transaction from the end of the last cycle
       new_transaction->merge(marginal_transaction_);
+
+      // check for hanging constraints (constraints added to variables that are about to be marginalized)
+      auto new_marginal_transaction = fuse_core::Transaction::make_shared();
+      std::vector<fuse_core::UUID> variables_to_remarginalize;
+      for (auto &uuid : marginal_transaction_->removedVariables()) {
+        // get constraints that are to be removed in marginal transaction
+        auto to_remove_constraints = marginal_transaction_->getRemovedConstraints();
+        // get connected constraint to this variable
+        auto all_constraints = graph_->getConnectedConstraints(uuid);
+        // check if there is a constraint on this variable that isnt in the
+        // marginal transaction
+        for (auto &constraint : all_constraints) {
+          fuse_core::UUID constraint_uuid = constraint->uuid();
+          bool is_contained = false;
+          for (auto &to_remove : to_remove_constraints) {
+            if (constraint_uuid == to_remove) {
+              is_contained = true;
+              break;
+            }
+          }
+          if (!is_contained) {
+            variables_to_remarginalize.push_back(uuid);
+          }
+        }
+      }
+
+      // if some variables have hanging constraints then remarginalize them
+      if (variables_to_remarginalize.size() > 0) {
+        new_marginal_transaction = fuse_constraints::marginalizeVariables(
+            ros::this_node::getName(), variables_to_remarginalize, *graph_);
+        postprocessMarginalization(*new_marginal_transaction);
+        new_transaction->merge(new_marginal_transaction);
+      }
+
       // Update the graph
       try
       {