diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index 4bf6f7de661059e7c9f96e7f12bb444f18bed013..0c94e4c5e82ce656caf7ac4a939a3d76a5a57100 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -44,17 +44,52 @@ struct ParamsCeres : public ParamsSolver if (update_immediately) min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations"); - // ceres solver options + // CERES SOLVER OPTIONS solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations"); + solver_options.function_tolerance = _server.getParam<double>(prefix + "function_tolerance"); + solver_options.gradient_tolerance = _server.getParam<double>(prefix + "gradient_tolerance"); solver_options.num_threads = _server.getParam<int>(prefix + "n_threads"); - covariance_options.num_threads = _server.getParam<int>(prefix + "n_threads"); + covariance_options.num_threads = solver_options.num_threads; + + // minimizer type + std::string minimizer = _server.getParam<std::string>(prefix + "minimizer"); + if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt") + { + solver_options.minimizer_type = ceres::TRUST_REGION; + solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; + } + else if (minimizer == "DOGLEG" or minimizer == "dogleg") + { + solver_options.minimizer_type = ceres::TRUST_REGION; + solver_options.trust_region_strategy_type = ceres::DOGLEG; + } + else if (minimizer == "LBFGS" or minimizer == "lbfgs") + { + solver_options.minimizer_type = ceres::LINE_SEARCH; + 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; + } + else + { + throw std::runtime_error("ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or 'BFGS' (upper or lowercase)"); + } + + // specific options for TRUST REGION + if (solver_options.minimizer_type == ceres::TRUST_REGION) + { + solver_options.use_nonmonotonic_steps = _server.getParam<bool>(prefix + "use_nonmonotonic_steps"); + if (solver_options.use_nonmonotonic_steps) + solver_options.max_consecutive_nonmonotonic_steps = _server.getParam<int>(prefix + "max_consecutive_nonmonotonic_steps"); + } } void loadHardcodedValues() { solver_options = ceres::Solver::Options(); - solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH; - solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG; problem_options = ceres::Problem::Options(); covariance_options = ceres::Covariance::Options(); problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml index 8f00f6499df2c96c9993bd6c486554579e7fbab9..0b030b80faa8a5f63dfa4c60aa19bcfd3c81a9b0 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -1,10 +1,6 @@ config: solver: - period: 1 - verbose: 2 - update_immediately: false - max_num_iterations: 10 - n_threads: 2 + follow: "test/yaml/solver.yaml" problem: frame_structure: "PO" dimension: 3 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml index 114e865bdc3a86b6d0cddf42e0f5360b7b2d5928..d00b201f9d84f53cf0b332315cd7617f60a490a5 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml @@ -1,9 +1,6 @@ config: solver: - period: 1 - verbose: 2 - update_immediately: false - max_num_iterations: 10 + follow: "test/yaml/solver.yaml" problem: frame_structure: "PO" dimension: 3 diff --git a/test/yaml/solver.yaml b/test/yaml/solver.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a748fc64917cf09c87ab4a278203f5c890fe7ecd --- /dev/null +++ b/test/yaml/solver.yaml @@ -0,0 +1,9 @@ +period: 1 +verbose: 2 +update_immediately: false +max_num_iterations: 1000 +n_threads: 2 +function_tolerance: 0.000001 +gradient_tolerance: 0.0000000001 +minimizer: "LEVENBERG_MARQUARDT" +use_nonmonotonic_steps: false \ No newline at end of file