diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index 196ce659d5b7800b94bd395c4c21565963028a96..0841f7421ef37f039b2fa77449ffafefd52b7118 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -62,10 +62,7 @@ class CeresManager : public SolverManager
ceres::Solver::Summary getSummary();
- std::unique_ptr<ceres::Problem>& getCeresProblem()
- {
- return ceres_problem_;
- }
+ std::unique_ptr<ceres::Problem>& getCeresProblem();
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
@@ -82,25 +79,26 @@ class CeresManager : public SolverManager
ceres::Solver::Options& getSolverOptions();
- virtual bool check(std::string prefix="") const override;
const Eigen::SparseMatrixd computeHessian() const;
protected:
- std::string solveDerived(const ReportVerbosity report_level) override;
+ virtual bool checkDerived(std::string prefix="") const override;
- void addFactorDerived(const FactorBasePtr& fac_ptr) override;
+ virtual std::string solveDerived(const ReportVerbosity report_level) override;
- void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) override;
- void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
- void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
+
+ virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
@@ -114,6 +112,11 @@ inline ceres::Solver::Summary CeresManager::getSummary()
return summary_;
}
+inline std::unique_ptr<ceres::Problem>& CeresManager::getCeresProblem()
+{
+ return ceres_problem_;
+}
+
inline ceres::Solver::Options& CeresManager::getSolverOptions()
{
return ceres_options_;
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 36e5e338bde88d49062931e383929baa005bc9df..ce9bebfc929f49dc0265663c397bc07bcdade414 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -62,8 +62,7 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
- mutable std::mutex mut_factor_notifications_;
- mutable std::mutex mut_state_block_notifications_;
+ mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
std::string frame_structure_;
PriorOptionsPtr prior_options_;
@@ -346,6 +345,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
protected:
+ /** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
+ */
+ void consumeNotifications(std::map<StateBlockPtr,Notification>&,
+ std::map<FactorBasePtr,Notification>&);
+
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
@@ -407,28 +411,25 @@ inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
return processor_is_motion_list_;
}
-inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
-{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- return std::move(state_block_notification_map_);
-}
-
inline SizeStd Problem::getStateBlockNotificationMapSize() const
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
return state_block_notification_map_.size();
}
-inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
+inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
- return std::move(factor_notification_map_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+ return factor_notification_map_.size();
}
-inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
+inline void Problem::consumeNotifications(std::map<StateBlockPtr,Notification>& _sb_notification_map,
+ std::map<FactorBasePtr,Notification>& _fac_notification_map)
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
- return factor_notification_map_.size();
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+
+ _sb_notification_map = std::move(state_block_notification_map_);
+ _fac_notification_map = std::move(factor_notification_map_);
}
} // namespace wolf
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index a72b87faa6f4f8fc30da5c3088a68e403d230706..3a5e613276c432db263cf43a484b2550ec1cfac2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -117,9 +117,7 @@ public:
virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
- virtual bool check(std::string prefix="") const = 0;
-
- void assertCheck() const;
+ bool check(std::string prefix="") const;
protected:
@@ -152,15 +150,9 @@ protected:
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+ virtual bool checkDerived(std::string prefix="") const = 0;
};
-inline void SolverManager::assertCheck() const
-{
- #ifdef _WOLF_DEBUG
- assert(check());
- #endif
-}
-
} // namespace wolf
#endif /* _WOLF_SOLVER_MANAGER_H_ */
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index db50a89faf643479c251aa6cd8226c06d6498400..4ba2bffc51e7715e74bd642b49966c02fd1478e2 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -70,7 +70,6 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
*/
if (!is_removing_)
{
- WOLF_INFO("CaptureBase::remove");
is_removing_ = true;
CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index e67a9f6d227b319a30b4d243cb7dd880a4d85e9e..b5bcfa4b7b6dc09e990adbecb93685c0294dbef5 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -415,24 +415,21 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa
throw std::invalid_argument( "Wrong Jacobian Method!" );
}
-bool CeresManager::check(std::string prefix) const
+bool CeresManager::checkDerived(std::string prefix) const
{
bool ok = true;
// Check numbers
- if (ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size())
+ if (ceres_problem_->NumResidualBlocks() != factors_.size() or
+ ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or
+ ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
{
- WOLF_ERROR("CeresManager::check: number of residuals mismatch - in ", prefix);
- ok = false;
- }
- if (ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
- {
- WOLF_ERROR("CeresManager::check: number of residuals mismatch - in ", prefix);
+ WOLF_ERROR("CeresManager::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
ok = false;
}
if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
{
- WOLF_ERROR("CeresManager::check: number of parameters mismatch. ceres_problem_->NumParameterBlocks() = ", ceres_problem_->NumParameterBlocks(), " - state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+ WOLF_ERROR("CeresManager::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
ok = false;
}
@@ -448,9 +445,15 @@ bool CeresManager::check(std::string prefix) const
}
// Check residual blocks
- //for (auto&& fac_res_pair : fac_2_residual_idx_)
for (const auto& fac_res_pair : fac_2_residual_idx_)
{
+ // SolverManager::factors_
+ if (factors_.count(fac_res_pair.first) == 0)
+ {
+ WOLF_ERROR("CeresManager::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
+ ok = false;
+ }
+
// costfunction - residual
if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
{
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index bc54b2f1a719932c95df6d539f14c8d8a29757f1..0c02f696c36f2c4bf3fe590fd36a3658fddbd383 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -74,7 +74,6 @@ void FactorBase::remove(bool viral_remove_empty_parent)
*/
if (!is_removing_)
{
- WOLF_INFO("FactorBase::remove");
is_removing_ = true;
FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 8f91944feaa9ca7b765936ba972be493133a30d7..bbfae96fd53f878838678e96b670d0943e493f63 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -46,7 +46,6 @@ void FeatureBase::remove(bool viral_remove_empty_parent)
*/
if (!is_removing_)
{
- WOLF_INFO("FeatureBase::remove");
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 47fb6fae2beaacc0d73e5053946f643e2b975731..d533513a086df309647c446acbc766bf17301837 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -124,7 +124,6 @@ void FrameBase::remove(bool viral_remove_empty_parent)
*/
if (!is_removing_)
{
- WOLF_INFO("FrameBase::remove");
is_removing_ = true;
FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f2d41a420415f8b0f9fb3ac9f6b5e286036c1160..e63bdb036f8154918a18ac2be56d2504cc9dd308 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -595,7 +595,7 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
// Check if there is already a notification for this state block
@@ -622,7 +622,7 @@ StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _
bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
return false;
@@ -632,7 +632,7 @@ bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notificatio
FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _noti)
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
//std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
// Check if there is already the same notification for this factor
@@ -647,7 +647,9 @@ FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _not
}
// opposite notification -> cancell out eachother
else
+ {
factor_notification_map_.erase(notification_it);
+ }
}
// Add notification
@@ -659,7 +661,7 @@ FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _not
bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
return false;
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 1c3bcc60aa2354318747c3c6c75d7a0fc8a8b631..d39b12fdf51afed09ec2e19c097e55bd7e03e49d 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -18,8 +18,13 @@ SolverManager::~SolverManager()
void SolverManager::update()
{
// Consume notification maps
- auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
- auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
+ std::map<StateBlockPtr,Notification> sb_notification_map;
+ std::map<FactorBasePtr,Notification> fac_notification_map;
+ wolf_problem_->consumeNotifications(sb_notification_map, fac_notification_map);
+
+ #ifdef _WOLF_DEBUG
+ assert(check("before update()"));
+ #endif
// REMOVE FACTORS
for (auto fac_notification_it = fac_notification_map.begin();
@@ -79,6 +84,10 @@ void SolverManager::update()
if (state_ptr->localParamUpdated())
updateStateBlockLocalParametrization(state_ptr);
}
+
+ #ifdef _WOLF_DEBUG
+ assert(check("after update()"));
+ #endif
}
wolf::ProblemPtr SolverManager::getProblem()
@@ -91,9 +100,6 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update problem
update();
- // Check
- assertCheck();
-
std::string report = solveDerived(report_level);
// update StateBlocks with optimized state value.
@@ -134,11 +140,10 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
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);
+ assert(false);
return;
}
- assert(check("before addFactor"));
-
// factors
factors_.insert(fac_ptr);
@@ -152,8 +157,6 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
// derived
addFactorDerived(fac_ptr);
-
- assert(check("after addFactor"));
}
void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
@@ -165,8 +168,6 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
return;
}
- assert(check("before removeFactor"));
-
// derived
removeFactorDerived(fac_ptr);
@@ -180,8 +181,6 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
state_blocks_2_factors_.at(st).remove(fac_ptr);
}
-
- assert(check("after removeFactor"));
}
void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
@@ -196,8 +195,6 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
- assert(check("before addStateBlock"));
-
// stateblock maps
state_blocks_.emplace(state_ptr, state_ptr->getState());
state_blocks_2_factors_.emplace(state_ptr, FactorBasePtrList());
@@ -209,8 +206,6 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
state_ptr->resetStateUpdated();
state_ptr->resetFixUpdated();
state_ptr->resetLocalParamUpdated();
-
- assert(check("after addStateBlock"));
}
void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
@@ -236,22 +231,21 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
{
WOLF_WARN("SolverManager::removeStateBlock(): 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
+ for (auto fac : state_blocks_2_factors_.at(state_ptr))
+ WOLF_INFO(fac->id());
wolf_problem_->notifyStateBlock(state_ptr, REMOVE);
+ assert(false);
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);
-
- assert(check("after removeStateBlock"));
}
void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
@@ -321,4 +315,59 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
return isFactorRegisteredDerived(fac_ptr);
}
+
+bool SolverManager::check(std::string prefix) const
+{
+ bool ok = true;
+
+ // state blocks
+ if (state_blocks_.size() != state_blocks_2_factors_.size())
+ {
+ WOLF_ERROR("SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ", prefix);
+ ok = false;
+ }
+ auto sb_vec_it = state_blocks_.begin();
+ auto sb_fac_it = state_blocks_2_factors_.begin();
+ while (sb_vec_it != state_blocks_.end())
+ {
+ // same state block in both maps
+ if (sb_vec_it->first != sb_fac_it->first)
+ {
+ WOLF_ERROR("SolverManager::check: mismatching state blocks ", sb_vec_it->first, " and ", sb_fac_it->first, " - in ", prefix);
+ ok = false;
+ }
+
+ // factor involving state block in factors_
+ for (auto fac : sb_fac_it->second)
+ {
+ if (factors_.count(fac) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+ ok = false;
+ }
+ }
+ sb_vec_it++;
+ sb_fac_it++;
+ }
+
+ // factors
+ for (auto fac : factors_)
+ {
+ // involved sb stored
+ for (auto sb : fac->getStateBlockPtrVector())
+ {
+ if (state_blocks_.count(sb) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb, " inolved in factor ", fac->id(), " missing in state_blocks_ map - in ", prefix);
+ ok = false;
+ }
+ }
+ }
+
+ // checkDerived
+ ok &= checkDerived(prefix);
+
+ return ok;
+}
+
} // namespace wolf
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index c31a2ff0179d95fb7c068287b95a4a2f640faafa..0e6a1956b97728e8440edca7931c6d5ac32c3bc0 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -61,12 +61,12 @@ class SolverManagerDummy : public SolverManager
SizeStd iterations() { return 1; }
double initialCost() { return double(1); }
double finalCost() { return double(0); }
- virtual bool check(std::string prefix="") const override {return true;}
protected:
+ virtual bool checkDerived(std::string prefix="") const override {return true;}
virtual std::string solveDerived(const ReportVerbosity report_level){ return std::string("");};
virtual void addFactorDerived(const FactorBasePtr& fac_ptr)
{
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 2139149485838749a2114b41212f0b7c3af2c4eb..e2d407caec84e3df3034de185632a6a46cf06fb0 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -271,8 +271,8 @@ TEST(Problem, StateBlocks)
// consume notifications
SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
- SM->update(); // calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map
+ SM->update(); // calls P->consumeNotifications();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consumeNotifications empties the notification map
// remove frame
auto SB_P = KF->getP();