Skip to content
Snippets Groups Projects
Commit 36f54747 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

period and verbose in SolverManager

parent db899945
No related branches found
No related tags found
1 merge request!384Resolve "SolverCeres stop solving if can update"
......@@ -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);
......
......@@ -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
......
......@@ -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);
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment