From 9a79b4b875bcfb529dba31de9128238f04337353 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 16 Jun 2020 11:55:30 +0200
Subject: [PATCH] implemented and working
---
.../ceres_wrapper/iteration_update_callback.h | 17 +++++++++++++++--
1 file changed, 15 insertions(+), 2 deletions(-)
diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h
index bf847c227..27e5d758a 100644
--- a/include/core/ceres_wrapper/iteration_update_callback.h
+++ b/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_;
};
}
--
GitLab