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

iteration callback working

parent 46190d13
No related branches found
No related tags found
1 merge request!384Resolve "SolverCeres stop solving if can update"
Pipeline #5727 passed
This commit is part of merge request !384. Comments created here will be created in the context of that merge request.
...@@ -28,8 +28,6 @@ class IterationUpdateCallback : public ceres::IterationCallback ...@@ -28,8 +28,6 @@ class IterationUpdateCallback : public ceres::IterationCallback
if (problem_->getStateBlockNotificationMapSize() != 0 or if (problem_->getStateBlockNotificationMapSize() != 0 or
problem_->getFactorNotificationMapSize() != 0) problem_->getFactorNotificationMapSize() != 0)
{ {
if (verbose_)
std::cout << "Stopping solver to update the problem!\n";
WOLF_INFO_COND(verbose_, "Stopping solver to update the problem!"); WOLF_INFO_COND(verbose_, "Stopping solver to update the problem!");
return ceres::SOLVER_TERMINATE_SUCCESSFULLY; return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
} }
......
...@@ -22,16 +22,36 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); ...@@ -22,16 +22,36 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver struct ParamsCeres : public ParamsSolver
{ {
bool update_immediately; bool update_immediately = false;
ceres::Solver::Options solver_options; ceres::Solver::Options solver_options;
ceres::Problem::Options problem_options; ceres::Problem::Options problem_options;
ceres::Covariance::Options covariance_options; ceres::Covariance::Options covariance_options;
ParamsCeres() : ParamsCeres() :
update_immediately(false), ParamsSolver(),
solver_options(), solver_options(),
problem_options(), problem_options(),
covariance_options() covariance_options()
{
loadHardcodedValues();
}
ParamsCeres(std::string _unique_name, const ParamsServer& _server) :
ParamsSolver(_unique_name, _server),
solver_options(),
problem_options(),
covariance_options()
{
loadHardcodedValues();
// stop solver whenever the problem is updated (via ceres::iterationCallback)
update_immediately = _server.getParam<bool>(prefix + "update_immediately");
// ceres solver options
solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
}
void loadHardcodedValues()
{ {
problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP; problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
...@@ -49,15 +69,6 @@ struct ParamsCeres : public ParamsSolver ...@@ -49,15 +69,6 @@ struct ParamsCeres : public ParamsSolver
covariance_options.apply_loss_function = false; covariance_options.apply_loss_function = false;
} }
ParamsCeres(std::string _unique_name, const ParamsServer& _server) :
ParamsCeres()
{
update_immediately = _server.getParam<bool>(prefix + "update_immediately");
// ceres solver options
solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
}
~ParamsCeres() override = default; ~ParamsCeres() override = default;
}; };
......
...@@ -22,7 +22,7 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, ...@@ -22,7 +22,7 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
: SolverManager(_wolf_problem, _params) : SolverManager(_wolf_problem, _params)
, params_ceres_(_params) , params_ceres_(_params)
{ {
WOLF_INFO("GIVEN PARAMS constructor"); WOLF_INFO("GIVEN PARAMS constructor verbosity: ", (int)params_ceres_->verbose);
covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options); covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
......
...@@ -109,8 +109,6 @@ std::string SolverManager::solve() ...@@ -109,8 +109,6 @@ std::string SolverManager::solve()
std::string SolverManager::solve(const ReportVerbosity report_level) std::string SolverManager::solve(const ReportVerbosity report_level)
{ {
WOLF_INFO_COND(report_level != SolverManager::ReportVerbosity::QUIET, "SOLVING...");
// update problem // update problem
update(); update();
......
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