handling truncated sets of notifications

include/core/solver/solver_manager.h

@@ -125,6 +125,7 @@ protected:
 
     std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
     std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
+    std::set<FactorBasePtr> factors_;
 
     virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
     const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;

src/ceres_wrapper/ceres_manager.cpp

@@ -36,6 +36,14 @@ CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
 
 CeresManager::~CeresManager()
 {
+    while (!fac_2_residual_idx_.empty())
+        removeFactorDerived(fac_2_residual_idx_.begin()->first);
+
+    while (!state_blocks_.empty())
+    {
+        removeStateBlockDerived(state_blocks_.begin()->first);
+        state_blocks_.erase(state_blocks_.begin());
+    }
 }
 
     SolverManagerPtr CeresManager::create(const ProblemPtr &_wolf_problem, const ParamsServer& _server)
@@ -293,10 +301,13 @@ void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr)
 
 void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr)
 {
+    assert(state_ptr);
+    assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added");
+    assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
+
     ceres::LocalParameterization* local_parametrization_ptr = nullptr;
 
-    if (state_ptr->hasLocalParametrization() &&
-        state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
+    if (state_ptr->hasLocalParametrization())
     {
         auto p = state_blocks_local_param_.emplace(state_ptr,
                                                    std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
@@ -318,6 +329,7 @@ void CeresManager::removeStateBlockDerived(const StateBlockPtr& state_ptr)
 {
     //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
     assert(state_ptr);
+    assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres");
     ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
     state_blocks_local_param_.erase(state_ptr);
 }
@@ -440,7 +452,7 @@ bool CeresManager::check(std::string prefix) const
     for (const auto& fac_res_pair : fac_2_residual_idx_)
     {
         // costfunction - residual
-        if (fac_2_costfunction_.find(fac_res_pair.first) == fac_2_costfunction_.end())
+        if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
         {
             WOLF_ERROR("CeresManager::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
             ok = false;
@@ -524,8 +536,8 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const
             StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
             if (!sb->isFixed())
             {
-                assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added");
-                assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols");
+                assert(state_blocks_local_param_.count(sb) != 0 && "factor involving a state block not added");
+                assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
 
                 // insert since A_block_row has just been created so it's empty for sure
                 if (sb->hasLocalParametrization()){

src/solver/solver_manager.cpp

@@ -13,13 +13,6 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
 
 SolverManager::~SolverManager()
 {
-    while (!state_blocks_.empty())
-    {
-        auto sb = state_blocks_.begin()->first;
-        while(!state_blocks_2_factors_.at(sb).empty())
-            removeFactor(state_blocks_2_factors_.at(sb).back());
-        removeStateBlock(sb);
-    }
 }
 
 void SolverManager::update()
@@ -28,37 +21,6 @@ void SolverManager::update()
     auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
     auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
 
-    // Check not-removed factors involved in removed state blocks
-    // This could happen in multi-threading if update() is called
-    // after removing a node with state blocks (e.g. a frame)
-    // and before the related factors are not removed (virally).
-    for (const auto& sb_notification : sb_notification_map)
-    {
-        // check only for removing state blocks (that actually are in registered)
-        if (sb_notification.second == REMOVE and state_blocks_2_factors_.count(sb_notification.first))
-        {
-            for (auto fac : state_blocks_2_factors_.at(sb_notification.first))
-            {
-                // All involved factors should have been notified to be removed
-                bool fac_remove_notification = false;
-                for (const auto& fac_notification : fac_notification_map)
-                {
-                    if (fac_notification.first == fac and fac_notification.second == REMOVE)
-                    {
-                        fac_remove_notification = true;
-                        break;
-                    }
-                }
-                // Notification of remove of this factor not found
-                if (!fac_remove_notification)
-                {
-                    WOLF_WARN("****\n****\nSolverManager::update(): The StateBlock ", sb_notification.first, " is notified to be removed but the involved factor ", fac->id(), " is not. Removing it.\n****\n****");
-                    fac_notification_map.emplace(fac, REMOVE);
-                }
-            }
-        }
-    }
-
     // REMOVE FACTORS
     for (auto fac_notification_it = fac_notification_map.begin();
          fac_notification_it != fac_notification_map.end();
@@ -152,16 +114,43 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
 
 void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
 {
+    // Warning if adding an already added
+    if (factors_.count(fac_ptr) != 0)
+    {
+        WOLF_WARN("Tried to add a factor that was already added !");
+        return;
+    }
+
+    /* HANDLING 'TRUNCATED' NOTIFICATION
+     * This happens in multi-threading if update() is called while emplacing/removing nodes
+     *
+     *      ADD factor without ADD state block constrained by the factor
+     *
+     * Notification is put back on problem notifications, it will be added next update() call.
+     */
+    for (const auto& st : fac_ptr->getStateBlockPtrVector())
+        if (state_blocks_.count(st) == 0)
+        {
+            WOLF_WARN("\n************\n************\nSolverManager::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;
+        }
+
     assert(check("before addFactor"));
 
-    // add to state-factor map
-    for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
+    // factors
+    factors_.insert(fac_ptr);
+
+    // state-factor map
+    for (const auto& st : fac_ptr->getStateBlockPtrVector())
     {
         assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
         assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
         state_blocks_2_factors_.at(st).push_back(fac_ptr);
     }
 
+    // derived
     addFactorDerived(fac_ptr);
 
     assert(check("after addFactor"));
@@ -169,10 +158,22 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
 
 void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
 {
+    // Warning if removing a missing factor
+    if (factors_.count(fac_ptr) == 0)
+    {
+        WOLF_WARN("Tried to remove a factor that is missing !");
+        return;
+    }
+
     assert(check("before removeFactor"));
 
+    // derived
     removeFactorDerived(fac_ptr);
 
+    // factors
+    factors_.erase(fac_ptr);
+
+    // state-factor map
     for (const auto& st : fac_ptr->getStateBlockPtrVector())
     {
         assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
@@ -197,9 +198,11 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
 
     assert(check("before addStateBlock"));
 
+    // stateblock maps
     state_blocks_.emplace(state_ptr, state_ptr->getState());
     state_blocks_2_factors_.emplace(state_ptr, FactorBasePtrList());
 
+    // derived
     addStateBlockDerived(state_ptr);
 
     // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
@@ -221,12 +224,30 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
 
     assert(state_blocks_.count(state_ptr) != 0 && "SolverManager::removeStateBlock missing state block");
     assert(state_blocks_2_factors_.count(state_ptr) != 0 && "SolverManager::removeStateBlock missing state block");
+
+    /* HANDLING 'TRUNCATED' NOTIFICATION
+     * This happens in multi-threading if update() is called while emplacing/removing nodes
+     *
+     *      REMOVE state block without REMOVE factor that constrains it
+     *
+     * Notification is put back on problem notifications, it will be removed next update() call.
+     */
+    if (!state_blocks_2_factors_.at(state_ptr).empty())
+    {
+        WOLF_WARN("\n************\n************\nSolverManager::addFactor(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later.");
+        // put back notification in problem (will be removed next update() call) and do nothing
+        wolf_problem_->notifyStateBlock(state_ptr, REMOVE);
+        return;
+    }
+
     assert(state_blocks_2_factors_.at(state_ptr).empty() && "SolverManager::removeStateBlock removing state block before removing all factors involved");
 
     assert(check("before removeStateBlock"));
 
+    // derived
     removeStateBlockDerived(state_ptr);
 
+    // stateblock maps
     state_blocks_.erase(state_ptr);
     state_blocks_2_factors_.erase(state_ptr);
 
@@ -235,7 +256,9 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
 
 void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
 {
+    // derived
     updateStateBlockStatusDerived(state_ptr);
+
     // reset flag
     state_ptr->resetFixUpdated();
 }
@@ -251,7 +274,9 @@ void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
 
 void SolverManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
 {
+    // derived
     updateStateBlockLocalParametrizationDerived(state_ptr);
+
     // reset flag
     state_ptr->resetLocalParamUpdated();
 }

test/gtest_ceres_manager.cpp

@@ -437,9 +437,8 @@ TEST(CeresManager, DoubleRemoveFactor)
     // remove factor
     P->notifyFactor(c,REMOVE);
 
-    ASSERT_DEATH({
     // update solver
-    ceres_manager_ptr->update();},"");
+    ceres_manager_ptr->update();
 
     // check factor
     ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));

test/gtest_problem.cpp

@@ -266,7 +266,7 @@ TEST(Problem, StateBlocks)
     Sm->unfixExtrinsics();
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
 
-    //    P->print(4,1,1,1);
+        P->print(4,1,1,1);
 
     // consume notifications
     SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
@@ -277,13 +277,15 @@ TEST(Problem, StateBlocks)
     // remove frame
     auto SB_P = KF->getP();
     auto SB_O = KF->getO();
+    WOLF_INFO("removing frame...");
     KF->remove();
+    WOLF_INFO("removed");
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2));
     ASSERT_TRUE(P->getStateBlockNotification(SB_P, notif));
     ASSERT_EQ(notif, REMOVE);
     ASSERT_TRUE(P->getStateBlockNotification(SB_O, notif));
     ASSERT_EQ(notif, REMOVE);
-
+    WOLF_INFO("test end");
 }
 
 TEST(Problem, Covariances)