diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index bbd383c66f3ea4edbe0a3caadeebbd5af47d2a02..26babf9af4af7e2b68efe1486b7f1d82a965b0df 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -351,6 +351,26 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta addConstraint(ctr); } +bool CeresManager::hasConverged() +{ + return summary_.termination_type == ceres::CONVERGENCE; +} + +SizeStd CeresManager::iterations() +{ + return summary_.iterations.size(); +} + +Scalar CeresManager::initialCost() +{ + return Scalar(summary_.initial_cost); +} + +Scalar CeresManager::finalCost() +{ + return Scalar(summary_.final_cost); +} + ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr) { assert(_ctr_ptr != nullptr); diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h index ebc8b04e945fb3007aa1e6c6926b450c84211746..efd9861bf380710ea027a06570f08910107a397a 100644 --- a/src/ceres_wrapper/ceres_manager.h +++ b/src/ceres_wrapper/ceres_manager.h @@ -53,6 +53,14 @@ class CeresManager : public SolverManager virtual void computeCovariances(const StateBlockList& st_list) override; + virtual bool hasConverged(); + + virtual SizeStd iterations(); + + virtual Scalar initialCost(); + + virtual Scalar finalCost(); + ceres::Solver::Options& getSolverOptions(); void check(); diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h index 5416067acf84572424f9017c606ab0aa910e92b3..9f1189495669a0b02648c2935f09494e40438c0d 100644 --- a/src/solver/solver_manager.h +++ b/src/solver/solver_manager.h @@ -55,6 +55,14 @@ public: virtual void computeCovariances(const StateBlockList& st_list) = 0; + virtual bool hasConverged() = 0; + + virtual SizeStd iterations() = 0; + + virtual Scalar initialCost() = 0; + + virtual Scalar finalCost() = 0; + virtual void update(); ProblemPtr getProblemPtr(); diff --git a/src/test/gtest_solver_manager.cpp b/src/test/gtest_solver_manager.cpp index effdfc1401c6ece1822651f287dfb7d4ffb1bbb3..62bc03454f7bfe68dfb2fecf7709b3ed8736170d 100644 --- a/src/test/gtest_solver_manager.cpp +++ b/src/test/gtest_solver_manager.cpp @@ -63,6 +63,14 @@ class SolverManagerWrapper : public SolverManager virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){}; virtual void computeCovariances(const StateBlockList& st_list){}; + // The following are dummy implementations + bool hasConverged() { return true; } + SizeStd iterations() { return 1; } + Scalar initialCost() { return Scalar(1); } + Scalar finalCost() { return Scalar(0); } + + + protected: virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};