better management of floating state blocks

src/solver/solver_manager.cpp

@@ -56,24 +56,12 @@ void SolverManager::update()
 
         // remove
         else
-        {
-            if (floating_state_blocks_.count(sb_notification_map.begin()->first) == 1)
-                floating_state_blocks_.erase(sb_notification_map.begin()->first);
-            else
-                removeStateBlock(sb_notification_map.begin()->first);
-        }
+            removeStateBlock(sb_notification_map.begin()->first);
 
         // remove notification
         sb_notification_map.erase(sb_notification_map.begin());
     }
 
-    // ADD "floating" STATE BLOCKS (last update they weren't involved in any factor)
-    while (!floating_state_blocks_.empty())
-    {
-        addStateBlock(*floating_state_blocks_.begin());
-        floating_state_blocks_.erase(floating_state_blocks_.begin());
-    }
-
     // ADD FACTORS
     while (!fac_notification_map.empty())
     {
@@ -87,15 +75,16 @@ void SolverManager::update()
     }
 
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
+    std::set<StateBlockPtr> new_floating_state_blocks;
     for (auto state_pair : state_blocks_)
     {
         auto state_ptr = state_pair.first;
 
-        // Check for "floating" state blocks (estimated but not involved in any factor -> not observable problem)
+        // Check for "floating" state blocks (not involved in any factor -> not observable problem)
         if (state_blocks_2_factors_.at(state_ptr).empty())
         {
-            WOLF_INFO("SolverManager::update(): 'Floating' StateBlock ", state_ptr, " (not involved in any factor) Storing it apart.");
-            floating_state_blocks_.insert(state_ptr);
+            WOLF_INFO("SolverManager::update(): StateBlock ", state_ptr, " is 'Floating' (not involved in any factor). Storing it apart.");
+            new_floating_state_blocks.insert(state_ptr);
             continue;
         }
 
@@ -112,11 +101,15 @@ void SolverManager::update()
             updateStateBlockLocalParametrization(state_ptr);
     }
 
-    // REMOVE "floating" STATE BLOCKS (will be added next update() call)
-    for (auto state_ptr : floating_state_blocks_)
+    // REMOVE NEWLY DETECTED "floating" STATE BLOCKS (will be added next update() call)
+    for (auto state_ptr : new_floating_state_blocks)
     {
         removeStateBlock(state_ptr);
-        // reset flags meaning "solver will handle this change" (state, fix and local param will be set in addStateBlock)
+        floating_state_blocks_.insert(state_ptr); // This line must be AFTER removeStateBlock()!
+    }
+    // RESET FLAGS for floating state blocks: meaning "solver handled this change" (state, fix and local param will be set in addStateBlock)
+    for (auto state_ptr : floating_state_blocks_)
+    {
         state_ptr->resetStateUpdated();
         state_ptr->resetFixUpdated();
         state_ptr->resetLocalParamUpdated();
@@ -180,10 +173,19 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
     for (const auto& st : fac_ptr->getStateBlockPtrVector())
         if (state_blocks_.count(st) == 0)
         {
-            WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
-            // put back notification in problem (will be added next update() call) and do nothing
-            wolf_problem_->notifyFactor(fac_ptr, ADD);
-            return;
+            // Check if it was stored as a 'floating' state block
+            if (floating_state_blocks_.count(st) == 1)
+            {
+                floating_state_blocks_.erase(st); // This line must be BEFORE addStateBlock()!
+                addStateBlock(st);
+            }
+            else
+            {
+                WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+                // put back notification in problem (will be added next update() call) and do nothing
+                wolf_problem_->notifyFactor(fac_ptr, ADD);
+                return;
+            }
         }
 
     // factors
@@ -230,7 +232,13 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
     // Warning if adding an already added state block
     if (state_blocks_.count(state_ptr) != 0)
     {
-        WOLF_WARN("Tried to add athe StateBloc ", state_ptr, " that was already added !");
+        WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that was already added !");
+        return;
+    }
+    // Warning if adding a floating state block
+    if (floating_state_blocks_.count(state_ptr) != 0)
+    {
+        WOLF_WARN("Tried to add the StateBloc ", state_ptr, " that is stored as floating !");
         return;
     }
 
@@ -253,6 +261,12 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
 
 void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
 {
+    // check if stored as 'floating' state block
+    if (floating_state_blocks_.count(state_ptr) == 1)
+    {
+        floating_state_blocks_.erase(state_ptr);
+        return;
+    }
     // Warning if removing a missing state block
     if (state_blocks_.count(state_ptr) == 0)
     {