removed solver->ready() and better iterationCallback machinery

include/core/solver/solver_manager.h

@@ -73,7 +73,6 @@ class SolverManager
 
         ProblemPtr wolf_problem_;
         ParamsSolverPtr params_;
-        TimeStamp last_solve_ts_;
 
     public:
         /**
@@ -116,10 +115,7 @@ class SolverManager
 
         ProblemPtr getProblem();
 
-        /**
-         * \brief Returns if solve() should be called (according to period, can be derived to implement other criteria)
-         */
-        virtual bool ready() const;
+        double getPeriod() const;
 
         ReportVerbosity getVerbosity() const;
 

src/ceres_wrapper/solver_ceres.cpp

@@ -51,6 +51,24 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
     // run Ceres Solver
     ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
 
+    // if termination by user (Iteration callback, update and solve again)
+    // solve until max iterations reached
+    if (params_ceres_->update_immediately)
+    {
+        auto max_num_iterations = getSolverOptions().max_num_iterations;
+        while (summary_.termination_type == ceres::USER_SUCCESS)
+        {
+            // decrease wasted iterations
+            getSolverOptions().max_num_iterations -= summary_.iterations.size();
+            if (getSolverOptions().max_num_iterations <= 0)
+                break;
+
+            // update and solve again
+            update();
+            ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
+        }
+        getSolverOptions().max_num_iterations = max_num_iterations;
+    }
     std::string report;
 
     //return report

src/solver/solver_manager.cpp

@@ -112,8 +112,6 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
     // update problem
     update();
 
-    last_solve_ts_ = TimeStamp::Now();
-
     // call derived solver
     std::string report = solveDerived(report_level);
 
@@ -318,11 +316,6 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
     return it->second.data();
 }
 
-bool SolverManager::ready() const
-{
-    return (!last_solve_ts_.ok() || (TimeStamp::Now() - last_solve_ts_) > params_->period);
-}
-
 SolverManager::ReportVerbosity SolverManager::getVerbosity() const
 {
     return params_->verbose;
@@ -338,6 +331,10 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
     return isFactorRegisteredDerived(fac_ptr);
 }
 
+double SolverManager::getPeriod() const
+{
+    return params_->period;
+}
 
 bool SolverManager::check(std::string prefix) const
 {