Merge branch '355-iterationupdatecallback-checking-total-cost-decreased' into 'devel'

Resolve "IterationUpdateCallback checking total cost decreased"

Closes #355

See merge request !386

include/core/ceres_wrapper/iteration_update_callback.h

@@ -19,17 +19,29 @@ class IterationUpdateCallback : public ceres::IterationCallback
         explicit IterationUpdateCallback(ProblemPtr _problem, bool verbose = false)
         : problem_(_problem)
         , verbose_(verbose)
+        , initial_cost_(0)
         {}
 
         ~IterationUpdateCallback() {}
 
         ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
         {
+            if (summary.iteration == 0)
+                initial_cost_ = summary.cost;
+
             if (problem_->getStateBlockNotificationMapSize() != 0 or
                 problem_->getFactorNotificationMapSize() != 0)
             {
-                WOLF_INFO_COND(verbose_, "Stopping solver to update the problem!");
-                return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
+                if (summary.cost >= initial_cost_)
+                {
+                    WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration);
+                    return ceres::SOLVER_ABORT;
+                }
+                else
+                {
+                    WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration);
+                    return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
+                }
             }
             return ceres::SOLVER_CONTINUE;
         }
@@ -37,6 +49,7 @@ class IterationUpdateCallback : public ceres::IterationCallback
     private:
         ProblemPtr problem_;
         bool verbose_;
+        double initial_cost_;
 };
 
 }