Merge branch '361-min-num-of-iterations-in-iteration_update_callback' into 'devel'

Resolve "Min num of iterations in iteration_update_callback"

Closes #361

See merge request !392

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_;
 };
 
 }

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_;

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()