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

implemented and ctest ok

parent f70ee812
No related branches found
No related tags found
1 merge request!392Resolve "Min num of iterations in iteration_update_callback"
Pipeline #5901 passed
This commit is part of merge request !392. Comments created here will be created in the context of that merge request.
...@@ -16,10 +16,11 @@ namespace wolf { ...@@ -16,10 +16,11 @@ namespace wolf {
class IterationUpdateCallback : public ceres::IterationCallback class IterationUpdateCallback : public ceres::IterationCallback
{ {
public: public:
explicit IterationUpdateCallback(ProblemPtr _problem, bool verbose = false) explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
: problem_(_problem) : problem_(_problem)
, verbose_(verbose) , min_num_iterations_(min_num_iterations)
, initial_cost_(0) , initial_cost_(0)
, verbose_(verbose)
{} {}
~IterationUpdateCallback() {} ~IterationUpdateCallback() {}
...@@ -29,8 +30,9 @@ class IterationUpdateCallback : public ceres::IterationCallback ...@@ -29,8 +30,9 @@ class IterationUpdateCallback : public ceres::IterationCallback
if (summary.iteration == 0) if (summary.iteration == 0)
initial_cost_ = summary.cost; initial_cost_ = summary.cost;
if (problem_->getStateBlockNotificationMapSize() != 0 or else if (summary.iteration >= min_num_iterations_ and
problem_->getFactorNotificationMapSize() != 0) (problem_->getStateBlockNotificationMapSize() != 0 or
problem_->getFactorNotificationMapSize() != 0))
{ {
if (summary.cost >= initial_cost_) if (summary.cost >= initial_cost_)
{ {
...@@ -48,8 +50,9 @@ class IterationUpdateCallback : public ceres::IterationCallback ...@@ -48,8 +50,9 @@ class IterationUpdateCallback : public ceres::IterationCallback
private: private:
ProblemPtr problem_; ProblemPtr problem_;
bool verbose_; int min_num_iterations_;
double initial_cost_; double initial_cost_;
bool verbose_;
}; };
} }
......
...@@ -23,6 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); ...@@ -23,6 +23,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver struct ParamsCeres : public ParamsSolver
{ {
bool update_immediately = false; bool update_immediately = false;
int min_num_iterations = 1;
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;
...@@ -39,10 +40,12 @@ struct ParamsCeres : public ParamsSolver ...@@ -39,10 +40,12 @@ struct ParamsCeres : public ParamsSolver
loadHardcodedValues(); loadHardcodedValues();
// stop solver whenever the problem is updated (via ceres::iterationCallback) // 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 // 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() void loadHardcodedValues()
...@@ -76,6 +79,7 @@ class SolverCeres : public SolverManager ...@@ -76,6 +79,7 @@ class SolverCeres : public SolverManager
std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_; std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_; 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_; std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
ceres::Solver::Summary summary_; ceres::Solver::Summary summary_;
......
...@@ -25,7 +25,9 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, ...@@ -25,7 +25,9 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
if (params_ceres_->update_immediately) 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() SolverCeres::~SolverCeres()
......
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