diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index e643c6f13c8c94d3e0085eeed627658cc01856c8..094b17429bb03774d83c780501239686f164eb4b 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/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;
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index a59401409a93dc6a16c47d53c44192f08232a35f..7f718f169bf02cdf17f701d3ae7de9fac9fc3c5d 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/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:
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 307e58ab5d010ff3082c1c83d27a0cd6e4a8d8a1..9f329f3177ec4267bbf33144f06e04b3c3b41436 100644
--- a/include/core/problem/problem.h
+++ b/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 -----------------------------------------
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 664231fd1ee366b60da32c2bdc44fe97716558f3..ab6e04e72fa59f28777e32c9fb413ac054fb7a4a 100644
--- a/include/core/solver/solver_manager.h
+++ b/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
diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema
index f681a495b1e7a73efed41159bdaf6d7c4e28b9b1..71d7f9b6930adf84fe83cee67b4cb4830aeae838 100644
--- a/schema/problem/Problem.schema
+++ b/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"
diff --git a/schema/problem/Problem2d.schema b/schema/problem/Problem2d.schema
index ab42adc11bdc80aec1ccd2b11b36affa22709a44..51ce8f238a9ff051d848ee55ddda3f49c4bc415e 100644
--- a/schema/problem/Problem2d.schema
+++ b/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
diff --git a/schema/problem/Problem3d.schema b/schema/problem/Problem3d.schema
index 3ef63833fd716f972cf9229777970c1d14d0e639..6f6dffd3fbe40d581abb219782532831121aa610 100644
--- a/schema/problem/Problem3d.schema
+++ b/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
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 766edef8ff256e05c63337a4b6ec537579eee771..fdf17296a82765151cce784bc6b5fd8034ab3549 100644
--- a/src/problem/problem.cpp
+++ b/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);
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index bdb62f31eb9db6685ebf65363c658d05cbb64964..ebd0274d5459ad66d7be6a1852b90fa508e0e224 100644
--- a/src/solver/solver_manager.cpp
+++ b/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);
}