diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index 802ab7b49dabf93455f8f93fc8c30745415fe33e..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();
- void check();
const Eigen::SparseMatrixd computeHessian() const;
protected:
- std::string solveImpl(const ReportVerbosity report_level) override;
+ virtual bool checkDerived(std::string prefix="") const override;
- void addFactor(const FactorBasePtr& fac_ptr) override;
+ virtual std::string solveDerived(const ReportVerbosity report_level) override;
- void removeFactor(const FactorBasePtr& fac_ptr) override;
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) override;
- void addStateBlock(const StateBlockPtr& state_ptr) override;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
- void removeStateBlock(const StateBlockPtr& state_ptr) override;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockLocalParametrization(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 c2b2c161caa8c3ee8401e8d1b3426ac8f8f64db0..3a5e613276c432db263cf43a484b2550ec1cfac2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -93,7 +93,7 @@ public:
SolverManager(const ProblemPtr& wolf_problem);
- virtual ~SolverManager() = default;
+ virtual ~SolverManager();
std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
@@ -117,30 +117,40 @@ public:
virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
-protected:
-
- std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
-
- virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
- virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
-
- virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
-
- virtual void addFactor(const FactorBasePtr& fac_ptr) = 0;
+ bool check(std::string prefix="") const;
- virtual void removeFactor(const FactorBasePtr& fac_ptr) = 0;
-
- virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
-
- virtual void removeStateBlock(const StateBlockPtr& state_ptr) = 0;
+protected:
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) = 0;
+ std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
+ std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
+ std::set<FactorBasePtr> factors_;
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) = 0;
+ virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
+ virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
+private:
+ // SolverManager functions
+ void addFactor(const FactorBasePtr& fac_ptr);
+ void removeFactor(const FactorBasePtr& fac_ptr);
+ void addStateBlock(const StateBlockPtr& state_ptr);
+ void removeStateBlock(const StateBlockPtr& state_ptr);
+ void updateStateBlockState(const StateBlockPtr& state_ptr);
+ void updateStateBlockStatus(const StateBlockPtr& state_ptr);
+ void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr);
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+protected:
+ // Derived virtual functions
+ virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
+ 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;
};
} // namespace wolf
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 79c40b7604f4edbfb5c0cae8605493e63eada692..072a729a6d0cc8ec278f0b30e33141bbf7ca16db 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -63,11 +63,26 @@ CaptureBase::~CaptureBase()
void CaptureBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
+ // remove downstream
+ while (!constrained_by_list_.empty())
+ {
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
+ }
+ while (!feature_list_.empty())
+ {
+ feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
+ }
+
// Remove State Blocks
removeStateBlocks(getProblem());
@@ -79,16 +94,6 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
F->remove(viral_remove_empty_parent); // remove upstream
}
-
- // remove downstream
- while (!feature_list_.empty())
- {
- feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
- }
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
- }
}
}
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0d0c6537fedfc21dec63760d877fb56386885adf..b5bcfa4b7b6dc09e990adbecb93685c0294dbef5 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -37,7 +37,13 @@ CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
CeresManager::~CeresManager()
{
while (!fac_2_residual_idx_.empty())
- removeFactor(fac_2_residual_idx_.begin()->first);
+ 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)
@@ -48,13 +54,8 @@ CeresManager::~CeresManager()
return std::make_shared<CeresManager>(_wolf_problem, opt);
}
-std::string CeresManager::solveImpl(const ReportVerbosity report_level)
+std::string CeresManager::solveDerived(const ReportVerbosity report_level)
{
- // Check
- #ifdef _WOLF_DEBUG
- check();
- #endif
-
// run Ceres Solver
ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
@@ -259,7 +260,7 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
+void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr)
{
assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
@@ -287,7 +288,7 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
+void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr)
{
assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
@@ -298,12 +299,15 @@ void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::addStateBlock(const StateBlockPtr& state_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,18 +322,19 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
if (state_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
- updateStateBlockStatus(state_ptr);
+ updateStateBlockStatusDerived(state_ptr);
}
-void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
+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);
}
-void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
+void CeresManager::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
{
assert(state_ptr != nullptr);
if (state_ptr->isFixed())
@@ -338,7 +343,7 @@ void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
}
-void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr)
{
assert(state_ptr != nullptr);
@@ -361,17 +366,17 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
// Remove all involved factors (it does not remove any parameter block)
for (auto fac : involved_factors)
- removeFactor(fac);
+ removeFactorDerived(fac);
// Remove state block (it removes all involved residual blocks but they just were removed)
- removeStateBlock(state_ptr);
+ removeStateBlockDerived(state_ptr);
// Add state block
- addStateBlock(state_ptr);
+ addStateBlockDerived(state_ptr);
// Add all involved factors
for (auto fac : involved_factors)
- addFactor(fac);
+ addFactorDerived(fac);
}
bool CeresManager::hasConverged()
@@ -410,37 +415,88 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa
throw std::invalid_argument( "Wrong Jacobian Method!" );
}
-void CeresManager::check()
+bool CeresManager::checkDerived(std::string prefix) const
{
+ bool ok = true;
+
// Check numbers
- assert(ceres_problem_->NumResidualBlocks() == fac_2_costfunction_.size());
- assert(ceres_problem_->NumResidualBlocks() == fac_2_residual_idx_.size());
- assert(ceres_problem_->NumParameterBlocks() == state_blocks_.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, ":\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);
+ ok = false;
+ }
// Check parameter blocks
- for (auto&& state_block_pair : state_blocks_)
- assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
+ //for (auto&& state_block_pair : state_blocks_)
+ for (const auto& state_block_pair : state_blocks_)
+ {
+ if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
+ {
+ WOLF_ERROR("CeresManager::check: any state block is missing in ceres problem - in ", prefix);
+ ok = false;
+ }
+ }
// 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
- assert(fac_2_costfunction_.find(fac_res_pair.first) != fac_2_costfunction_.end());
- assert(fac_2_costfunction_[fac_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second));
+ 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;
+ }
+ //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ {
+ WOLF_ERROR("CeresManager::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
+ ok = false;
+ }
// factor - residual
- assert(fac_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor());
+ if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
+ {
+ WOLF_ERROR("CeresManager::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
+ ok = false;
+ }
// parameter blocks - state blocks
std::vector<double*> param_blocks;
ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
- auto i = 0;
- for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
+ if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size())
+ {
+ WOLF_ERROR("CeresManager::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
+ ok = false;
+ }
+ else
{
- assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
- i++;
+ auto i = 0;
+ for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
+ {
+ if (getAssociatedMemBlockPtr(st) != param_blocks[i])
+ {
+ WOLF_ERROR("CeresManager::check: parameter", i, " mismatch - in ", prefix);
+ ok = false;
+ }
+ i++;
+ }
}
}
+ return ok;
}
void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
@@ -483,8 +539,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/factor/factor_base.cpp b/src/factor/factor_base.cpp
index f85054ef85efb67e019d1720fb5c364b1774a4fc..0c9286337d850796006c208a67aba4c2775e137e 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -67,20 +67,28 @@ FactorBase::FactorBase(const std::string& _tp,
void FactorBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
+
+ // add factor to be removed from solver
+ if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
+ getProblem()->notifyFactor(this_fac,REMOVE);
+
+ // remove from upstream
FeatureBasePtr f = feature_ptr_.lock();
if (f)
{
- f->removeFactor(this_fac); // remove from upstream
+ f->removeFactor(this_fac);
if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
f->remove(viral_remove_empty_parent); // remove upstream
}
- // add factor to be removed from solver
- if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
- getProblem()->notifyFactor(this_fac,REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
for (auto& frm_ow : frame_other_list_)
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 0acccaf7a9fb9404e76ac9c5b142ed0839a9405c..86d56b3e46bf20390a7083d695b0151b910d17ee 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -39,20 +39,16 @@ FeatureBase::~FeatureBase()
void FeatureBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
- // remove from upstream
- CaptureBasePtr C = capture_ptr_.lock();
- if (C)
- {
- C->removeFeature(this_f); // remove from upstream
- if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
- C->remove(viral_remove_empty_parent); // remove upstream
- }
-
// remove downstream
while (!factor_list_.empty())
{
@@ -62,6 +58,15 @@ void FeatureBase::remove(bool viral_remove_empty_parent)
{
constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
+
+ // remove from upstream
+ CaptureBasePtr C = capture_ptr_.lock();
+ if (C)
+ {
+ C->removeFeature(this_f); // remove from upstream
+ if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
+ C->remove(viral_remove_empty_parent); // remove upstream
+ }
}
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index bca65f5a4bb2e7b11e0e48fd54013f265dd8db95..991ae7f97ca57d09e7b31785229ab5a3550bd3af 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -117,31 +117,36 @@ FrameBase::~FrameBase()
void FrameBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
- // remove from upstream
- TrajectoryBasePtr T = trajectory_ptr_.lock();
- if (T)
+ // remove downstream
+ while (!constrained_by_list_.empty())
{
- T->removeFrame(this_F); // remove from upstream
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
-
- // remove downstream
while (!capture_list_.empty())
{
capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
- }
// Remove Frame State Blocks
if ( isKeyOrAux() )
removeStateBlocks(getProblem());
+
+ // remove from upstream
+ TrajectoryBasePtr T = trajectory_ptr_.lock();
+ if (T)
+ {
+ T->removeFrame(this_F); // remove from upstream
+ }
}
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 9f2552615dc76f2c631b3007578da62bcfbea462..a89afb2fda060dcaf503501ea81f34c03b3f5324 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -37,16 +37,16 @@ LandmarkBase::~LandmarkBase()
void LandmarkBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
- // remove from upstream
- auto M = map_ptr_.lock();
- if (M)
- M->removeLandmark(this_L);
-
// remove constrained by
while (!constrained_by_list_.empty())
{
@@ -55,6 +55,11 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
// Remove State Blocks
removeStateBlocks(getProblem());
+
+ // remove from upstream
+ auto M = map_ptr_.lock();
+ if (M)
+ M->removeLandmark(this_L);
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 58112a40d249aaadaad330dbf9ae08fe05263e85..c5d21f25e12ac3ced6c77862b2c608f30636b51a 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 6532dfc6a2b95d43451e00f8fad4797e7c53edf9..24a674f5f517e7bbd5ac8448c8678a7517632b90 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -11,11 +11,20 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
}
+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();
@@ -34,39 +43,15 @@ void SolverManager::update()
// ADD/REMOVE STATE BLOCS
while ( !sb_notification_map.empty() )
{
- StateBlockPtr state_ptr = sb_notification_map.begin()->first;
-
+ // add
if (sb_notification_map.begin()->second == ADD)
- {
- // only add if not added
- if (state_blocks_.find(state_ptr) == state_blocks_.end())
- {
- state_blocks_.emplace(state_ptr, state_ptr->getState());
- addStateBlock(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
- state_ptr->resetStateUpdated();
- state_ptr->resetFixUpdated();
- state_ptr->resetLocalParamUpdated();
- }
- else
- {
- WOLF_DEBUG("Tried to add a StateBlock that was already added !");
- }
- }
+ addStateBlock(sb_notification_map.begin()->first);
+
+ // remove
else
- {
- // only remove if it exists
- if (state_blocks_.find(state_ptr)!=state_blocks_.end())
- {
- removeStateBlock(state_ptr);
- state_blocks_.erase(state_ptr);
- }
- else
- {
- WOLF_DEBUG("Tried to remove a StateBlock that was not added !");
- }
- }
- // next notification
+ removeStateBlock(sb_notification_map.begin()->first);
+
+ // remove notification
sb_notification_map.erase(sb_notification_map.begin());
}
@@ -77,6 +62,7 @@ void SolverManager::update()
// add factor
addFactor(fac_notification_map.begin()->first);
+
// remove notification
fac_notification_map.erase(fac_notification_map.begin());
}
@@ -88,28 +74,20 @@ void SolverManager::update()
// state update
if (state_ptr->stateUpdated())
- {
- Eigen::VectorXd new_state = state_ptr->getState();
- // We assume the same size for the states in both WOLF and the solver.
- std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
- // reset flag
- state_ptr->resetStateUpdated();
- }
+ updateStateBlockState(state_ptr);
+
// fix update
if (state_ptr->fixUpdated())
- {
updateStateBlockStatus(state_ptr);
- // reset flag
- state_ptr->resetFixUpdated();
- }
+
// local parameterization update
if (state_ptr->localParamUpdated())
- {
updateStateBlockLocalParametrization(state_ptr);
- // reset flag
- state_ptr->resetLocalParamUpdated();
- }
}
+
+ #ifdef _WOLF_DEBUG
+ assert(check("after update()"));
+ #endif
}
wolf::ProblemPtr SolverManager::getProblem()
@@ -122,24 +100,179 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update problem
update();
- std::string report = solveImpl(report_level);
+ std::string report = solveDerived(report_level);
// update StateBlocks with optimized state value.
/// @todo whatif someone has changed the state notification during opti ??
/// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
- std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
- it_end = state_blocks_.end();
- for (; it != it_end; ++it)
+ //std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
+ // it_end = state_blocks_.end();
+ for (auto& stateblock_statevector : state_blocks_)
{
// Avoid usuless copies
- if (!it->first->isFixed())
- it->first->setState(it->second, false); // false = do not raise the flag state_updated_
+ if (!stateblock_statevector.first->isFixed())
+ stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
}
return report;
}
+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("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
+ 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);
+}
+
+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;
+ }
+
+ // 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");
+ assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
+ state_blocks_2_factors_.at(st).remove(fac_ptr);
+ }
+}
+
+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 a StateBlock that was already added !");
+ return;
+ }
+
+ 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");
+
+ // 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
+ state_ptr->resetStateUpdated();
+ state_ptr->resetFixUpdated();
+ state_ptr->resetLocalParamUpdated();
+}
+
+void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
+{
+ // Warning if removing a missing state block
+ if (state_blocks_.count(state_ptr) == 0)
+ {
+ WOLF_WARN("Tried to remove a StateBlock that was not added !");
+ return;
+ }
+
+ 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("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);
+ return;
+ }
+
+ assert(state_blocks_2_factors_.at(state_ptr).empty() && "SolverManager::removeStateBlock removing state block before removing all factors involved");
+
+ // derived
+ removeStateBlockDerived(state_ptr);
+
+ // stateblock maps
+ state_blocks_.erase(state_ptr);
+ state_blocks_2_factors_.erase(state_ptr);
+}
+
+void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
+{
+ // derived
+ updateStateBlockStatusDerived(state_ptr);
+
+ // reset flag
+ state_ptr->resetFixUpdated();
+}
+
+void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
+{
+ Eigen::VectorXd new_state = state_ptr->getState();
+ // We assume the same size for the states in both WOLF and the solver.
+ std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
+ // reset flag
+ state_ptr->resetStateUpdated();
+}
+
+void SolverManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+{
+ // derived
+ updateStateBlockLocalParametrizationDerived(state_ptr);
+
+ // reset flag
+ state_ptr->resetLocalParamUpdated();
+}
+
Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
{
auto it = state_blocks_.find(state_ptr);
@@ -150,6 +283,16 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
return it->second;
}
+const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const
+{
+ auto it = state_blocks_.find(state_ptr);
+
+ if (it == state_blocks_.end())
+ throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+
+ return it->second.data();
+}
+
double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
{
auto it = state_blocks_.find(state_ptr);
@@ -162,7 +305,7 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
{
- return state_blocks_.find(state_ptr) != state_blocks_.end() && isStateBlockRegisteredDerived(state_ptr);
+ return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);
}
bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
@@ -170,4 +313,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
new file mode 100644
index 0000000000000000000000000000000000000000..0e6a1956b97728e8440edca7931c6d5ac32c3bc0
--- /dev/null
+++ b/test/dummy/solver_manager_dummy.h
@@ -0,0 +1,104 @@
+/*
+ * solver_manager_dummy.h
+ *
+ * Created on: May 27, 2020
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+#define TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+
+#include "core/solver/solver_manager.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(SolverManagerDummy);
+class SolverManagerDummy : public SolverManager
+{
+ public:
+ std::list<FactorBasePtr> factors_;
+ std::map<StateBlockPtr,bool> state_block_fixed_;
+ std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
+
+ SolverManagerDummy(const ProblemPtr& wolf_problem) :
+ SolverManager(wolf_problem)
+ {
+ };
+
+ bool isStateBlockRegistered(const StateBlockPtr& st)
+ {
+ return state_blocks_.find(st)!=state_blocks_.end();
+ };
+
+ bool isStateBlockFixed(const StateBlockPtr& st) const
+ {
+ return state_block_fixed_.at(st);
+ };
+
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
+ {
+ return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+ };
+
+ bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+ {
+ return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
+ };
+
+ bool hasLocalParametrization(const StateBlockPtr& st) const
+ {
+ return state_block_local_param_.find(st) != state_block_local_param_.end();
+ };
+
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
+
+ // The following are dummy implementations
+ bool hasConverged() { return true; }
+ SizeStd iterations() { return 1; }
+ double initialCost() { return double(1); }
+ double finalCost() { return double(0); }
+
+
+
+ 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)
+ {
+ factors_.push_back(fac_ptr);
+ };
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr)
+ {
+ factors_.remove(fac_ptr);
+ };
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr)
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr)
+ {
+ state_block_fixed_.erase(state_ptr);
+ state_block_local_param_.erase(state_ptr);
+ };
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ };
+ virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr)
+ {
+ if (state_ptr->getLocalParametrization() == nullptr)
+ state_block_local_param_.erase(state_ptr);
+ else
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+};
+
+}
+
+#endif /* TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_ */
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 7ead2b8f801602e664cae1e2ec4249f21ef916a5..0aa78e0ea31b11c742178ed491a016b421f70b93 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -88,7 +88,7 @@ TEST(CeresManager, Create)
ASSERT_EQ(P, ceres_manager_ptr->getProblem());
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddStateBlock)
@@ -111,7 +111,7 @@ TEST(CeresManager, AddStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleAddStateBlock)
@@ -140,7 +140,7 @@ TEST(CeresManager, DoubleAddStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, UpdateStateBlock)
@@ -171,7 +171,7 @@ TEST(CeresManager, UpdateStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddUpdateStateBlock)
@@ -198,7 +198,7 @@ TEST(CeresManager, AddUpdateStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveStateBlock)
@@ -230,7 +230,7 @@ TEST(CeresManager, RemoveStateBlock)
ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddRemoveStateBlock)
@@ -256,7 +256,7 @@ TEST(CeresManager, AddRemoveStateBlock)
ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveUpdateStateBlock)
@@ -281,7 +281,7 @@ TEST(CeresManager, RemoveUpdateStateBlock)
ceres_manager_ptr->update();
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleRemoveStateBlock)
@@ -309,7 +309,7 @@ TEST(CeresManager, DoubleRemoveStateBlock)
ceres_manager_ptr->update();
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddFactor)
@@ -331,7 +331,7 @@ TEST(CeresManager, AddFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleAddFactor)
@@ -356,7 +356,7 @@ TEST(CeresManager, DoubleAddFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveFactor)
@@ -384,7 +384,7 @@ TEST(CeresManager, RemoveFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddRemoveFactor)
@@ -411,7 +411,7 @@ TEST(CeresManager, AddRemoveFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleRemoveFactor)
@@ -437,16 +437,15 @@ 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));
ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddStateBlockLocalParam)
@@ -473,7 +472,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveLocalParam)
@@ -505,7 +504,7 @@ TEST(CeresManager, RemoveLocalParam)
ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddLocalParam)
@@ -538,7 +537,7 @@ TEST(CeresManager, AddLocalParam)
ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, FactorsRemoveLocalParam)
@@ -580,7 +579,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, FactorsUpdateLocalParam)
@@ -624,7 +623,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
int main(int argc, char **argv)
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index bf878e87c1cb5a277225ac20be11fce7f1b12c5d..e2d407caec84e3df3034de185632a6a46cf06fb0 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -15,6 +15,7 @@
#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
@@ -34,36 +35,6 @@ using namespace Eigen;
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-
-WOLF_PTR_TYPEDEFS(DummySolverManager);
-
-class DummySolverManager : public SolverManager
-{
-public:
- DummySolverManager(const ProblemPtr& _problem)
- : SolverManager(_problem)
- {
- //
- }
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool hasConverged(){return true;};
- virtual SizeStd iterations(){return 0;};
- virtual double initialCost(){return 0;};
- virtual double finalCost(){return 0;};
- virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr){};
- virtual void removeFactor(const FactorBasePtr& fac_ptr){};
- virtual void addStateBlock(const StateBlockPtr& state_ptr){};
- virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-};
-
TEST(Problem, create)
{
ProblemPtr P = Problem::create("POV", 3);
@@ -295,24 +266,26 @@ 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
- DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
+ 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();
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)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 640dc71faefcdfcaa937bdc928c904ef618f05d9..646f43d120a868d83102e50c3bc59f711502698a 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,104 +13,20 @@
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
-#include "core/solver/solver_manager.h"
#include "core/state_block/local_parametrization_base.h"
#include "core/state_block/local_parametrization_angle.h"
+#include "dummy/solver_manager_dummy.h"
#include <iostream>
+#include <thread>
using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
-class SolverManagerWrapper : public SolverManager
-{
- public:
- std::list<FactorBasePtr> factors_;
- std::map<StateBlockPtr,bool> state_block_fixed_;
- std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
-
- SolverManagerWrapper(const ProblemPtr& wolf_problem) :
- SolverManager(wolf_problem)
- {
- };
-
- bool isStateBlockRegistered(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st) const
- {
- return state_block_fixed_.at(st);
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
- {
- return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
- {
- return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
- };
-
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_block_local_param_.find(st) != state_block_local_param_.end();
- };
-
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-
- // The following are dummy implementations
- bool hasConverged() { return true; }
- SizeStd iterations() { return 1; }
- double initialCost() { return double(1); }
- double finalCost() { return double(0); }
-
-
-
- protected:
-
- virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr)
- {
- factors_.push_back(fac_ptr);
- };
- virtual void removeFactor(const FactorBasePtr& fac_ptr)
- {
- factors_.remove(fac_ptr);
- };
- virtual void addStateBlock(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
- virtual void removeStateBlock(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_.erase(state_ptr);
- state_block_local_param_.erase(state_ptr);
- };
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- };
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
- {
- if (state_ptr->getLocalParametrization() == nullptr)
- state_block_local_param_.erase(state_ptr);
- else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
-};
-
TEST(SolverManager, Create)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// check double pointers to branches
ASSERT_EQ(P, solver_manager_ptr->getProblem());
@@ -119,7 +35,7 @@ TEST(SolverManager, Create)
TEST(SolverManager, AddStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -138,7 +54,7 @@ TEST(SolverManager, AddStateBlock)
TEST(SolverManager, DoubleAddStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -163,7 +79,7 @@ TEST(SolverManager, DoubleAddStateBlock)
TEST(SolverManager, UpdateStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -211,7 +127,7 @@ TEST(SolverManager, UpdateStateBlock)
TEST(SolverManager, AddUpdateStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -249,7 +165,7 @@ TEST(SolverManager, AddUpdateStateBlock)
TEST(SolverManager, AddUpdateLocalParamStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -292,7 +208,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -339,7 +255,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
TEST(SolverManager, RemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -364,7 +280,7 @@ TEST(SolverManager, RemoveStateBlock)
TEST(SolverManager, AddRemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -386,7 +302,7 @@ TEST(SolverManager, AddRemoveStateBlock)
TEST(SolverManager, RemoveUpdateStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -405,7 +321,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
TEST(SolverManager, DoubleRemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -430,7 +346,7 @@ TEST(SolverManager, DoubleRemoveStateBlock)
TEST(SolverManager, AddUpdatedStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -508,7 +424,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
TEST(SolverManager, AddFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -518,7 +434,7 @@ TEST(SolverManager, AddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -535,7 +451,7 @@ TEST(SolverManager, AddFactor)
TEST(SolverManager, RemoveFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -545,7 +461,7 @@ TEST(SolverManager, RemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -568,7 +484,7 @@ TEST(SolverManager, RemoveFactor)
TEST(SolverManager, AddRemoveFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -579,7 +495,7 @@ TEST(SolverManager, AddRemoveFactor)
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -603,18 +519,18 @@ TEST(SolverManager, AddRemoveFactor)
TEST(SolverManager, DoubleRemoveFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) factor point 2d
+ // Create (and add) factor pose 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -635,9 +551,44 @@ TEST(SolverManager, DoubleRemoveFactor)
ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
}
+TEST(SolverManager, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_manager_ptr->update();
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+ if (P->getTrajectory()->getFrameList().size() > 10)
+ P->getTrajectory()->getFrameList().front()->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 82c65c284c5f06b518943679ac70ae3f2803bc42..8eeaf7c0bdc28846f0c4d09a7ed25dcd4cbe0a9c 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -12,37 +12,12 @@
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
#include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
#include <iostream>
using namespace wolf;
-struct DummySolverManager : public SolverManager
-{
- DummySolverManager(const ProblemPtr& _problem)
- : SolverManager(_problem)
- {
- //
- }
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool hasConverged(){return true;};
- virtual SizeStd iterations(){return 0;};
- virtual double initialCost(){return 0;};
- virtual double finalCost(){return 0;};
- virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr){};
- virtual void removeFactor(const FactorBasePtr& fac_ptr){};
- virtual void addStateBlock(const StateBlockPtr& state_ptr){};
- virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-};
-
/// Set to true if you want debug info
bool debug = false;
@@ -123,7 +98,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- DummySolverManager N(P);
+ SolverManagerDummy N(P);
// Trajectory status:
// KF1 KF2 F3 frames