diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 4793228db629b54d71462d069827d4909f8e32f6..99638dc99f3a69241f3a6d983b30213757178541 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -122,7 +122,7 @@ int main()
// Solver. Configure a Ceres solver
SolverManagerPtr ceres =
- SolverManager::autoSetup(problem, server.getNode()["solver"], {wolf_path}, problem->getLoader());
+ SolverManager::autoSetup(problem, server.getNode()["solver"], {wolf_path});
if (not ceres)
{
WOLF_ERROR("Couldn't load or validate the yaml file for the solver");
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 2ea7babb1fae1631398a34ea8e0c9767aff93031..6e4493691848ede6165078d534baa3aaff8940c5 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -133,13 +133,13 @@ class SolverManager
static SolverManagerPtr autoSetup(const ProblemPtr& _problem,
const std::string& _input_yaml_file,
std::vector<std::string> _primary_schema_folders,
- LoaderPtr _loader,
+ LoaderPtr _loader = nullptr,
bool _skip_yaml_validation = false);
static SolverManagerPtr autoSetup(const ProblemPtr& _problem,
YAML::Node _param_node,
std::vector<std::string> _primary_schema_folders,
- LoaderPtr _loader,
+ LoaderPtr _loader = nullptr,
bool _skip_yaml_validation = false);
virtual ~SolverManager();
diff --git a/include/core/utils/loader_utils.h b/include/core/utils/loader_utils.h
index 6ceca75a10ab7292d016c8cc0888123e151143aa..12caf680b56af506bd9186830faa72f44c2c5182 100644
--- a/include/core/utils/loader_utils.h
+++ b/include/core/utils/loader_utils.h
@@ -32,13 +32,8 @@ std::set<std::string> searchPlugins(const YAML::Node& _node, std::list<YAML::Nod
void appendPluginsInstalledSchemaFolders(const YAML::Node& _node, std::vector<std::string>& _local_folders);
void loadLibrary(LoaderPtr _loader, const std::string& lib_name, const std::string& root);
-void searchAndLoadLibraries(const YAML::Node& _node,
- LoaderPtr _loader,
- const std::string& lib_field,
- const std::string& root);
std::set<std::string> searchFieldsRecursive(const YAML::Node& _node,
std::list<YAML::Node>& _visited_nodes,
const std::string& field);
-// void appendPluginsInstalledSchemaFolders(const YAML::Node& _node, std::vector<std::string>& _local_folders);
} /* namespace wolf */
diff --git a/schema/solver/SolverCeres.schema b/schema/solver/SolverCeres.schema
index 09240190ea7e0dfe8f67071449404e060a275f3a..fb8979983e18b56ce97b4f714aece98109122e20 100644
--- a/schema/solver/SolverCeres.schema
+++ b/schema/solver/SolverCeres.schema
@@ -3,7 +3,7 @@ follow: SolverManager.schema
minimizer:
_mandatory: true
_type: string
- _options: [LEVENBERG_MARQUARDT, levenberg_marquardt, DOGLEG, dogleg, LBFGS, lbfgs, BFGS, bfgs]
+ _options: [levenberg_marquardt, dogleg, lbfgs, bfgs]
_doc: Type of minimizer.
interrupt_on_problem_change:
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 3fd7867b30c61d4d2932896d2cfffb605939f721..9eb429ae462ba42fd5233270322577f617596217 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -47,22 +47,22 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _par
solver_options_.num_threads = _params["n_threads"].as<int>();
std::string minimizer = _params["minimizer"].as<std::string>();
- if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt")
+ if (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")
+ else if (minimizer == "dogleg")
{
solver_options_.minimizer_type = ceres::TRUST_REGION;
solver_options_.trust_region_strategy_type = ceres::DOGLEG;
}
- else if (minimizer == "LBFGS" or minimizer == "lbfgs")
+ else if (minimizer == "lbfgs")
{
solver_options_.minimizer_type = ceres::LINE_SEARCH;
solver_options_.line_search_direction_type = ceres::LBFGS;
}
- else if (minimizer == "BFGS" or minimizer == "bfgs")
+ else if (minimizer == "bfgs")
{
solver_options_.minimizer_type = ceres::LINE_SEARCH;
solver_options_.line_search_direction_type = ceres::BFGS;
@@ -70,8 +70,7 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _par
else
{
throw std::runtime_error(
- "ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or "
- "'BFGS' (upper or lowercase)");
+ "ParamsCeres: Wrong parameter 'minimizer'. Should be 'levenberg_marquardt', 'dogleg', 'lbfgs' or 'bfgs'");
}
if (solver_options_.minimizer_type == ceres::TRUST_REGION) // specific options for TRUST REGION
{
@@ -897,11 +896,10 @@ bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr&
void SolverCeres::printProfilingDerived(std::ostream& _stream) const
{
- _stream << "Iterations:"
- << "\n\tUser-defined limit: " << solver_options_.max_num_iterations
+ _stream << "Iterations:" << "\n\tUser-defined limit: " << solver_options_.max_num_iterations
<< "\n\tAverage iterations: " << n_iter_acc_ / n_solve_
- << "\n\tMax. iterations: " << n_iter_max_ << "\nTermination:"
- << "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %"
+ << "\n\tMax. iterations: " << n_iter_max_
+ << "\nTermination:" << "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %"
<< "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve_ << " %"
<< "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %" << std::endl;
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index e255601a24a3c6ddee90c85ac955d30f48cd720a..d926bd33dfa08ed6c185f8db396be8c20c71095c 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -90,9 +90,10 @@ SolverManagerPtr SolverManager::autoSetup(const ProblemPtr& _problem,
throw std::runtime_error("Solver could not be created! Missing type or plugin");
}
- // Load plugin if not loaded
+ // Load plugin if not loaded (null loader -> use problem loader)
auto plugin = _param_node["plugin"].as<std::string>();
- if (not FolderRegistry::isFolderRegistered(plugin)) loadPlugin(_loader, plugin);
+ if (not FolderRegistry::isFolderRegistered(plugin))
+ loadPlugin((_loader ? _loader : _problem->getLoader()), plugin);
// SCHEMA FOLDERS (optional _schema_folders specify folders where to search schemas before installed ones)
// Add the installed schema folders after optional input folders
@@ -111,9 +112,11 @@ SolverManagerPtr SolverManager::autoSetup(const ProblemPtr& _problem,
return nullptr;
}
WOLF_DEBUG("YAML validated with '", solver_type, ".schema'", "! Node after validation:\n", server.getNode());
+ _param_node = server.getNode();
}
// Create sensor (without validation)
+ WOLF_DEBUG("Creating '", _param_node["type"].as<std::string>(), "' with params:\n", _param_node);
return FactorySolverNode::create(_param_node["type"].as<std::string>(), _problem, _param_node, {});
}
diff --git a/src/utils/loader_utils.cpp b/src/utils/loader_utils.cpp
index 06de6634b5dcb2794f4f3f65f170ed702c0d106b..6df57319e83f498e13607476d0e432ac5935baa6 100644
--- a/src/utils/loader_utils.cpp
+++ b/src/utils/loader_utils.cpp
@@ -26,65 +26,19 @@ namespace wolf
void loadPlugin(LoaderPtr _loader, const std::string& plugin_name)
{
loadLibrary(_loader, "libwolf" + plugin_name, _WOLF_LIB_DIR);
- // #if __APPLE__
- // std::string lib_extension = ".dylib";
- // #else
- // std::string lib_extension = ".so";
- // #endif
-
- // std::string plugins_path = _WOLF_LIB_DIR;
- // std::string plugin = plugins_path + "/libwolf" + plugin_name + lib_extension;
- // WOLF_DEBUG("Loading plugin " + plugin_name + " in " + plugins_path);
- // _loader->load(plugin);
}
void searchAndLoadPlugins(const YAML::Node& _node, LoaderPtr _loader)
{
- searchAndLoadLibraries(_node, _loader, "plugin", _WOLF_LIB_DIR);
- // std::list<YAML::Node> visited_nodes;
- // auto plugins = searchPlugins(_node, visited_nodes);
- // for (auto plugin : plugins)
- // if (not FolderRegistry::isFolderRegistered(plugin)) loadPlugin(_loader, plugin);
+ std::list<YAML::Node> visited_nodes;
+ auto plugins = searchPlugins(_node, visited_nodes);
+ for (auto plugin : plugins)
+ if (not FolderRegistry::isFolderRegistered(plugin)) loadPlugin(_loader, plugin);
}
std::set<std::string> searchPlugins(const YAML::Node& _node, std::list<YAML::Node>& _visited_nodes)
{
return searchFieldsRecursive(_node, _visited_nodes, "plugin");
- // std::set<std::string> plugins;
-
- // if (std::find(_visited_nodes.begin(), _visited_nodes.end(), _node) != _visited_nodes.end()) return {};
-
- // _visited_nodes.push_back(_node);
-
- // if (_node.IsMap())
- // {
- // for (const auto& pair : _node)
- // {
- // // is plugin?
- // if (pair.first.as<std::string>() == "plugin")
- // plugins.insert(pair.second.as<std::string>());
-
- // else
- // {
- // auto rec_plugins = searchPlugins(pair.second, _visited_nodes);
- // plugins.insert(rec_plugins.begin(), rec_plugins.end());
- // }
- // }
- // }
- // else if (_node.IsSequence())
- // {
- // for (const auto& child : _node)
- // {
- // auto rec_plugins = searchPlugins(child, _visited_nodes);
- // plugins.insert(rec_plugins.begin(), rec_plugins.end());
- // }
- // }
- // else if (_node.IsScalar())
- // {
- // return {};
- // }
-
- // return plugins;
}
void appendPluginsInstalledSchemaFolders(const YAML::Node& _node, std::vector<std::string>& _local_folders)
@@ -113,18 +67,6 @@ void loadLibrary(LoaderPtr _loader, const std::string& lib_name, const std::stri
_loader->load(lib);
}
-void searchAndLoadLibraries(const YAML::Node& _node,
- LoaderPtr _loader,
- const std::string& lib_field,
- const std::string& root)
-
-{
- std::list<YAML::Node> visited_nodes;
- auto libs = searchFieldsRecursive(_node, visited_nodes, lib_field);
- for (auto lib_name : libs)
- if (not FolderRegistry::isFolderRegistered(lib_name)) loadLibrary(_loader, lib_name, root);
-}
-
std::set<std::string> searchFieldsRecursive(const YAML::Node& _node,
std::list<YAML::Node>& _visited_nodes,
const std::string& lib_field)
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index 5680a9a93ccd73a7696613e1bf4c60e1739e6da6..ee6c196ee4bdc2328230585fb958b1b223fe5adc 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -99,7 +99,7 @@ TEST(SolverCeresTestFactories, FactoryNode)
params["parameter_tolerance"] = 1e-9;
params["function_tolerance"] = 1e-6;
params["gradient_tolerance"] = 1e-9;
- params["minimizer"] = "LEVENBERG_MARQUARDT";
+ params["minimizer"] = "levenberg_marquardt";
params["use_nonmonotonic_steps"] = false;
params["max_consecutive_nonmonotonic_steps"] = 5;
params["compute_cov"] = true;
@@ -114,11 +114,54 @@ TEST(SolverCeresTestFactories, FactoryNode)
EXPECT_TRUE(solver_ptr->check());
}
+TEST(SolverCeresTestFactories, autoSetupFile)
+{
+ auto problem = Problem::create(3);
+ auto solver_ptr = SolverManager::autoSetup(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeresTestFactories, autoSetupNode)
+{
+ auto problem = Problem::create(3);
+ YAML::Node params;
+ params["type"] = "SolverCeres";
+ params["plugin"] = "core";
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ params["period"] = 1;
+ params["verbose"] = 2;
+ params["interrupt_on_problem_change"] = false;
+ params["max_num_iterations"] = 1000;
+ params["n_threads"] = 2;
+ params["parameter_tolerance"] = 1e-9;
+ params["function_tolerance"] = 1e-6;
+ params["gradient_tolerance"] = 1e-9;
+ params["minimizer"] = "levenberg_marquardt";
+ params["use_nonmonotonic_steps"] = false;
+ params["max_consecutive_nonmonotonic_steps"] = 5;
+ params["compute_cov"] = true;
+ params["cov_period"] = 1;
+ params["cov_enum"] = 2;
+ auto solver_ptr = SolverManager::autoSetup(problem, params, {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
TEST(SolverCeresTestFactories, FactoryFile)
{
- auto problem = Problem::create(3);
- auto solver_ptr =
- FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+ auto problem = Problem::create(3);
+ auto solver_ptr = SolverManager::autoSetup(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// check pointers
EXPECT_EQ(problem, solver_ptr->getProblem());
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 1cb380c6befbddf00e4145c962b48cd0c27cc117..1a90e6f7a507ef7d5e0da3ee8b05c07ebadb1de4 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -112,6 +112,37 @@ TEST(SolverManagerFactories, FactoryFile)
EXPECT_TRUE(solver_ptr->check());
}
+TEST(SolverManagerFactories, autoSetupNode)
+{
+ auto problem = Problem::create(3);
+ YAML::Node params;
+ params["type"] = "SolverDummy";
+ params["plugin"] = "core";
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ auto solver_ptr = SolverManager::autoSetup(problem, params, {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManagerFactories, autoSetupFile)
+{
+ auto problem = Problem::create(3);
+ auto solver_ptr =
+ SolverManager::autoSetup(problem, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
////////////////////////////////////////////////////////
// FLOATING STATE BLOCKS
TEST_F(SolverManagerTest, FloatingStateBlock_Add)
diff --git a/test/yaml/solver_ceres.yaml b/test/yaml/solver_ceres.yaml
index a95aa0fc442083b0644cc3220dc554063d73dfb0..30a2ffca95a71ddd7b681e6a2802d280235c7ee2 100644
--- a/test/yaml/solver_ceres.yaml
+++ b/test/yaml/solver_ceres.yaml
@@ -1,3 +1,5 @@
+type: SolverCeres
+plugin: core
period: 1
verbose: 2
interrupt_on_problem_change: false
@@ -6,8 +8,8 @@ n_threads: 2
parameter_tolerance: 1e-10
function_tolerance: 1e-10
gradient_tolerance: 1e-10
-minimizer: "LEVENBERG_MARQUARDT"
-use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG
+minimizer: "levenberg_marquardt"
+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
diff --git a/test/yaml/solver_dummy.yaml b/test/yaml/solver_dummy.yaml
index b8ba9d35719a1447882d86e2ef8381731c825f98..6ca49506e44a91fa4ffe44585bf0e4622a4b9e88 100644
--- a/test/yaml/solver_dummy.yaml
+++ b/test/yaml/solver_dummy.yaml
@@ -1,3 +1,5 @@
+type: SolverDummy
+plugin: core
period: 1
verbose: 2
compute_cov: false
\ No newline at end of file