diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h index 27e5d758a84821dcaada80e9062c917c7af50020..1717be546ab820d24e13985227dd19753530440e 100644 --- a/include/core/ceres_wrapper/iteration_update_callback.h +++ b/include/core/ceres_wrapper/iteration_update_callback.h @@ -16,10 +16,11 @@ namespace wolf { class IterationUpdateCallback : public ceres::IterationCallback { public: - explicit IterationUpdateCallback(ProblemPtr _problem, bool verbose = false) + explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false) : problem_(_problem) - , verbose_(verbose) + , min_num_iterations_(min_num_iterations) , initial_cost_(0) + , verbose_(verbose) {} ~IterationUpdateCallback() {} @@ -29,8 +30,9 @@ class IterationUpdateCallback : public ceres::IterationCallback if (summary.iteration == 0) initial_cost_ = summary.cost; - if (problem_->getStateBlockNotificationMapSize() != 0 or - problem_->getFactorNotificationMapSize() != 0) + else if (summary.iteration >= min_num_iterations_ and + (problem_->getStateBlockNotificationMapSize() != 0 or + problem_->getFactorNotificationMapSize() != 0)) { if (summary.cost >= initial_cost_) { @@ -48,8 +50,9 @@ class IterationUpdateCallback : public ceres::IterationCallback private: ProblemPtr problem_; - bool verbose_; + int min_num_iterations_; double initial_cost_; + bool verbose_; }; } diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index c899b7a054d6e1a9ebab5f45cca35fa0bd86c3d0..a32f91a7f55247aaf102ac4f861bcf4b80456d41 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -23,6 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); struct ParamsCeres : public ParamsSolver { bool update_immediately = false; + int min_num_iterations = 1; ceres::Solver::Options solver_options; ceres::Problem::Options problem_options; ceres::Covariance::Options covariance_options; @@ -39,10 +40,12 @@ struct ParamsCeres : public ParamsSolver loadHardcodedValues(); // stop solver whenever the problem is updated (via ceres::iterationCallback) - update_immediately = _server.getParam<bool>(prefix + "update_immediately"); + update_immediately = _server.getParam<bool>(prefix + "update_immediately"); + if (update_immediately) + min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations"); // ceres solver options - solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations"); + solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations"); } void loadHardcodedValues() @@ -76,6 +79,7 @@ class SolverCeres : public SolverManager std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_; std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_; + // this map is only for handling automatic destruction of localParametrizationWrapper objects std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_; ceres::Solver::Summary summary_; diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp index 1d9cc7dd2e8f43c76820974caa473ad876b0a97c..0f058b107cb9355c93bb2852d73aac862faddcb5 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -25,7 +25,9 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); if (params_ceres_->update_immediately) - getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_, params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET)); + getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_, + params_ceres_->min_num_iterations, + params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET)); } SolverCeres::~SolverCeres()