diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index 2e61df70b18ec9251e1e3cedb14f386dbc2914cb..a72b87faa6f4f8fc30da5c3088a68e403d230706 100644 --- a/include/core/solver/solver_manager.h +++ b/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; diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 7092f97a3ba8c3fd945cc493bc23dda7784522de..e67a9f6d227b319a30b4d243cb7dd880a4d85e9e 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/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()){ diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 5489ea265c0f740a0ddc12abfc2eb3a26f6a5fba..00c9c44542ab01963d4b8b894edb24c1ac4a5f48 100644 --- a/src/solver/solver_manager.cpp +++ b/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(); } diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index a9824d43ff2709308ee44de0d23d4403460ef96e..0aa78e0ea31b11c742178ed491a016b421f70b93 100644 --- a/test/gtest_ceres_manager.cpp +++ b/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)); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index ceb1145ec6243c3467c0338490f48b3d76423dbe..2139149485838749a2114b41212f0b7c3af2c4eb 100644 --- a/test/gtest_problem.cpp +++ b/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)