Added SolverManager::autoSetup()

demos/hello_wolf/hello_wolf_autoconf.cpp

@@ -111,22 +111,17 @@ int main()
     std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
     std::string wolf_path   = _WOLF_CODE_DIR;
 
-    // parse file into params server: each param will be retrievable from this params server:
+    // parse file into params node
     yaml_schema_cpp::YamlServer server({wolf_path}, wolf_path + "/" + config_file);
-    if (not server.applySchema("Problem2d"))
-    {
-        WOLF_ERROR(server.getLog());
-        return 1;
-    }
-    // Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
-    ProblemPtr problem = Problem::autoSetup(server.getNode());
+
+    // Wolf problem: automatically build the left branch of the wolf tree from the contents of the yaml node
+    ProblemPtr problem = Problem::autoSetup(server.getNode(), {wolf_path});
 
     // Print problem to see its status before processing any sensor data
     problem->print(4, 0, 1, 0);
 
     // Solver. Configure a Ceres solver
-    SolverManagerPtr ceres =
-        FactorySolverNode::create("SolverCeres", problem, server.getNode()["solver"], {wolf_path});
+    SolverManagerPtr ceres = SolverManager::autoSetup(problem, server.getNode()["solver"], {wolf_path});
     if (not ceres)
     {
         WOLF_ERROR("Couldn't load or validate the yaml file for the solver");
@@ -229,7 +224,7 @@ int main()
 
     // SOLVE with exact initial guess
     WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
-    std::string report = ceres->solve(SolverManager::ReportVerbosity::FULL);
+    std::string report = ceres->solve(ReportVerbosity::FULL);
     WOLF_INFO(report);  // should show a very low iteration number (possibly 1)
     problem->print(1, 0, 1, 0);
 
@@ -240,13 +235,13 @@ int main()
 
     // SOLVE again
     WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
-    report = ceres->solve(SolverManager::ReportVerbosity::FULL);
+    report = ceres->solve(ReportVerbosity::FULL);
     WOLF_INFO(report);  // should show a very high iteration number (more than 10, or than 100!)
     problem->print(1, 0, 1, 0);
 
     // GET COVARIANCES of all states
     WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
-    ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    ceres->computeCovariances(CovarianceBlocksToBeComputed::ALL_MARGINALS);
     for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
     {
         Eigen::MatrixXd cov;

demos/hello_wolf/yaml/hello_wolf_config.yaml

@@ -19,6 +19,8 @@ problem:
     type: "none"
 
 solver:
+  type: SolverCeres
+  plugin: core
   max_num_iterations: 100
   verbose: 0
   period: 0.2
@@ -27,9 +29,9 @@ solver:
   compute_cov: false
   minimizer: LEVENBERG_MARQUARDT
   use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
-  parameter_tolerance: 0.000001
-  function_tolerance: 0.000001
-  gradient_tolerance: 0.0000000001
+  parameter_tolerance: 1e-6
+  function_tolerance: 1e-6
+  gradient_tolerance: 1e-10
   
 sensors:
   

include/core/problem/problem.h

@@ -105,8 +105,11 @@ class Problem : public std::enable_shared_from_this<Problem>
                              const TypeComposite& _frame_structure = {},
                              LoaderPtr            _loader          = nullptr);  // USE THIS AS A CONSTRUCTOR!
     static ProblemPtr autoSetup(const std::string&       _input_yaml_file,
-                                std::vector<std::string> _primary_schema_folders = {});
-    static ProblemPtr autoSetup(YAML::Node _param_node, LoaderPtr _loader = nullptr);
+                                std::vector<std::string> _primary_schema_folders = {},
+                                LoaderPtr                _loader                 = nullptr);
+    static ProblemPtr autoSetup(YAML::Node               _param_node,
+                                std::vector<std::string> _primary_schema_folders = {},
+                                LoaderPtr                _loader                 = nullptr);
     virtual ~Problem();
 
     // Properties -----------------------------------------

include/core/solver/solver_manager.h

@@ -33,13 +33,13 @@ WOLF_PTR_TYPEDEFS(SolverManager)
 /*
  * Macro for defining Autoconf solver creator for WOLF's high level API.
  *
- * Place a call to this macro inside your class declaration (in the solver_manager_class.h file),
+ * Place a call to this macro inside your class declaration (in the solver_class.h file),
  * preferably just after the constructors.
  *
- * In order to use this macro, the derived solver manager class, SolverManagerClass,
+ * In order to use this macro, the derived solver manager class, SolverClass,
  * must have a constructor available with the API:
  *
- *   SolverManagerClass(const ProblemPtr& wolf_problem,
+ *   SolverClass(const ProblemPtr& wolf_problem,
  *                      const YAML::Node _params);
  */
 #define WOLF_SOLVER_CREATE(SolverClass)                                                                               \
