diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index 0c94e4c5e82ce656caf7ac4a939a3d76a5a57100..b6f14e05f7d3e4c9b5e05fb4fc407a09717b93f1 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/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 { diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h index f3fbeefb3dce65e58ad82d3c604ba3e554169624..7908ae476dfdd4f241c44be5b62b74a18bb832d9 100644 --- a/include/core/solver/solver_manager.h +++ b/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; diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 99dfb8729c9bc5cb05f00758494571de9d52fc23..00154b034967965bb2ba8bbb1e6959fe907bfe93 100644 --- a/src/solver/solver_manager.cpp +++ b/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; diff --git a/test/yaml/solver.yaml b/test/yaml/solver.yaml index a748fc64917cf09c87ab4a278203f5c890fe7ecd..ec6a06a9eaefa6163ef6c6bb7512ac00d24c7725 100644 --- a/test/yaml/solver.yaml +++ b/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 \ No newline at end of file +use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG +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