diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h index 7875cb7c1d28d9775e05a4af7c1b99757fbf0baf..bf847c2270dd58af2ea02ba399909073ac125afc 100644 --- a/include/core/ceres_wrapper/iteration_update_callback.h +++ b/include/core/ceres_wrapper/iteration_update_callback.h @@ -28,8 +28,6 @@ class IterationUpdateCallback : public ceres::IterationCallback if (problem_->getStateBlockNotificationMapSize() != 0 or problem_->getFactorNotificationMapSize() != 0) { - if (verbose_) - std::cout << "Stopping solver to update the problem!\n"; WOLF_INFO_COND(verbose_, "Stopping solver to update the problem!"); return ceres::SOLVER_TERMINATE_SUCCESSFULLY; } diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index d3c6d17ace1b7cf5dd75497954ed0aeaa270c067..424e858f1a6af27e75135649155424dcafc260aa 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -22,16 +22,36 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); struct ParamsCeres : public ParamsSolver { - bool update_immediately; + bool update_immediately = false; ceres::Solver::Options solver_options; ceres::Problem::Options problem_options; ceres::Covariance::Options covariance_options; ParamsCeres() : - update_immediately(false), + ParamsSolver(), solver_options(), problem_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.loss_function_ownership = ceres::TAKE_OWNERSHIP; @@ -49,15 +69,6 @@ struct ParamsCeres : public ParamsSolver 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; }; diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index 1b6d5893f4e8b61f07702459ad95d4ce1d6df021..c1b02b709874bce11ea89d5d7e335fe01860d7c7 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -22,7 +22,7 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, : SolverManager(_wolf_problem, _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); ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 6aa6371d1a4d3c1072771ce4150841cfa85823c8..a0b6f028ce349227ad94a979c16e788c51a09915 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -109,8 +109,6 @@ std::string SolverManager::solve() std::string SolverManager::solve(const ReportVerbosity report_level) { - WOLF_INFO_COND(report_level != SolverManager::ReportVerbosity::QUIET, "SOLVING..."); - // update problem update();