diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index e5c0ab3b057cfbd48c4bc280b27d142723b49f70..cb160c87dc4e7d4dca9136656737ac4fce36419f 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -84,7 +84,7 @@ class SolverCeres : public SolverManager
SolverCeres(const ProblemPtr& _wolf_problem);
SolverCeres(const ProblemPtr& _wolf_problem,
- const ParamsCeresPtr& _params);
+ const ParamsCeresPtr& _params);
WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 5c0faab09b6bf1729c1d47d7dfd53dea530ae795..1fc5e60943ef0d37a72f5e8d2283afb8bbfcc9f9 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -38,115 +38,145 @@ static SolverManagerPtr create(const ProblemPtr& _problem,
return std::make_shared<SolverClass>(_problem, params); \
} \
-struct ParamsSolver: public ParamsBase
-{
- std::string prefix = "solver/";
-
- ParamsSolver() = default;
- ParamsSolver(std::string _unique_name, const ParamsServer& _server):
- ParamsBase(_unique_name, _server)
- {
- //
- }
-
- ~ParamsSolver() override = default;
-};
+ struct ParamsSolver;
/**
* \brief Solver manager for WOLF
*/
class SolverManager
{
-public:
-
- /** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
- enum class CovarianceBlocksToBeComputed : std::size_t
- {
- ALL, ///< All blocks and all cross-covariances
- ALL_MARGINALS, ///< All marginals
- ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
- };
-
- /**
- * \brief Enumeration for the verbosity of the solver report.
- */
- enum class ReportVerbosity : std::size_t
- {
- QUIET = 0,
- BRIEF,
- FULL
- };
-
-protected:
-
- ProblemPtr wolf_problem_;
-
-public:
-
- SolverManager(const ProblemPtr& wolf_problem);
-
- virtual ~SolverManager();
-
- std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
-
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
-
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
-
- virtual bool hasConverged() = 0;
-
- virtual SizeStd iterations() = 0;
-
- virtual double initialCost() = 0;
-
- virtual double finalCost() = 0;
-
- virtual void update();
-
- ProblemPtr getProblem();
-
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
-
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
-
- bool check(std::string prefix="") const;
-
-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;
- virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
-
-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);
+ public:
+
+ /** \brief Enumeration of covariance blocks to be computed
+ *
+ * Enumeration of covariance blocks to be computed
+ *
+ */
+ enum class CovarianceBlocksToBeComputed : std::size_t
+ {
+ ALL, ///< All blocks and all cross-covariances
+ ALL_MARGINALS, ///< All marginals
+ ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+ };
+
+ /**
+ * \brief Enumeration for the verbosity of the solver report.
+ */
+ enum class ReportVerbosity : std::size_t
+ {
+ QUIET = 0,
+ BRIEF,
+ FULL
+ };
+
+ protected:
+
+ ProblemPtr wolf_problem_;
+ ParamsSolverPtr params_;
+ TimeStamp last_solve_ts_;
+
+ public:
+ /**
+ * \brief Constructor with default params_
+ */
+ SolverManager(const ProblemPtr& _problem);
+ /**
+ * \brief Constructor with given params_
+ */
+ SolverManager(const ProblemPtr& _problem,
+ const ParamsSolverPtr& _params);
+
+ virtual ~SolverManager();
+
+ /**
+ * \brief Solves with the verbosity defined in params_
+ */
+ std::string solve();
+ /**
+ * \brief Solves with a given verbosity
+ */
+ std::string solve(const ReportVerbosity report_level);
+
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
+
+ virtual bool hasConverged() = 0;
+
+ virtual SizeStd iterations() = 0;
+
+ virtual double initialCost() = 0;
+
+ virtual double finalCost() = 0;
+
+ /**
+ * \brief Updates solver's problem according to the wolf_problem
+ */
+ virtual void update();
+
+ ProblemPtr getProblem();
+
+ /**
+ * \brief Returns if solve() should be called (according to period, can be derived to implement other criteria)
+ */
+ virtual bool ready() const;
+
+ virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
+
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
+
+ bool check(std::string prefix="") const;
+
+ 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;
+ virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+
+ 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);
+
+ 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;
+};
-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;
+// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
+struct ParamsSolver: public ParamsBase
+{
+ std::string prefix = "solver/";
+ double period = 0.0;
+ SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
+
+ ParamsSolver() = default;
+ ParamsSolver(std::string _unique_name, const ParamsServer& _server):
+ ParamsBase(_unique_name, _server)
+ {
+ period = _server.getParam<double>(prefix + "period");
+ verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
+ }
+
+ ~ParamsSolver() override = default;
};
} // namespace wolf
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index e499173a8d9c2f2551393beea506b3d47d099273..6e7c8ac56a03d1738c2d1caaa6debad2448f62de 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -15,8 +15,8 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) :
}
SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
- const ParamsCeresPtr& _params) :
- SolverManager(_wolf_problem),
+ const ParamsCeresPtr& _params) :
+ SolverManager(_wolf_problem, _params),
params_ceres_(_params)
{
covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 24a674f5f517e7bbd5ac8448c8678a7517632b90..4d32ac6de815acb26ba5e7759facbf2e5d3f9ea2 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -5,10 +5,17 @@
namespace wolf {
-SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
- wolf_problem_(_wolf_problem)
+SolverManager::SolverManager(const ProblemPtr& _problem) :
+ SolverManager(_problem, std::make_shared<ParamsSolver>())
{
- assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
+}
+
+SolverManager::SolverManager(const ProblemPtr& _problem,
+ const ParamsSolverPtr& _params) :
+ wolf_problem_(_problem),
+ params_(_params)
+{
+ assert(_problem != nullptr && "Passed a nullptr ProblemPtr.");
}
SolverManager::~SolverManager()
@@ -95,11 +102,19 @@ wolf::ProblemPtr SolverManager::getProblem()
return wolf_problem_;
}
+std::string SolverManager::solve()
+{
+ return solve(params_->verbose);
+}
+
std::string SolverManager::solve(const ReportVerbosity report_level)
{
// update problem
update();
+ last_solve_ts_ = TimeStamp::Now();
+
+ // call derived solver
std::string report = solveDerived(report_level);
// update StateBlocks with optimized state value.
@@ -303,6 +318,11 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
return it->second.data();
}
+bool SolverManager::ready() const
+{
+ return (!last_solve_ts_.ok() || (TimeStamp::Now() - last_solve_ts_) > params_->period);
+}
+
bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
{
return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);