Merge branch 'feature/solver_getters' into 'devel'

Solver getters

See merge request mobile_robotics/wolf!236

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

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

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

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("");};