working

include/core/ceres_wrapper/solver_ceres.h

@@ -66,12 +66,12 @@ struct ParamsCeres : public ParamsSolver
         else if (minimizer == "LBFGS" or minimizer == "lbfgs")
         {
             solver_options.minimizer_type = ceres::LINE_SEARCH;
-            solver_options.line_search_direction_type == ceres::LBFGS;
+            solver_options.line_search_direction_type = ceres::LBFGS;
         }
         else if (minimizer == "BFGS" or minimizer == "bfgs")
         {
             solver_options.minimizer_type = ceres::LINE_SEARCH;
-            solver_options.line_search_direction_type == ceres::LBFGS;
+            solver_options.line_search_direction_type = ceres::BFGS;
         }
         else
         {

include/core/solver/solver_manager.h

@@ -38,7 +38,7 @@ static SolverManagerPtr create(const ProblemPtr& _problem,
     return std::make_shared<SolverClass>(_problem, params);                     \
 }                                                                               \
 
-        struct ParamsSolver;
+struct ParamsSolver;
 
 /**
  * \brief Solver manager for WOLF
@@ -115,6 +115,8 @@ class SolverManager
          */
         std::string solve(const ReportVerbosity report_level);
 
+        virtual bool computeCovariances() final;
+
         virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
 
         virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
@@ -140,8 +142,12 @@ class SolverManager
 
         ProblemPtr getProblem();
 
+        virtual ParamsSolverPtr getParams() const;
+
         double getPeriod() const;
 
+        double getCovPeriod() const;
+
         ReportVerbosity getVerbosity() const;
 
         virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
@@ -210,6 +216,9 @@ struct ParamsSolver: public ParamsBase
         std::string prefix = "solver/";
         double period = 0.0;
         SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
+        bool compute_cov = false;
+        SolverManager::CovarianceBlocksToBeComputed cov_enum = SolverManager::CovarianceBlocksToBeComputed::ROBOT_LANDMARKS;
+        double cov_period = 1.0;
 
         ParamsSolver() = default;
         ParamsSolver(std::string _unique_name, const ParamsServer& _server):
@@ -217,10 +226,20 @@ struct ParamsSolver: public ParamsBase
         {
             period  = _server.getParam<double>(prefix + "period");
             verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
+            compute_cov = _server.getParam<bool>(prefix + "compute_cov");
+            if (compute_cov)
+            {
+                cov_enum   = (SolverManager::CovarianceBlocksToBeComputed)_server.getParam<int>(prefix + "cov_enum");
+                cov_period = _server.getParam<double>(prefix + "cov_period");
+            }
         }
         std::string print() const override
         {
-            return  "period: "                   + std::to_string(period)         + "\n";
+            return  "period: "      + std::to_string(period)        + "\n" +
+                    "verbose: "     + std::to_string((int)verbose)  + "\n" +
+                    "compute_cov: " + std::to_string(compute_cov)   + "\n" +
+                    "cov_enum: "    + std::to_string((int)cov_enum) + "\n" +
+                    "cov_period: "  + std::to_string(cov_period)    + "\n";
         }
 
         ~ParamsSolver() override = default;

src/solver/solver_manager.cpp

@@ -185,6 +185,10 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
     return report;
 }
 
+bool SolverManager::computeCovariances()
+{
+    return computeCovariances(params_->cov_enum);
+}
 
 bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
 {
@@ -500,11 +504,21 @@ int SolverManager::numStateBlocksFloating() const
     return floating_state_blocks_.size();
 }
 
+ParamsSolverPtr SolverManager::getParams() const
+{
+    return params_;
+}
+
 double SolverManager::getPeriod() const
 {
     return params_->period;
 }
 
+double SolverManager::getCovPeriod() const
+{
+    return params_->cov_period;
+}
+
 bool SolverManager::check(std::string prefix) const
 {
     bool ok = true;

test/yaml/solver.yaml

@@ -6,4 +6,8 @@ n_threads: 2
 function_tolerance: 0.000001
 gradient_tolerance: 0.0000000001
 minimizer: "LEVENBERG_MARQUARDT"
-use_nonmonotonic_steps: false
+use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
\ No newline at end of file
+max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
+compute_cov: true
+cov_period: 1                         #only if compute_cov
+cov_enum: 2                           #only if compute_cov
\ No newline at end of file