@@ -79,9 +79,6 @@ class SolverManager
 {
   public:
     /** \brief Enumeration of covariance blocks to be computed
-     *
-     * Enumeration of covariance blocks to be computed
-     *
      */
     enum class CovarianceBlocksToBeComputed : std::size_t
     {
@@ -94,8 +91,7 @@ class SolverManager
         GAUSS_TUCAN_ONLY_POSITION = 5   ///< GAUSS: last frame P and T
     };
 
-    /**
-     * \brief Enumeration for the verbosity of the solver report.
+    /** \brief Enumeration for the verbosity of the solver report.
      */
     enum class ReportVerbosity : std::size_t
     {
@@ -134,6 +130,16 @@ class SolverManager
      */
     SolverManager(const ProblemPtr& _problem, const YAML::Node& _params);
 
+    static SolverManagerPtr autoSetup(const ProblemPtr&        _problem,
+                                      const std::string&       _input_yaml_file,
+                                      std::vector<std::string> _primary_schema_folders = {},
+                                      LoaderPtr                _loader                 = nullptr);
+
+    static SolverManagerPtr autoSetup(const ProblemPtr&        _problem,
+                                      YAML::Node               _param_node,
+                                      std::vector<std::string> _primary_schema_folders = {},
+                                      LoaderPtr                _loader                 = nullptr);
+
     virtual ~SolverManager();
 
     /**
@@ -241,11 +247,11 @@ class SolverManager
 
   protected:
     // PARAMS
-    double                                      period_;
-    SolverManager::ReportVerbosity              verbose_;
-    bool                                        compute_cov_;
-    SolverManager::CovarianceBlocksToBeComputed cov_enum_;
-    double                                      cov_period_;
+    double                       period_;
+    ReportVerbosity              verbose_;
+    bool                         compute_cov_;
+    CovarianceBlocksToBeComputed cov_enum_;
+    double                       cov_period_;
 };
 
 }  // namespace wolf

schema/problem/Problem.schema

@@ -5,7 +5,7 @@ problem:
     _mandatory: true
     _doc: Tree manager parameters
   dimension:
-    _type: int
+    _type: unsigned int
     _mandatory: true
     _options: [2, 3]
     _doc: "Dimension of the problem. '2' for 2D or '3' for 3D"

schema/problem/Problem2d.schema

@@ -15,7 +15,7 @@ problem:
       _doc: "specification for the first frame linear velocity state"
       
   dimension:
-    _type: int
+    _type: unsigned int
     _mandatory: true
     _options: [2]
     _doc: "Dimension of the problem: '2' for 2D"
\ No newline at end of file

schema/problem/Problem3d.schema

@@ -15,7 +15,7 @@ problem:
       _doc: "specification for the first frame linear velocity state"
       
   dimension:
-    _type: int
+    _type: unsigned int
     _mandatory: true
     _options: [3]
     _doc: "Dimension of the problem: '3' for 3D"
\ No newline at end of file

src/problem/problem.cpp

@@ -81,17 +81,28 @@ ProblemPtr Problem::create(SizeEigen _dim, const TypeComposite& _frame_types, Lo
     return p->shared_from_this();
 }
 
-ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, std::vector<std::string> _schema_folders)
+ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, std::vector<std::string> _schema_folders, LoaderPtr _loader)
 {
-    // Create YAML server
+    // Load YAML node (yaml schema server used to flatten "follows")
     WOLF_DEBUG("Loading YAML file...");
     yaml_schema_cpp::YamlServer server = yaml_schema_cpp::YamlServer(_schema_folders, _input_yaml_file);
-    // YAML::Node                  params_node = server.getNode();
+
+    // Call autoSetup via YAML node
+    return Problem::autoSetup(server.getNode(), _schema_folders, _loader);
+}
+
+ProblemPtr Problem::autoSetup(YAML::Node _param_node, std::vector<std::string> _schema_folders, LoaderPtr _loader)
+{
+    // VALIDATION ===============================================================================
+    // Create YAML server
+    yaml_schema_cpp::YamlServer server = yaml_schema_cpp::YamlServer(_schema_folders);
+    server.setYaml(_param_node);
 
     // SCHEMA FOLDERS (optional _schema_folders specify folders where to search schemas before installed ones)
     // Search and load plugins to get the installed schema folders registered
-    auto loader = std::make_shared<Loader>();
-    searchAndLoadPlugins(server.getNode(), loader);
+    if (not _loader)
+        _loader = std::make_shared<Loader>();
+    searchAndLoadPlugins(server.getNode(), _loader);
 
     // Add the installed schema folders after optional input folders
     server.addFolderSchema(FolderRegistry::getRegisteredFolders());
@@ -103,7 +114,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, std::vector<s
         WOLF_ERROR(server.getLog());
         return nullptr;
     }
-    // Validate against schema of specific dimension
+    // Validate against specific schema 2D or 3D
     bool is2D = server.getNode()["problem"]["dimension"].as<int>() == 2;
     if (not server.applySchema(is2D ? "Problem2d.schema" : "Problem3d.schema"))
     {
@@ -115,12 +126,6 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, std::vector<s
                "! Node after validation:\n",
                server.getNode());
 
-    // Get param node
-    return Problem::autoSetup(server.getNode(), loader);
-}
-
-ProblemPtr Problem::autoSetup(YAML::Node _param_node, LoaderPtr _loader)
-{
     // PROBLEM ===============================================================================
     // dimension
     WOLF_DEBUG("Loading problem parameters...");
@@ -1295,7 +1300,7 @@ void Problem::perturb(double amplitude)
 
 void Problem::transform(const VectorComposite& _transformation)
 {
-    std::lock_guard<std::mutex> lock_transform(mut_transform_);  // Protect especially from SolverManager::update()
+    std::lock_guard<std::mutex> lock_transform(mut_transform_);  // Protect especially from SolverBase::update()
 
     transformation_ = _transformation;
     transformed_.store(true);

src/solver/solver_manager.cpp

@@ -22,6 +22,7 @@
 #include "core/trajectory/trajectory_base.h"
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
+#include "core/utils/loader_utils.h"
 
 namespace wolf
 {
@@ -44,17 +45,69 @@ SolverManager::SolverManager(const ProblemPtr& _problem, const YAML::Node& _para
     assert(_problem != nullptr && "Passed a nullptr ProblemPtr.");
 
     period_      = _params["period"].as<double>();
-    verbose_     = (SolverManager::ReportVerbosity)_params["verbose"].as<int>();
+    verbose_     = (ReportVerbosity)_params["verbose"].as<int>();
     compute_cov_ = _params["compute_cov"].as<bool>();
     if (compute_cov_)
     {
-        cov_enum_   = (SolverManager::CovarianceBlocksToBeComputed)_params["cov_enum"].as<int>();
+        cov_enum_   = (CovarianceBlocksToBeComputed)_params["cov_enum"].as<int>();
         cov_period_ = _params["cov_period"].as<double>();
     }
 }
 
 SolverManager::~SolverManager() {}
 
+SolverManagerPtr SolverManager::autoSetup(const ProblemPtr&        _problem,
+                                          const std::string&       _input_yaml_file,
+                                          std::vector<std::string> _schema_folders,
+                                          LoaderPtr                _loader)
+{
+    // Load YAML node (yaml schema server used to flatten "follows")
+    WOLF_DEBUG("Loading YAML file...");
+    yaml_schema_cpp::YamlServer server = yaml_schema_cpp::YamlServer(_schema_folders, _input_yaml_file);
+
+    // Call autoSetup via YAML node
+    return SolverManager::autoSetup(_problem, server.getNode(), _schema_folders, _loader);
+}
+
+SolverManagerPtr SolverManager::autoSetup(const ProblemPtr&        _problem,
+                                          YAML::Node               _param_node,
+                                          std::vector<std::string> _schema_folders,
+                                          LoaderPtr                _loader)
+{
+    // VALIDATION ===============================================================================
+    // Create YAML server
+    yaml_schema_cpp::YamlServer server = yaml_schema_cpp::YamlServer(_schema_folders);
+    server.setYaml(_param_node);
+
+    // SCHEMA FOLDERS (optional _schema_folders specify folders where to search schemas before installed ones)
+    // Search and load plugins to get the installed schema folders registered
+    if (not _loader) _loader = std::make_shared<Loader>();
+    searchAndLoadPlugins(server.getNode(), _loader);
+
+    // Add the installed schema folders after optional input folders
+    server.addFolderSchema(FolderRegistry::getRegisteredFolders());
+
+    // Validate against TypeAndPlugin (check if it has 'type' and 'plugin')
+    // NOTE: Validation with the correponding schema is done in the creator
+    if (not server.applySchema("TypeAndPlugin"))
+    {
+        WOLF_ERROR(server.getLog());
+        throw std::runtime_error("Solver could not be created! Missing type or plugin");
+    }
+
+    // Load plugin if factory not registered
+    auto solver_type = _param_node["type"].as<std::string>();
+    if (not FactorySolverNode::isCreatorRegistered(solver_type))
+    {
+        loadPlugin(_loader, _param_node["plugin"].as<std::string>());
+        if (not FactorySolverNode::isCreatorRegistered(solver_type))
+            throw std::runtime_error("Solver creator not registered after loading its plugin.");
+    }
+
+    // Create sensor
+    return FactorySolverNode::create(solver_type, _problem, _param_node, _schema_folders);
+}
+
 void SolverManager::update()
 {
     // lock mutex to prevent Problem::transform() during this update()
@@ -690,28 +743,29 @@ bool SolverManager::check(std::string prefix) const
 
 void SolverManager::printProfiling(std::ostream& _stream) const
 {
-    _stream << "\nSolverManager:"
-            << "\nTotal values:"
-            << "\n\tSolving state:          " << 1e-6 * acc_duration_total_.count() << " s - executions: " << n_solve_
-            << "\n\t\tUpdate problem:   " << 1e-6 * acc_duration_update_.count() << " s"
-            << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
-            << "\n\t\tSolver:           " << 1e-6 * acc_duration_solver_.count() << " s"
-            << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
-            << "\n\t\tUpdate state:     " << 1e-6 * acc_duration_state_.count() << " s"
-            << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
-            << "\n\tSolving covariance:     " << 1e-6 * acc_duration_cov_.count() << " s - executions: " << n_cov_
-            << "\nAverage:"
-            << "\n\tSolving state:          " << 1e-3 * acc_duration_total_.count() / n_solve_ << " ms"
-            << "\n\t\tUpdate problem:   " << 1e-3 * acc_duration_update_.count() / n_solve_ << " ms"
-            << "\n\t\tSolver:           " << 1e-3 * acc_duration_solver_.count() / n_solve_ << " ms"
-            << "\n\t\tUpdate state:     " << 1e-3 * acc_duration_state_.count() / n_solve_ << " ms"
-            << "\n\tSolving covariance:     " << 1e-3 * acc_duration_cov_.count() / n_cov_ << " ms"
-            << "\nMax values:"
-            << "\n\tSolving state:          " << 1e-3 * max_duration_total_.count() << " ms"
-            << "\n\t\tUpdate problem:   " << 1e-3 * max_duration_update_.count() << " ms"
-            << "\n\t\tSolver:           " << 1e-3 * max_duration_solver_.count() << " ms"
-            << "\n\t\tUpdate state:     " << 1e-3 * max_duration_state_.count() << " ms"
-            << "\n\tSolving covariance:     " << 1e-3 * max_duration_cov_.count() << " ms" << std::endl;
+    _stream << "\nSolverManager:"  //
+            << "\nTotal values:"   //
+            << "\n\tSolving state:          " << 1e-6 * acc_duration_total_.count()
+            << " s - executions: " << n_solve_                                                                      //
+            << "\n\t\tUpdate problem:   " << 1e-6 * acc_duration_update_.count() << " s"                            //
+            << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"                  //
+            << "\n\t\tSolver:           " << 1e-6 * acc_duration_solver_.count() << " s"                            //
+            << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"                  //
+            << "\n\t\tUpdate state:     " << 1e-6 * acc_duration_state_.count() << " s"                             //
+            << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"                   //
+            << "\n\tSolving covariance:     " << 1e-6 * acc_duration_cov_.count() << " s - executions: " << n_cov_  //
+            << "\nAverage:"                                                                                         //
+            << "\n\tSolving state:          " << 1e-3 * acc_duration_total_.count() / n_solve_ << " ms"             //
+            << "\n\t\tUpdate problem:   " << 1e-3 * acc_duration_update_.count() / n_solve_ << " ms"                //
+            << "\n\t\tSolver:           " << 1e-3 * acc_duration_solver_.count() / n_solve_ << " ms"                //
+            << "\n\t\tUpdate state:     " << 1e-3 * acc_duration_state_.count() / n_solve_ << " ms"                 //
+            << "\n\tSolving covariance:     " << 1e-3 * acc_duration_cov_.count() / n_cov_ << " ms"                 //
+            << "\nMax values:"                                                                                      //
+            << "\n\tSolving state:          " << 1e-3 * max_duration_total_.count() << " ms"                        //
+            << "\n\t\tUpdate problem:   " << 1e-3 * max_duration_update_.count() << " ms"                           //
+            << "\n\t\tSolver:           " << 1e-3 * max_duration_solver_.count() << " ms"                           //
+            << "\n\t\tUpdate state:     " << 1e-3 * max_duration_state_.count() << " ms"                            //
+            << "\n\tSolving covariance:     " << 1e-3 * max_duration_cov_.count() << " ms" << std::endl;            //
 
     printProfilingDerived(_stream);
 }