Resolve "SolverCeres: more parameters"

Closes #415 (closed)

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;