diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4ab6259469d8e939b44cef15a87adf55f3e032d0..3cd3359159288c7e55baa80a9d601f2f2155989c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -251,6 +251,7 @@ SET(HDRS_UTILS
include/${PROJECT_NAME}/utils/loader.h
include/${PROJECT_NAME}/utils/logging.h
include/${PROJECT_NAME}/utils/singleton.h
+ include/${PROJECT_NAME}/utils/string_utils.h
include/${PROJECT_NAME}/utils/utils_gtest.h
)
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 4aec632c5dc62d0650607ac8a27167af17e9c4e3..2b68ab2ac01bdc47a239dc2541713840f8b3358d 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -108,9 +108,19 @@ int main()
using namespace wolf;
// Wolf problem and solver
- ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem);
- ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
+ ProblemPtr problem = Problem::create("PO", 2);
+ YAML::Node params_solver;
+ params_solver["period"] = 1;
+ params_solver["verbose"] = 2;
+ params_solver["interrupt_on_problem_change"] = false;
+ params_solver["max_num_iterations"] = 1000;
+ params_solver["n_threads"] = 2;
+ params_solver["function_tolerance"] = 0.000001;
+ params_solver["gradient_tolerance"] = 0.0000000001;
+ params_solver["minimizer"] = "LEVENBERG_MARQUARDT";
+ params_solver["use_nonmonotonic_steps"] = false;
+ params_solver["compute_cov"] = false;
+ SolverCeresPtr ceres = std::static_pointer_cast<SolverCeres>(SolverCeres::create(problem, params_solver));
// sensor odometer 2d
YAML::Node params_sen_odo;
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 3c636d092e98cc7e0021b2b746f1fdd69b83534c..1399367da99229aa5c16f639c8b02292080ff21f 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -125,7 +125,7 @@ int main()
problem->print(4, 0, 1, 0);
// Solver. Configure a Ceres solver
- SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server.getNode()["solver"]);
+ SolverManagerPtr ceres = FactorySolverNode::create("SolverCeres", problem, server.getNode()["solver"],{wolf_path});
// recover sensor pointers and other stuff for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->findSensor("sen odom");
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index e1f9e39fed242bb441c0afee15b44a15fb8a391b..54618a68261c0512d0f300fffd5553f49d1d1d78 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -52,8 +52,6 @@ class SolverCeres : public SolverManager
std::unique_ptr<ceres::Problem> ceres_problem_;
std::unique_ptr<ceres::Covariance> covariance_;
- ParamsCeresPtr params_ceres_;
-
// profiling
unsigned int n_iter_acc_, n_iter_max_;
unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 9be1c0426bb8ef0c018ef9f3f5c8b9872879a5b5..4a40d5a8d149808a2599a69bed37c1d35ccdbd83 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -23,6 +23,7 @@
// Enable project-specific definitions and macros
#include "core/internal/config.h"
#include "core/utils/logging.h"
+#include "core/utils/string_utils.h"
// Folder Registry
#include "core/utils/folder_registry.h"
diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h
index f2a803269964461a23b925ed1aa87a8e89fa0b03..2aa5616c254b1ef27af84c597434853260c2cbb0 100644
--- a/include/core/map/factory_map.h
+++ b/include/core/map/factory_map.h
@@ -51,13 +51,13 @@ namespace wolf
* Map creators have the following API:
*
* \code
- * static MapBasePtr create(const YAML::Node& _params_node);
+ * static MapBasePtr create(const YAML::Node& _params_node, const std::vector<std::string> _schema_folders);
* \endcode
*
* They follow the general implementation shown below:
*
* \code
- * static MapBasePtr create(const YAML::Node& _params_node)
+ * static MapBasePtr create(const YAML::Node& _params_node, const std::vector<std::string> _schema_folders)
* {
* // Do create the Map object --- example: MapGrid
* return map_ptr = std::make_shared<MapGrid>(_params_node);
@@ -71,7 +71,7 @@ namespace wolf
* To create e.g. a MapGrid, you type:
*
* \code
- * auto grid_ptr = FactoryMap::create("MapGrid", _params_node);
+ * auto grid_ptr = FactoryMap::create("MapGrid", _params_node, {});
* \endcode
*
* #### Example 2: registering a map creator into the factory
@@ -95,28 +95,38 @@ namespace wolf
* \endcode
*
* - __Automatic registration__: registration is performed at library level.
- * Put the code at the last line of the map_xxx.cpp file,
+ * Put the code at the end of the map_xxx.cpp file inside wolf namespace:
* \code
- * namespace {
- * const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
- * }
+ * WOLF_REGISTER_MAP(MapGrid);
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
* You are free to comment out these lines and place them wherever you consider it more convenient for your design.
*
*/
-typedef Factory<MapBasePtr, const YAML::Node&> FactoryMap;
+typedef Factory<MapBasePtr, const YAML::Node&, const std::vector<std::string>&> FactoryMapNode;
+
+template <>
+inline std::string FactoryMapNode::getClass() const
+{
+ return "FactoryMapNode";
+}
+
+typedef Factory<MapBasePtr, const std::string&, const std::vector<std::string>&> FactoryMapFile;
template <>
-inline std::string FactoryMap::getClass() const
+inline std::string FactoryMapFile::getClass() const
{
- return "FactoryMap";
+ return "FactoryMapFile";
}
#define WOLF_REGISTER_MAP(MapType) \
namespace \
{ \
- const bool WOLF_UNUSED MapType##Registered = FactoryMap::registerCreator(#MapType, MapType::create); \
+ const bool WOLF_UNUSED MapType##Registered = FactoryMapNode::registerCreator(#MapType, MapType::create); \
+ } \
+ namespace \
+ { \
+ const bool WOLF_UNUSED MapType##RegisteredYaml = FactoryMapFile::registerCreator(#MapType, MapType::create); \
}
} /* namespace wolf */
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index c1fd207c467e098543e6e4d0f13bb4be4f209372..e5533e051f6c059cd68e2a42e8348e48de5b6cae 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -46,38 +46,33 @@ namespace wolf
* In order to use this macro, the derived map class, MapClass,
* must have a constructor available with the API:
*
- * MapClass(const ParamsMapClassPtr _params);
+ * MapClass(const YAML::Node& _params);
*
* We recommend writing one of such constructors in your derived maps.
*/
-#define WOLF_MAP_CREATE(MapClass, ParamsMapClass) \
- static MapBasePtr create(const YAML::Node& _node_input) \
+#define WOLF_MAP_CREATE(MapClass) \
+ static MapBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
{ \
- auto params = std::make_shared<ParamsMapClass>(_node_input); \
- \
- return std::make_shared<MapClass>(params); \
- }
-
-/** \brief base struct for map parameters
- *
- * Derive from this struct to create structs of map parameters.
- */
-struct ParamsMapBase : public ParamsBase
-{
- std::string prefix = "map";
-
- ParamsMapBase(const YAML::Node& _input_node)
- : ParamsBase(_input_node){
-
- };
-
- ~ParamsMapBase() override = default;
-
- std::string print() const override
- {
- return "";
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#MapClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::shared_ptr<MapClass>(new MapClass(_input_node)); \
+ } \
+ static MapBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ if (not server.applySchema(#MapClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ return create(server.getNode(), {}); \
}
-};
// class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
@@ -89,8 +84,8 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
MapBase();
- MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
- WOLF_MAP_CREATE(MapBase, ParamsMapBase);
+ MapBase(const YAML::Node& _params);
+ WOLF_MAP_CREATE(MapBase);
~MapBase() override = default;
bool hasChildren() const override;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 721d964c5e5d0d585b9c7e462ce42d993667b86e..a1d0a53083e2e488143120c46c690a4c4e840d21 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -95,7 +95,7 @@ namespace wolf
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
- if (not server.applySchema(#ProcessorClass + std::to_string(Dim) + "d")) \
+ if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \
{ \
throw std::runtime_error(server.getLog()); \
} \
@@ -106,7 +106,7 @@ namespace wolf
const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- if (not server.applySchema(#ProcessorClass + std::to_string(Dim) + "d")) \
+ if (not server.applySchema(#ProcessorClass + toString(Dim) + "d")) \
{ \
throw std::runtime_error(server.getLog()); \
} \
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 797123a6225e250c1cdf6a4a113982ac96d00389..94ed373177c0340daa7e45d0dfdc3aa0d9dc6cae 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -89,7 +89,7 @@ namespace wolf
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
- if (not server.applySchema(#SensorClass + std::to_string(Dim) + "d")) \
+ if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \
{ \
throw std::runtime_error(server.getLog()); \
} \
@@ -101,7 +101,7 @@ namespace wolf
static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- if (not server.applySchema(#SensorClass + std::to_string(Dim) + "d")) \
+ if (not server.applySchema(#SensorClass + toString(Dim) + "d")) \
{ \
throw std::runtime_error(server.getLog()); \
} \
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index e320d8437367c27bae1650e8502027593e389ab7..7be084b310a86f2eae80562fcad9f0ee76ffff62 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -38,33 +38,34 @@ namespace wolf
*
*/
-typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const YAML::Node&> FactorySolver;
+typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const YAML::Node&, const std::vector<std::string>&>
+ FactorySolverNode;
template <>
-inline std::string FactorySolver::getClass() const
+inline std::string FactorySolverNode::getClass() const
{
- return "FactorySolver";
+ return "FactorySolverNode";
}
typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const std::string&, const std::vector<std::string>&>
- FactorySolverYaml;
+ FactorySolverFile;
template <>
-inline std::string FactorySolverYaml::getClass() const
+inline std::string FactorySolverFile::getClass() const
{
- return "FactorySolverYaml";
+ return "FactorySolverFile";
}
#define WOLF_REGISTER_SOLVER(SolverType) \
namespace \
{ \
const bool WOLF_UNUSED SolverType##Registered = \
- wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); \
+ wolf::FactorySolverNode::registerCreator(#SolverType, SolverType::create); \
} \
namespace \
{ \
const bool WOLF_UNUSED SolverType##RegisteredYaml = \
- wolf::FactorySolverYaml::registerCreator(#SolverType, SolverType::create); \
+ wolf::FactorySolverFile::registerCreator(#SolverType, SolverType::create); \
}
} /* namespace wolf */
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index d77b9dd26a42ff7bd7988260cc408d769819083c..802d1a6997d4a636b503792f49277c39052e197a 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -28,31 +28,32 @@
namespace wolf
{
-typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&> FactoryTreeManager;
+typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&, const std::vector<std::string>&>
+ FactoryTreeManagerNode;
template <>
-inline std::string FactoryTreeManager::getClass() const
+inline std::string FactoryTreeManagerNode::getClass() const
{
- return "FactoryTreeManager";
+ return "FactoryTreeManagerNode";
}
typedef Factory<std::shared_ptr<TreeManagerBase>, const std::string&, const std::vector<std::string>&>
- FactoryTreeManagerYaml;
+ FactoryTreeManagerFile;
template <>
-inline std::string FactoryTreeManagerYaml::getClass() const
+inline std::string FactoryTreeManagerFile::getClass() const
{
- return "FactoryTreeManagerYaml";
+ return "FactoryTreeManagerFile";
}
#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
namespace \
{ \
const bool WOLF_UNUSED TreeManagerType##Registered = \
- wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); \
+ wolf::FactoryTreeManagerNode::registerCreator(#TreeManagerType, TreeManagerType::create); \
} \
namespace \
{ \
const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \
- wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); \
+ wolf::FactoryTreeManagerFile::registerCreator(#TreeManagerType, TreeManagerType::create); \
}
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index a9fd352984b80cb809692f40045ad108c68f2e59..d6b1881487d4de0162c5585d58f7720a4c7c87dc 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -37,53 +37,41 @@ namespace wolf
* In order to use this macro, the derived processor class, ProcessorClass,
* must have a constructor available with the API:
*
- * TreeManagerClass(const ParamsTreeManagerPtr _params);
+ * TreeManagerClass(const YAML::Node& _params);
*
* Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications
* of the user input yaml file.
*/
-#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
- static TreeManagerBasePtr create(const YAML::Node& _input_node) \
+#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass) \
+ static TreeManagerBasePtr create(const YAML::Node& _input_node, \
+ const std::vector<std::string>& _folders_schema = {}) \
{ \
- auto params = std::make_shared<ParamsTreeManagerClass>(_input_node); \
- \
- return std::make_shared<TreeManagerClass>(params); \
+ if (not _folders_schema.empty()) \
+ { \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
+ server.setYaml(_input_node); \
+ if (not server.applySchema(#TreeManagerClass)) \
+ { \
+ throw std::runtime_error(server.getLog()); \
+ } \
+ } \
+ return std::shared_ptr<TreeManagerClass>(new TreeManagerClass(_input_node)); \
} \
static TreeManagerBasePtr create(const std::string& _yaml_filepath, \
const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
- \
if (not server.applySchema(#TreeManagerClass)) \
{ \
- WOLF_ERROR(server.getLog()); \
- return nullptr; \
+ throw std::runtime_error(server.getLog()); \
} \
- auto params = std::make_shared<ParamsTreeManagerClass>(server.getNode()); \
- \
- return std::make_shared<TreeManagerClass>(params); \
- }
-
-struct ParamsTreeManagerBase : public ParamsBase
-{
- ParamsTreeManagerBase() = default;
- ParamsTreeManagerBase(const YAML::Node& _input_node) : ParamsBase(_input_node) {}
-
- ~ParamsTreeManagerBase() override = default;
-
- std::string print() const override
- {
- return "";
+ return create(server.getNode(), {}); \
}
-};
class TreeManagerBase : public NodeBase
{
public:
- TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params)
- : NodeBase("TREE_MANAGER", _type), params_(_params)
- {
- }
+ TreeManagerBase(const std::string& _type, const YAML::Node& _params) : NodeBase("TREE_MANAGER", _type) {}
virtual ~TreeManagerBase() {}
@@ -93,9 +81,6 @@ class TreeManagerBase : public NodeBase
{
return true;
};
-
- protected:
- ParamsTreeManagerBasePtr params_;
};
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 7e86ed37836b886c43ca20390a5cfaeb8bf91bea..377230c025853387691f0779d119d27df9ae9d71 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -24,47 +24,23 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
-struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
-{
- ParamsTreeManagerSlidingWindow() = default;
- ParamsTreeManagerSlidingWindow(const YAML::Node& _node_input) : ParamsTreeManagerBase(_node_input)
- {
- n_frames = _node_input["n_frames"].as<unsigned int>();
- n_fix_first_frames = _node_input["n_fix_first_frames"].as<unsigned int>();
- viral_remove_parent_without_children = _node_input["viral_remove_parent_without_children"].as<bool>();
- if (n_frames <= n_fix_first_frames)
- throw std::runtime_error(
- "TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than "
- "'n_frames'!");
- }
- std::string print() const override
- {
- return ParamsTreeManagerBase::print() + "\n" + "n_frames: " + toString(n_frames) + "\n" +
- "n_fix_first_frames: " + toString(n_fix_first_frames) + "\n" +
- "viral_remove_parent_without_children: " + toString(viral_remove_parent_without_children) + "\n";
- }
-
- unsigned int n_frames;
- unsigned int n_fix_first_frames;
- bool viral_remove_parent_without_children;
-};
-
class TreeManagerSlidingWindow : public TreeManagerBase
{
public:
- TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params)
- : TreeManagerBase("TreeManagerSlidingWindow", _params), params_sw_(_params){};
- WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
+ TreeManagerSlidingWindow(const YAML::Node& _params);
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow)
~TreeManagerSlidingWindow() override {}
void keyFrameCallback(FrameBasePtr _frame) override;
protected:
- ParamsTreeManagerSlidingWindowPtr params_sw_;
+ // PARAMS
+ unsigned int n_frames_;
+ unsigned int n_fix_first_frames_;
+ bool viral_remove_parent_without_children_;
};
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
index aa1fa012304519a49404a053e4f258bd9daa7958..9b95310e4382bf5d8e8197a8ba4b21bfc4639f99 100644
--- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -24,46 +24,25 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
-struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
-{
- ParamsTreeManagerSlidingWindowDualRate() = default;
- ParamsTreeManagerSlidingWindowDualRate(const YAML::Node& _input_node) : ParamsTreeManagerSlidingWindow(_input_node)
- {
- n_frames_recent = _input_node["n_frames_recent"].as<unsigned int>();
- rate_old_frames = _input_node["rate_old_frames"].as<unsigned int>();
- if (n_frames_recent >= n_frames)
- {
- throw std::runtime_error(
- "ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'");
- }
- }
- std::string print() const override
- {
- return ParamsTreeManagerBase::print() + "\n" + "n_frames_recent: " + toString(n_frames_recent) + "\n" +
- "rate_old_frames: " + toString(rate_old_frames) + "\n";
- }
-
- unsigned int n_frames_recent, rate_old_frames;
-};
-
class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
{
public:
- TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
+ TreeManagerSlidingWindowDualRate(const YAML::Node& _params);
- WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate)
~TreeManagerSlidingWindowDualRate() override {}
void keyFrameCallback(FrameBasePtr _frame) override;
protected:
- ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
- unsigned int count_frames_;
- // TrajectoryIter first_recent_frame_it_;
+ unsigned int count_frames_; ///< counter of frames
+
+ // PARAMS
+ unsigned int n_frames_recent_;
+ unsigned int rate_old_frames_;
};
} /* namespace wolf */
diff --git a/include/core/utils/string_utils.h b/include/core/utils/string_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..66d397985b842f9ba2e06da37412d8a6a0d7a85d
--- /dev/null
+++ b/include/core/utils/string_utils.h
@@ -0,0 +1,120 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <ctime>
+#include <iostream>
+#include <string>
+#include <sstream>
+
+namespace wolf
+{
+
+// date now in string
+std::string dateTimeNow();
+
+// to string
+std::string toString(bool _arg);
+std::string toString(int _arg);
+std::string toString(long _arg);
+std::string toString(long long _arg);
+std::string toString(unsigned _arg);
+std::string toString(unsigned long _arg);
+std::string toString(unsigned long long _arg);
+std::string toString(float _arg);
+std::string toString(double _arg);
+std::string toString(long double _arg);
+
+template<typename Derived>
+std::string toString(const Eigen::DenseBase<Derived>& mat)
+{
+ std::stringstream ss;
+ if (mat.cols() == 1)
+ ss << mat.transpose();
+ else
+ ss << std::endl << mat;
+ return ss.str();
+}
+
+inline std::string toString(bool _arg)
+{
+ return (_arg ? "true" : "false");
+}
+
+inline std::string toString(int _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(long _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(long long _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(unsigned _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(unsigned long _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(unsigned long long _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(float _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(double _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string toString(long double _arg)
+{
+ return std::to_string(_arg);
+}
+
+inline std::string dateTimeNow()
+{
+ // Get date and time for archiving purposes
+ std::time_t rawtime;
+ std::time(&rawtime);
+ const std::tm* timeinfo = std::localtime(&rawtime);
+ char time_char[30];
+ std::strftime(time_char, sizeof(time_char), "%d/%m/%Y at %H:%M:%S", timeinfo);
+ std::string date_time(time_char);
+ return date_time;
+}
+
+} // namespace wolf
diff --git a/schema/map/MapBase.schema b/schema/map/MapBase.schema
index bf5f648e68b1b63eb0a4d67ac26ed99c7aa8dc0e..c78f4158bfe0c1c114179b48d244012829d69df0 100644
--- a/schema/map/MapBase.schema
+++ b/schema/map/MapBase.schema
@@ -5,15 +5,9 @@ type:
_doc: Type of the Map used in the problem.
plugin:
- _mandatory: false
+ _mandatory: true
_type: string
- _default: core
_doc: Name of the wolf plugin where the map type is implemented.
-
-filename:
- _mandatory: false
- _type: string
- _doc: Optional. Absolute path of the YAML file containing the landmarks.
landmarks:
_mandatory: false
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 42af536adc8024fa3f556c8c9df67650350d60cf..12658d3ab25cbb5805d9795c4a15790a31fc5671 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -306,7 +306,7 @@ void CaptureBase::printHeader(int _depth,
else
_stream << " -> Sen-";
- _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ _stream << ((_depth < 3) ? " -- " + toString(getFeatureList().size()) + "f" : "");
if (_factored_by)
{
_stream << " <-- ";
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 9ba0776f367190e8911438097989e75bf931e02a..c153969ae757639b75ff74718a593a07417aa8d3 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -105,7 +105,7 @@ void CaptureMotion::printHeader(int _depth,
if (auto OF = OC->getFrame()) _stream << " ; Frm" << OF->id();
}
- _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ _stream << ((_depth < 3) ? " -- " + toString(getFeatureList().size()) + "f" : "");
if (_factored_by)
{
_stream << " <-- ";
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 37e525cb373737d7343476a7120773ee67d6ace8..c75b397249b115f51485abbedfb6c95b32807b4d 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -29,13 +29,9 @@
namespace wolf
{
-SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) : SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>())
-{
-}
-SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const ParamsCeresPtr& _params)
+SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params)
: SolverManager(_wolf_problem, _params),
- params_ceres_(_params),
n_iter_acc_(0),
n_iter_max_(0),
n_convergence_(0),
@@ -908,7 +904,7 @@ bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr&
void SolverCeres::printProfilingDerived(std::ostream& _stream) const
{
_stream << "Iterations:"
- << "\n\tUser-defined limit: " << params_ceres_->solver_options_.max_num_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_ << " %"
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index e227b4e68590a66e78d42262a65e2cb67f7ee743..dea59351493161154123f6d16c8c0e70544f50f8 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -175,7 +175,7 @@ void FeatureBase::printHeader(int _depth,
std::string _tabs) const
{
_stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType()
- << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "") << std::endl;
+ << ((_depth < 4) ? " -- " + toString(getFactorList().size()) + "fac " : "") << std::endl;
if (_metric)
_stream << _tabs << " "
<< "m = ( " << std::setprecision(2) << getMeasurement().transpose() << " )" << std::endl;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 75eed4795ead83cc1555522a56e66c4b8c9f6ef1..c363b040ba435c7dda0efb9eff9a92fb56f00686 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -296,7 +296,7 @@ void FrameBase::printHeader(int _depth,
std::string _tabs) const
{
_stream << _tabs << "Frm" << id() << " " << getKeys() << " ts = " << std::setprecision(3) << getTimeStamp()
- << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
+ << ((_depth < 2) ? " -- " + toString(getCaptureList().size()) + "C " : "");
if (_factored_by)
{
_stream << " <-- ";
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index d4a8a45ed943bd61c8ab2d297db6d22293a06949..1dd3cfe0305fa57276688c6a470788ea6e3dfc1f 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -64,7 +64,7 @@ void HardwareBase::printHeader(int _depth,
std::ostream& _stream,
std::string _tabs) const
{
- _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "")
+ _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + toString(getSensorList().size()) + "S") : "")
<< std::endl;
}
void HardwareBase::print(int _depth,
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 7b757f95c0e2dff5ccc0620302a5591d2a8900dd..130f2f220e289f7bf831a7147f5daf6e5f8bc561 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -26,23 +26,22 @@
// YAML
#include <yaml-cpp/yaml.h>
-// stl
-#include <fstream>
-#include <ctime>
-#include <iomanip>
-#include <iostream>
-#include <sstream>
-
namespace wolf
{
-MapBase::MapBase() : NodeBase("MAP", "Base")
+MapBase::MapBase() : NodeBase("MAP", "MapBase")
{
// std::cout << "constructed M"<< std::endl;
}
-MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) : NodeBase("MAP", _type)
+MapBase::MapBase(const YAML::Node& _params) : NodeBase("MAP", _params["type"].as<std::string>())
{
- // std::cout << "constructed M"<< std::endl;
+ if (_params["landmarks"])
+ for (unsigned int i = 0; i < _params["landmarks"].size(); i++)
+ {
+ YAML::Node lmk_node = _params["landmarks"][i];
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
+ lmk_ptr->link(shared_from_this());
+ }
}
bool MapBase::hasChildren() const
@@ -144,7 +143,7 @@ void MapBase::printHeader(int _depth,
std::ostream& _stream,
std::string _tabs) const
{
- _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "")
+ _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + toString(getLandmarkList().size()) + "L") : "")
<< std::endl;
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d06e06e27a39e153fba4d521fcde6c65a9ad489b..95f9ef37ad52a9ec5b07ea54162a4bbdecfa482a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -179,11 +179,11 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node)
std::string tree_manager_type = tree_manager_node["type"].as<std::string>();
if (tree_manager_type != "none" and tree_manager_type != "None" and tree_manager_type != "NONE")
{
- if (not FactoryTreeManager::isCreatorRegistered(tree_manager_type))
+ if (not FactoryTreeManagerNode::isCreatorRegistered(tree_manager_type))
{
problem->loadPlugin(tree_manager_node["plugin"].as<std::string>());
}
- problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
+ problem->setTreeManager(FactoryTreeManagerNode::create(tree_manager_type, tree_manager_node, {}));
}
// First frame
@@ -231,19 +231,14 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node)
// load plugin if map is not registered
YAML::Node map_node = _param_node["map"];
auto map_type = map_node["type"].as<std::string>();
- if (not FactoryMap::isCreatorRegistered(map_type))
+ if (not FactoryMapNode::isCreatorRegistered(map_type))
{
problem->loadPlugin(map_node["plugin"].as<std::string>());
}
WOLF_TRACE("Map Type: ", map_type);
- auto map = FactoryMap::create(map_type, map_node);
+ auto map = FactoryMapNode::create(map_type, map_node, {});
map->setProblem(problem);
problem->setMap(map);
- // load map from file (optional)
- if (map_node["filename"])
- {
- problem->loadMap(map_node["filename"].as<std::string>());
- }
// Done
return problem;
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 44f3cc0f160ab30f21f5300a8d4ddf7024c606de..7dfbc232a9fbceeebfe863fbb0ab8f38cf24e511 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -147,8 +147,8 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
if (_problem == nullptr or _problem == this->getProblem()) return;
if (dim_ != 0 and dim_ != _problem->getDim())
- throw std::runtime_error("Processor works with " + std::to_string(dim_) + "D but problem is " +
- std::to_string(_problem->getDim()) + "D");
+ throw std::runtime_error("Processor works with " + toString(dim_) + "D but problem is " +
+ toString(_problem->getDim()) + "D");
NodeBase::setProblem(_problem);
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 4a1be6f2691a47696529db338230ca64e2466bbc..aa42661a252c37ecd9a5955afcbfac2dc1224b07 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -157,7 +157,7 @@ void TrajectoryBase::printHeader(int _depth,
std::ostream& _stream,
std::string _tabs) const
{
- _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "")
+ _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + toString(getFrameMap().size()) + "F") : "")
<< std::endl;
}
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index cf3d61af8edd503cdccf62d5ec8b86adbb878bb1..e00e3e1f478af5d13c7017a91bdeb7fc81656910 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -22,12 +22,23 @@
namespace wolf
{
+TreeManagerSlidingWindow::TreeManagerSlidingWindow(const YAML::Node& _params)
+ : TreeManagerBase("TreeManagerSlidingWindow", _params)
+{
+ n_frames_ = _params["n_frames"].as<unsigned int>();
+ n_fix_first_frames_ = _params["n_fix_first_frames"].as<unsigned int>();
+ viral_remove_parent_without_children_ = _params["viral_remove_parent_without_children"].as<bool>();
+ if (n_frames_ <= n_fix_first_frames_)
+ throw std::runtime_error(
+ "TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than "
+ "'n_frames'!");
+}
+
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
{
int n_f = getProblem()->getTrajectory()->size();
- bool remove_first = (n_f > params_sw_->n_frames);
- int n_fix = (n_f >= params_sw_->n_frames ? params_sw_->n_fix_first_frames
- : n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames));
+ bool remove_first = (n_f > n_frames_);
+ int n_fix = (n_f >= n_frames_ ? n_fix_first_frames_ : n_f - (n_frames_ - n_fix_first_frames_));
auto frame = (remove_first ? getProblem()->getTrajectory()->getFirstFrame()->getNextFrame()
: getProblem()->getTrajectory()->getFirstFrame());
@@ -50,7 +61,7 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
if (remove_first)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
- getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_parent_without_children);
+ getProblem()->getTrajectory()->getFirstFrame()->remove(viral_remove_parent_without_children_);
}
}
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 3a7cc6491075b5648b2ca4d65e23c07413a11399..4eaa9fb93c9faf5b9f7235e120bd036d6e0cb844 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -24,10 +24,18 @@
namespace wolf
{
-TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params)
- : TreeManagerSlidingWindow(_params), params_swdr_(_params), count_frames_(0)
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(const YAML::Node& _params)
+ : TreeManagerSlidingWindow(_params), count_frames_(0)
{
NodeBase::setType("TreeManagerSlidingWindowDualRate");
+
+ n_frames_recent_ = _params["n_frames_recent"].as<unsigned int>();
+ rate_old_frames_ = _params["rate_old_frames"].as<unsigned int>();
+ if (n_frames_recent_ >= n_frames_)
+ {
+ throw std::runtime_error(
+ "ParamsTreeManagerSlidingWindowDualRate: 'n_frames_recent' should be smaller than 'n_frames'");
+ }
}
void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
@@ -35,14 +43,14 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
int n_f = getProblem()->getTrajectory()->size(); // in trajectory there are only key frames
// recent segment not complete
- if (n_f <= params_swdr_->n_frames_recent) return;
+ if (n_f <= n_frames_recent_) return;
// REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
if (count_frames_ != 0)
{
WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
FrameBasePtr remove_recent_frame =
- getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(params_swdr_->n_frames_recent);
+ getProblem()->getTrajectory()->getLastFrame()->getPreviousFrame(n_frames_recent_);
FrameBasePtr keep_recent_frame = remove_recent_frame->getNextFrame();
// compose motion captures for all processors motion
@@ -73,7 +81,7 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
}
// remove frame
- remove_recent_frame->remove(params_swdr_->viral_remove_parent_without_children);
+ remove_recent_frame->remove(viral_remove_parent_without_children_);
}
// Call tree manager sliding window
// It will remove oldest frame if tfirst recent frame has been kept
@@ -81,7 +89,7 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
// iterate counter
count_frames_++;
- if (count_frames_ == params_swdr_->rate_old_frames) count_frames_ = 0;
+ if (count_frames_ == rate_old_frames_) count_frames_ = 0;
}
// Register in the FactoryTreeManager
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
index 05fa8e86eccc379eb9b81b1064178cb1ef46aab8..448d00138b94e6fdcf74678c077701a1511f09ef 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.cpp
@@ -47,7 +47,7 @@ FactorBasePtrList GraphSearch::computeShortestPath(NodeStateBlocksPtr node1,
//{
// std::string node_neigs_str(depth, '.');
// for (auto node : node_neigs)
- // node_neigs_str += std::to_string(node->id()) + std::string(" ");
+ // node_neigs_str += toString(node->id()) + std::string(" ");
// WOLF_INFO(node_neigs_str);
//}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 091e27122a47e7264a89be5a9f9e856016e3da13..457c8fe19a67549514bd08c17da48bab3d21b07d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,7 +16,7 @@ target_link_libraries(dummy ${PLUGIN_NAME})
# Create a specific test executable for gtest_example #
wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
-# OPTIONAL: if the gtest depends on the dummy library #
+# if the gtest depends on the dummy library #
# (used for implementing pure virtual base classes), #
# add a line #
# target_link_libraries(gtest_example dummy) #
diff --git a/test/dummy/SolverDummy.schema b/test/dummy/SolverDummy.schema
new file mode 100644
index 0000000000000000000000000000000000000000..dba4fce7c0626913aa7ef141253d2b103c591bd3
--- /dev/null
+++ b/test/dummy/SolverDummy.schema
@@ -0,0 +1 @@
+follow: SolverManager.schema
\ No newline at end of file
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_dummy.h
similarity index 94%
rename from test/dummy/solver_manager_dummy.h
rename to test/dummy/solver_dummy.h
index 33f83a029d3a41f2b0cfcf672312fa6b2342e105..895436557a5ec65ef8f93e1bf6585e2dd466c194 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_dummy.h
@@ -24,15 +24,16 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(SolverManagerDummy);
-class SolverManagerDummy : public SolverManager
+WOLF_PTR_TYPEDEFS(SolverDummy);
+class SolverDummy : public SolverManager
{
public:
std::set<FactorBasePtr> factors_derived_;
std::map<StateBlockPtr, bool> state_block_fixed_;
std::map<StateBlockPtr, LocalParametrizationBasePtr> state_block_local_param_;
- SolverManagerDummy(const ProblemPtr& wolf_problem) : SolverManager(wolf_problem){};
+ SolverDummy(const ProblemPtr& wolf_problem, const YAML::Node params) : SolverManager(wolf_problem, params){};
+ WOLF_SOLVER_CREATE(SolverDummy);
bool isStateBlockFixedDerived(const StateBlockPtr& st) override
{
@@ -142,4 +143,5 @@ class SolverManagerDummy : public SolverManager
};
};
+WOLF_REGISTER_SOLVER(SolverDummy)
} // namespace wolf
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index 90aacad7ec072cab30a0d02ce78a203519cbb2d9..ba43cf0dc479981029f467ee88d15cee41f292e7 100644
--- a/test/dummy/tree_manager_dummy.h
+++ b/test/dummy/tree_manager_dummy.h
@@ -24,32 +24,17 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
-struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
-{
- ParamsTreeManagerDummy() = default;
- ParamsTreeManagerDummy(const YAML::Node& _n) : ParamsTreeManagerBase(_n)
- {
- toy_param = _n["toy_param"].as<double>();
- }
-
- ~ParamsTreeManagerDummy() override = default;
-
- bool toy_param;
-
- std::string print() const override
- {
- return ParamsTreeManagerBase::print() + "\n" + "toy_param: " + toString(toy_param) + "\n";
- }
-};
WOLF_PTR_TYPEDEFS(TreeManagerDummy)
class TreeManagerDummy : public TreeManagerBase
{
public:
- TreeManagerDummy(ParamsTreeManagerBasePtr _params) : TreeManagerBase("TreeManagerDummy", _params), n_KF_(0){};
- WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase)
+ TreeManagerDummy(const YAML::Node& _params) : TreeManagerBase("TreeManagerDummy", _params), n_KF_(0)
+ {
+ toy_param_ = _params["toy_param"].as<double>();
+ };
+ WOLF_TREE_MANAGER_CREATE(TreeManagerDummy)
~TreeManagerDummy() override {}
@@ -58,14 +43,9 @@ class TreeManagerDummy : public TreeManagerBase
n_KF_++;
};
- int n_KF_;
+ int n_KF_;
+ double toy_param_;
};
-} /* namespace wolf */
-
-// Register in the FactoryTreeManager
-#include "core/tree_manager/factory_tree_manager.h"
-namespace wolf
-{
WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy)
} /* namespace wolf */
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 5f3b37c73120312cd2ce3e7df5b160a232bcfe09..bc986b63f3112ddf6f4e9eaa93d0163708c42d8f 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -29,14 +29,15 @@ using namespace wolf;
using std::cout;
using std::endl;
-int N = 10;
+int N = 10;
+std::string wolf_dir = _WOLF_CODE_DIR;
-ProblemPtr problem;
-SolverCeresPtr solver;
-FrameBasePtr frm;
-CaptureBasePtr cap;
-VectorComposite pose;
-Matrix9d pose_cov = 1e-3 * Matrix9d::Identity();
+ProblemPtr problem;
+SolverManagerPtr solver;
+FrameBasePtr frm;
+CaptureBasePtr cap;
+VectorComposite pose;
+Matrix9d pose_cov = 1e-3 * Matrix9d::Identity();
void randomSetup(int dim)
{
@@ -46,7 +47,7 @@ void randomSetup(int dim)
// Problem and solver
problem = Problem::create("POV", dim);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// random pose
if (dim == 2)
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index f974f971c781ff2c0f95fc2e62f059e45976318f..79d819a9d438ac9b68b6490e203ca42af2a84952 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -31,15 +31,16 @@
using namespace Eigen;
using namespace wolf;
-const Vector6d zero6 = Vector6d::Zero();
-const Vector3d zero3 = zero6.head<3>();
-const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
+std::string wolf_dir = _WOLF_CODE_DIR;
+const Vector6d zero6 = Vector6d::Zero();
+const Vector3d zero3 = zero6.head<3>();
+const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
class FixtureFactorBlockDifference : public testing::Test
{
public:
ProblemPtr problem_;
- SolverCeresPtr solver_;
+ SolverManagerPtr solver_;
FrameBasePtr KF0_;
FrameBasePtr KF1_;
CaptureBasePtr Cap_;
@@ -51,7 +52,7 @@ class FixtureFactorBlockDifference : public testing::Test
{
// Problem and solver
problem_ = Problem::create("POV", 3);
- solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_ = SolverCeres::create(problem_, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
TimeStamp t0(0);
TimeStamp t1(1);
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index ca2e3c220a4bc6d10cdab7ebae3b79edeef5acc0..5f2da6cf15c983aae5c10a8d530f32a82cdc5117 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -37,7 +37,7 @@ class FactorDiffDriveTest : public testing::Test
{
public:
ProblemPtr problem;
- SolverCeresPtr solver;
+ SolverManagerPtr solver;
TrajectoryBasePtr trajectory;
SensorDiffDrivePtr sensor;
ProcessorDiffDriveMockPtr processor;
@@ -57,8 +57,7 @@ class FactorDiffDriveTest : public testing::Test
{
problem = Problem::create("PO", 2);
trajectory = problem->getTrajectory();
-
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// sensor
sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
@@ -320,8 +319,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
* - The sensor intrinsics are also estimated.
*/
- ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ ProblemPtr problem = Problem::create("PO", 2);
+ SolverManagerPtr solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// sensor
auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
@@ -443,8 +442,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
* - The sensor intrinsics are also estimated.
*/
- ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ ProblemPtr problem = Problem::create("PO", 2);
+ SolverManagerPtr solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// sensor
auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor(
diff --git a/test/gtest_factor_distance_3d.cpp b/test/gtest_factor_distance_3d.cpp
index 109300794585ecc401d3c383e7fe80682ed433db..a8cf2891935e7edb4aa90d0491808753a04f80ff 100644
--- a/test/gtest_factor_distance_3d.cpp
+++ b/test/gtest_factor_distance_3d.cpp
@@ -28,6 +28,7 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_dir = _WOLF_CODE_DIR;
class FactorAutodiffDistance3d_Test : public testing::Test
{
@@ -46,8 +47,8 @@ class FactorAutodiffDistance3d_Test : public testing::Test
Vector1d dist;
Matrix1d dist_cov;
- ProblemPtr problem;
- SolverCeresPtr solver;
+ ProblemPtr problem;
+ SolverManagerPtr solver;
void SetUp() override
{
@@ -66,7 +67,7 @@ class FactorAutodiffDistance3d_Test : public testing::Test
dist_cov = Matrix1d(0.01);
problem = Problem::create("PO", 3);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
F1 = problem->emplaceFrame(1.0, "PO", pose1);
F2 = problem->emplaceFrame(2.0, "PO", pose2);
@@ -97,7 +98,7 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
double res_expected = (measurement - (pos2 - pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0, 0);
double res;
- c2-> operator()(pos1.data(), pos2.data(), &res);
+ c2->operator()(pos1.data(), pos2.data(), &res);
ASSERT_NEAR(res, res_expected, 1e-5);
}
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index eeb08d2db5efce3521e26d574dc293f78c447b93..e0f307ba537ab2d474c0591fe27a0783abc6e905 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -29,14 +29,15 @@
using namespace Eigen;
using namespace wolf;
-int N = 1e2;
+int N = 1e2;
+std::string wolf_dir = _WOLF_CODE_DIR;
// Input odometry data and covariance
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -70,7 +71,7 @@ TEST(FactorPose2d, solve_f)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp
index 05387053815c37c49fb30c8101ed6dced18cb596..1a74f1acf81ac080070c2f312d8e70a649c394a1 100644
--- a/test/gtest_factor_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp
@@ -40,12 +40,12 @@ std::string wolf_dir = _WOLF_CODE_DIR;
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
auto sensor_pose2d =
- problem_ptr -> installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
+ problem_ptr->installSensor("SensorPose2d", wolf_dir + "/test/yaml/sensor_pose_2d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -95,7 +95,7 @@ TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
@@ -144,7 +144,7 @@ TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
sensor_pose2d->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE2d_APPROX(sensor_pose2d->getStateVector("PO"), xs, 1e-6);
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index 82b5b63a5a81560139b8718c2e2f2306491fc8e0..1283b4e9d997f79c36335c09b9816cdb96f9be82 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -31,7 +31,8 @@ using namespace wolf;
using std::cout;
using std::endl;
-int N = 1e2;
+int N = 1e2;
+std::string wolf_dir = _WOLF_CODE_DIR;
Vector7d pose6toPose7(Vector6d _pose6)
{
@@ -42,8 +43,8 @@ Vector7d pose6toPose7(Vector6d _pose6)
Matrix6d data_cov = 0.1 * Matrix6d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -77,7 +78,7 @@ TEST(FactorPose3dWithExtrinsics, solve_f_fix_s)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp
index 536f22a521cbbbfb7c88037e5aeef321abba79da..5312d5a49fd6a13c72d1d406fc6883cbe535c7bb 100644
--- a/test/gtest_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp
@@ -40,12 +40,12 @@ std::string wolf_dir = _WOLF_CODE_DIR;
Matrix6d data_cov = 0.1 * Matrix6d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
auto sensor_pose3d =
- problem_ptr -> installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
+ problem_ptr->installSensor("SensorPose3d", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -95,7 +95,7 @@ TEST(FactorPose3dWithExtrinsics, solve_f_fix_s)
frm->perturb(10);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
@@ -144,7 +144,7 @@ TEST(FactorPose3dWithExtrinsics, fix_f_solve_s)
sensor_pose3d->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
ASSERT_POSE3d_APPROX(sensor_pose3d->getStateVector("PO"), xs, 1e-6);
diff --git a/test/gtest_factor_prior.cpp b/test/gtest_factor_prior.cpp
index 6587d6226f4a2b10f80653f15956e0325cf6c234..f3449dbeced9d862572583581a0e0b45dbad2f97 100644
--- a/test/gtest_factor_prior.cpp
+++ b/test/gtest_factor_prior.cpp
@@ -31,13 +31,14 @@ using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
-Vector3d initial_extrinsics_p((Vector3d() << 1, 2, 3).finished());
-Vector4d initial_extrinsics_o((Vector4d() << 1, 0, 0, 0).finished());
-Vector3d prior_extrinsics_p((Vector3d() << 10, 20, 30).finished());
-Vector4d prior_extrinsics_o((Vector4d() << 0, 1, 0, 0).finished());
-Vector3d prior2_extrinsics_p((Vector3d() << 100, 200, 300).finished());
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+SolverManagerPtr solver = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+
+Vector3d initial_extrinsics_p((Vector3d() << 1, 2, 3).finished());
+Vector4d initial_extrinsics_o((Vector4d() << 1, 0, 0, 0).finished());
+Vector3d prior_extrinsics_p((Vector3d() << 10, 20, 30).finished());
+Vector4d prior_extrinsics_o((Vector4d() << 0, 1, 0, 0).finished());
+Vector3d prior2_extrinsics_p((Vector3d() << 100, 200, 300).finished());
SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom3d",
wolf_dir + "/test/yaml/sensor_odom_3d_gtest_factor_prior.yaml",
@@ -52,7 +53,7 @@ TEST(ParameterPrior, add_prior_p)
sensor_ptr_->emplaceFactorStateBlock('P', prior_extrinsics_p, Matrix3d::Identity());
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), prior_extrinsics_p, 1e-6);
}
@@ -66,7 +67,7 @@ TEST(ParameterPrior, add_prior_o)
sensor_ptr_->emplaceFactorStateBlock('O', prior_extrinsics_o, Matrix3d::Identity());
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_QUATERNION_VECTOR_APPROX(sensor_ptr_->getO()->getState(), prior_extrinsics_o, 1e-6);
}
@@ -86,7 +87,7 @@ TEST(ParameterPrior, prior_p_overwrite)
ASSERT_TRUE(sensor_ptr_->getPriorFactor('P'));
ASSERT_FALSE(sensor_ptr_->getPriorFactorMap().empty());
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(), prior2_extrinsics_p, 1e-6);
}
@@ -97,7 +98,7 @@ TEST(ParameterPrior, prior_p_segment)
sensor_ptr_->emplaceFactorStateBlock('P', prior_extrinsics_p.segment(1, 2), Matrix2d::Identity(), 1);
- std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState().segment(1, 2), prior_extrinsics_p.segment(1, 2), 1e-6);
}
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index 88eb1fd390cb19e1f98e1c9e17baa0172ba4a105..05902b77c3b8568e4d1748d2aa104f545fb80ef3 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -32,6 +32,8 @@ using namespace wolf;
using std::cout;
using std::endl;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
int N = 1e2;
// Vectors
@@ -41,8 +43,8 @@ Vector3d delta, x_0, x_1, x_f, x_l;
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, "PO", Vector3d::Zero());
@@ -63,11 +65,6 @@ FactorRelativePose2dPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -101,11 +98,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -187,7 +179,7 @@ TEST(FactorRelativePose2d, frame_solve_frame1)
frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -207,7 +199,7 @@ TEST(FactorRelativePose2d, frame_solve_frame0)
frm0->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report);
checksFrameFrame();
@@ -228,7 +220,7 @@ TEST(FactorRelativePose2d, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -248,7 +240,7 @@ TEST(FactorRelativePose2d, landmark_solve_frame)
frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_pose_2d_autodiff.cpp b/test/gtest_factor_relative_pose_2d_autodiff.cpp
index 6154a736d5bd685aa944826a1d51e17ab0a6d2dd..9e2900fe85a8960637de8a187b8a7e8c4f7bb164 100644
--- a/test/gtest_factor_relative_pose_2d_autodiff.cpp
+++ b/test/gtest_factor_relative_pose_2d_autodiff.cpp
@@ -30,12 +30,14 @@ using namespace wolf;
using std::cout;
using std::endl;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "PO", Vector3d::Zero());
@@ -81,7 +83,7 @@ TEST(FactorRelativePose2dAutodiff, fix_0_solve)
frm1->perturb(5);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6);
@@ -122,7 +124,7 @@ TEST(FactorRelativePose2dAutodiff, fix_1_solve)
frm0->perturb(5);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6);
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index 94d4d03741f4a4defa44e42890fedbc360d2740b..be8620ebac35a3dfd4394483eb9bbc3ee4bc691a 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -45,11 +45,11 @@ Vector3d delta, x_0, x_1, x_f, x_l, x_s;
Matrix3d data_cov = 1e-3 * Matrix3d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -70,11 +70,6 @@ FactorRelativePose2dWithExtrinsicsPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -115,11 +110,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -218,7 +208,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
frm1->perturb();
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -242,7 +232,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame0)
frm0->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -267,7 +257,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
sensor->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// checksFrameFrame();
}
@@ -290,7 +280,7 @@ TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_o)
sensor->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -315,7 +305,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
lmk->perturb();
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -339,7 +329,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
frm1->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -363,7 +353,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -389,7 +379,7 @@ TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
// std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 7ad85c7096c5ca16ccf9fb8f1a82e1e2435147d5..74dc315287c4dbc3d79b9f377658870b231382f8 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -43,9 +43,8 @@ Matrix6d data_cov = 1e-3 * Matrix6d::Identity();
// Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
@@ -69,11 +68,6 @@ FactorRelativePose3dPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -109,11 +103,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -196,7 +185,7 @@ TEST(FactorRelativePose3d, frame_solve_frame1)
frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -216,7 +205,7 @@ TEST(FactorRelativePose3d, frame_solve_frame0)
frm0->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -237,7 +226,7 @@ TEST(FactorRelativePose3d, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -257,7 +246,7 @@ TEST(FactorRelativePose3d, landmark_solve_frame)
frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 7afafe8d970e5953633f1c5fa1333a1f2cea4939..f6c4c1a67200ac18cd486171308c03209b6b1dba 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -42,12 +42,11 @@ Vector7d delta, x_0, x_1, x_f, x_l, x_s;
Matrix6d data_cov = 1e-3 * Matrix6d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
+auto sensor = problem_ptr->installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -69,11 +68,6 @@ FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -117,11 +111,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -221,7 +210,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -245,7 +234,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
frm0->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -269,7 +258,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
// checksFrameFrame(); // JV: The position extrinsics in case of pair of frames is not observable
@@ -293,7 +282,7 @@ TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
sensor->getO()->perturb(.2);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -318,7 +307,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -342,7 +331,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -366,7 +355,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -390,7 +379,7 @@ TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
sensor->getO()->perturb(.2);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
index 19c65d36849131960f616b04f3bafa1a90c36dcb..2d62a11b9f9feab75d4da47fabbb3fe694e6477e 100644
--- a/test/gtest_factor_relative_position_2d.cpp
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -45,8 +45,8 @@ Vector2d delta, x_l;
Matrix2d data_cov = 0.1 * Matrix2d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -67,11 +67,6 @@ FactorRelativePosition2dPtr fac = nullptr;
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -102,11 +97,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -187,7 +177,7 @@ TEST(FactorRelativePosition2d, frame_solve_frame1_p)
frm1->getP()->perturb();
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -208,7 +198,7 @@ TEST(FactorRelativePosition2d, frame_solve_frame0_p)
frm0->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -229,7 +219,7 @@ TEST(FactorRelativePosition2d, frame_solve_frame0_o)
frm0->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -250,7 +240,7 @@ TEST(FactorRelativePosition2d, landmark_solve_lmk)
lmk->perturb();
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -271,7 +261,7 @@ TEST(FactorRelativePosition2d, landmark_solve_frame_p)
frm1->getP()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -292,7 +282,7 @@ TEST(FactorRelativePosition2d, landmark_solve_frame_o)
frm1->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
index 086376faec4ca414c30bdf9857b92dab844832fc..668ee7267fa77c36130d02384f76dc8c3fe48951 100644
--- a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -45,11 +45,11 @@ Vector2d delta, x_l;
Matrix2d data_cov = 0.1 * Matrix2d::Identity();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 2);
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 2);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
-auto sensor = problem_ptr -> installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
// Frame
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero());
@@ -69,11 +69,6 @@ FactorRelativePosition2dWithExtrinsicsPtr fac = nullptr;
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea) fea->remove(false);
// Random delta
@@ -141,7 +136,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_lmk)
lmk->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -164,7 +159,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_p)
frm->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -187,7 +182,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_o)
frm->getO()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -209,7 +204,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -233,7 +228,7 @@ TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_o_solve)
// std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
index 368f033745acaa40b04237cbcf3939bd7c570226..3b6776e341bf529766d1da80d2810eed2db16f8f 100644
--- a/test/gtest_factor_relative_position_3d.cpp
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -43,9 +43,8 @@ Vector3d delta1, delta2, delta3, x_1, x_2, x_3;
Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Two frames
FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
@@ -79,11 +78,6 @@ FactorRelativePosition3dPtr fac1, fac2, fac3;
////////////////////////////////////////////////////////
void generateRandomProblemFrame()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (cap1)
{
cap1->remove(false);
@@ -147,11 +141,6 @@ void generateRandomProblemFrame()
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (cap1)
{
cap1->remove(false);
@@ -293,7 +282,7 @@ TEST(FactorRelativePosition3d, frame_solve_frame)
frm->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -317,7 +306,7 @@ TEST(FactorRelativePosition3d, frame_solve_frames)
frm3->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameFrame();
@@ -340,7 +329,7 @@ TEST(FactorRelativePosition3d, landmark_solve_frame)
frm->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -364,7 +353,7 @@ TEST(FactorRelativePosition3d, landmark_solve_lmks)
lmk3->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
index b63ebe1895dff57859d37d7eae5423e2df311a8e..3b44c9a915582c82b61f16fa087859b21edbafb5 100644
--- a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -45,9 +45,8 @@ Vector3d delta1, delta2, delta3, x_l1, x_l2, x_l3;
Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal();
// Problem and solver
-ProblemPtr problem_ptr = Problem::create("PO", 3);
-
-SolverCeres solver(problem_ptr);
+auto problem_ptr = Problem::create("PO", 3);
+auto solver_ptr = SolverCeres::create(problem_ptr, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensor
auto sensor = problem_ptr -> installSensor("SensorOdom3d", wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_dir});
@@ -80,11 +79,6 @@ FactorRelativePosition3dWithExtrinsicsPtr fac1, fac2, fac3;
void generateRandomProblemLandmark()
{
- // solver options
- solver.getSolverOptions().max_num_iterations = 100;
- solver.getSolverOptions().gradient_tolerance = 1e-9;
- solver.getSolverOptions().function_tolerance = 1e-9;
-
if (fea1)
{
fea1->remove(false);
@@ -216,7 +210,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_lmks)
lmk3->perturb(1);
// solve for landmark
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -244,7 +238,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_frame)
frm->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -272,7 +266,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_p_solve)
sensor->getP()->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
@@ -300,7 +294,7 @@ TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_o_solve)
sensor->getO()->perturb(.2);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ptr->solve(SolverManager::ReportVerbosity::FULL);
// WOLF_INFO(report);
checksFrameLandmark();
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index 977149e6f139f8a95fad52412673f28dbf2e72a0..81e4f5d150980f0de33b4583c1d55d8d75f6fc04 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/test/gtest_factor_velocity_local_direction_3d.cpp
@@ -28,7 +28,8 @@
using namespace Eigen;
using namespace wolf;
-int N_TESTS = 100;
+int N_TESTS = 100;
+std::string wolf_dir = _WOLF_CODE_DIR;
class FactorVelocityLocalDirection3dTest : public testing::Test
{
@@ -50,9 +51,7 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
problem = Problem::create("POV", 3);
// create solver
- auto params_solver = std::make_shared<ParamsCeres>();
- params_solver->solver_options.max_num_iterations = 1e3;
- solver = std::make_shared<SolverCeres>(problem, params_solver);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Frame
frm = problem->emplaceFrame(0.0, "POV", (Vector10d() << 0, 0, 0, 0, 0, 0, 1, 1, 0, 0).finished());
diff --git a/test/gtest_node_state_blocks.cpp b/test/gtest_node_state_blocks.cpp
index 6ee7208a79c080a4547149fb71fc20b55624ccfa..0f58cadd22c259e16341ccc07ed6af6cfcfe614f 100644
--- a/test/gtest_node_state_blocks.cpp
+++ b/test/gtest_node_state_blocks.cpp
@@ -199,15 +199,15 @@ class NodeStateBlocksDerivedTest : public testing::Test
{
problem = Problem::create("POV", 3);
- sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
+ sbp0 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo0 = std::make_shared<StateQuaternion>();
sbv0 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
sbp1 = std::make_shared<StatePoint3d>(Vector3d::Zero()); // size 3
sbo1 = std::make_shared<StateQuaternion>();
sbv1 = std::make_shared<StateVector3d>(Vector3d::Zero()); // size 3
- F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
- F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+ F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
}
void TearDown() override {}
};
@@ -258,7 +258,7 @@ TEST_F(NodeStateBlocksDerivedTest, Notifications_makeKF_add)
TEST_F(NodeStateBlocksDerivedTest, Add_solve_notify_solve_add)
{
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
Notification n;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index c353ccaddebf009a8602c1c2b76289c1d41c1d6f..dd0fddbc02c857f368de6086f03742d0c38686d1 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -27,7 +27,7 @@
#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
#include "core/capture/capture_diff_drive.h"
@@ -322,9 +322,9 @@ TEST(Problem, StateBlocks)
P->print(4, 1, 1, 1);
// consume notifications
- SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
+ SolverManagerPtr SM = SolverDummy::create(P, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
- SM->update(); // calls P->consumeNotifications();
+ SM->update(); // calls P->consumeNotifications();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),
(SizeStd)(0)); // consumeNotifications empties the notification map
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index a6fa39febbd5adca3371c6402755651a2057852c..ac9903bdd8f54d9dbd19b329e1387b86d7b46b5f 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -49,7 +49,7 @@ class ProcessorFixedWingModelTest : public testing::Test
problem = Problem::create("POV", 3);
// create solver
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Emplace sensor
sensor = problem->installSensor("SensorMotionModel", wolf_dir + "/test/yaml/sensor_pose_3d.yaml", {wolf_dir});
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index c43e75fd1a93a320e42b7f6e1a4816b58e188f52..e4808eb359cf8bafd0b6164cae56867080f50514 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -96,12 +96,12 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
t = TimeStamp(0);
problem = Problem::create("PO", dim);
- solver = std::make_shared<SolverCeres>(problem);
+ solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Sensors
sensor = problem->installSensor("SensorOdom" + toString(dim) + "d",
wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml",
- {wolf_dir});
+ {wolf_dir});
sensor_odom = problem->installSensor("SensorOdom" + toString(dim) + "d",
wolf_dir + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml",
{wolf_dir});
diff --git a/test/gtest_processor_odom_2d.cpp b/test/gtest_processor_odom_2d.cpp
index 65b356bd7eededed4f4defb69a6b6d0c0cc1cee4..85296e9d21df2c2558393e91a481dd812e2c9ae1 100644
--- a/test/gtest_processor_odom_2d.cpp
+++ b/test/gtest_processor_odom_2d.cpp
@@ -92,7 +92,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
// motion data
VectorXd data(Vector2d(1, M_PI_4)); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
- int N = 7; // number of process() steps
+ int N = 7; // number of process() steps
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
@@ -106,7 +106,7 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
// KF - * -- * -- KF - * -- * -- KF - *
// Ceres wrapper
- SolverCeres solver(problem);
+ auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Origin Key Frame
SpecStateComposite prior;
@@ -115,8 +115,8 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
FrameBasePtr KF = problem->setPrior(prior, t0);
processor_odom2d->setOrigin(KF);
- solver.solve(SolverManager::ReportVerbosity::BRIEF);
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
// std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl;
@@ -190,8 +190,8 @@ TEST(ProcessorOdom2d, VoteForKfAndSolve)
}
// Solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(4, 1, 1, 1);
@@ -221,7 +221,7 @@ TEST(ProcessorOdom2d, KF_callback)
double dt = .01;
VectorXd data(Vector2d(1, M_PI_4)); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
- int N = 8; // number of process() steps
+ int N = 8; // number of process() steps
// NOTE: We integrate and create KFs as follows:
// n = 0 1 2 3 4 5 6 7 8
@@ -239,7 +239,7 @@ TEST(ProcessorOdom2d, KF_callback)
"ProcessorOdom2d", sensor_odom2d, wolf_dir + "/test/yaml/processor_odom_2d_inactive.yaml", {wolf_dir}));
// Ceres wrapper
- SolverCeres solver(problem);
+ auto solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// Origin Key Frame
SpecStateComposite prior;
@@ -327,9 +327,9 @@ TEST(ProcessorOdom2d, KF_callback)
MotionBuffer key_buffer_n = key_capture_n->getBuffer();
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// std::cout << report << std::endl;
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO"), integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
@@ -364,8 +364,8 @@ TEST(ProcessorOdom2d, KF_callback)
keyframe_1->setState(Vector3d(2, 3, 1), "PO");
keyframe_2->setState(Vector3d(3, 1, 2), "PO");
- report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
problem->print(4, 1, 1, 1);
diff --git a/test/gtest_processor_pose_3d.cpp b/test/gtest_processor_pose_3d.cpp
index f82e0f284f86c25bf57e73849c5dd4d614226f57..773ae79e1f2c72031eaacf064fb43ae2aae97306 100644
--- a/test/gtest_processor_pose_3d.cpp
+++ b/test/gtest_processor_pose_3d.cpp
@@ -50,12 +50,12 @@ class ProcessorPose3d_Base_Test : public testing::Test
// The problem and Pose Sensor/Processor are implemented
public:
- ProblemPtr problem_;
- SolverCeresPtr solver_;
- SensorBasePtr sensor_pose_;
- FrameBasePtr KF1_;
- FrameBasePtr KF2_;
- FrameBasePtr KF3_;
+ ProblemPtr problem_;
+ SolverManagerPtr solver_;
+ SensorBasePtr sensor_pose_;
+ FrameBasePtr KF1_;
+ FrameBasePtr KF2_;
+ FrameBasePtr KF3_;
Vector7d pose1_;
Vector7d pose2_;
@@ -112,7 +112,7 @@ class ProcessorPose3d_Base_Test : public testing::Test
// Problem and solver_
problem_ = Problem::create("PO", 3);
- solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_ = SolverCeres::create(problem_, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// pose sensor (load yaml and modify to put random extrinsics)
auto params = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_pose_3d_initial_guess.yaml");
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
index 95c428aa8cc0e37e68e2567dac887d0147cb0a44..79dd09f53092c5dca94c52f00500fe5ef230172f 100644
--- a/test/gtest_schema.cpp
+++ b/test/gtest_schema.cpp
@@ -68,15 +68,15 @@ TEST(Schema, check_schema_existence)
// Check that there is an schema file for each of the registered creators of all Factories from yaml nodes/files
// (all except FactoryStateBlock)
- // FactorySolver
- auto registered_solvers = FactorySolver::getRegisteredKeys();
+ // FactorySolverNode
+ auto registered_solvers = FactorySolverNode::getRegisteredKeys();
for (auto key : registered_solvers)
{
EXPECT_TRUE(existsSchema(key));
}
- // FactoryMap
- auto registered_maps = FactoryMap::getRegisteredKeys();
+ // FactoryMapNode
+ auto registered_maps = FactoryMapNode::getRegisteredKeys();
for (auto key : registered_maps)
{
EXPECT_TRUE(existsSchema(key));
@@ -89,15 +89,15 @@ TEST(Schema, check_schema_existence)
EXPECT_TRUE(existsSchema(key));
}
- // FactoryProcessor
+ // FactoryProcessorNode
auto registered_processors = FactoryProcessorNode::getRegisteredKeys();
for (auto key : registered_processors)
{
EXPECT_TRUE(existsSchema(key));
}
- // FactoryTreeManager
- auto registered_tree_managers = FactoryTreeManager::getRegisteredKeys();
+ // FactoryTreeManagerNode
+ auto registered_tree_managers = FactoryTreeManagerNode::getRegisteredKeys();
for (auto key : registered_tree_managers)
{
EXPECT_TRUE(existsSchema(key));
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 32c958a68a2b5dc33775b2295c9b8779584ecadd..6853b1c4658efd28beba1885b5d6e1d6b6a6f41a 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -115,7 +115,7 @@ TEST(SensorBase, sensor_dummy)
WOLF_INFO("---- Sensor ", name, "...");
YAML::Node params;
- params["type"] = "SensorDummy" + std::to_string(dim) + "d";
+ params["type"] = "SensorDummy" + toString(dim) + "d";
params["name"] = name;
params["noise_p_std"] = noise_p_std;
params["noise_o_std"] = noise_o_std;
@@ -198,7 +198,7 @@ TEST(SensorBase, sensor_dummy)
try
{
auto S3 = FactorySensorNode::create(
- "SensorDummy" + std::to_string(dim) + "d", params_node, {wolf_dir});
+ "SensorDummy" + toString(dim) + "d", params_node, {wolf_dir});
// noise
ASSERT_MATRIX_APPROX(S3->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -230,7 +230,7 @@ TEST(SensorBase, sensor_dummy)
try
{
auto S4 = FactorySensorFile::create(
- "SensorDummy" + std::to_string(dim) + "d", yaml_file, {wolf_dir});
+ "SensorDummy" + toString(dim) + "d", yaml_file, {wolf_dir});
// noise
ASSERT_MATRIX_APPROX(S4->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 94aec75efaf2f8a8231d1894b107e6b181c50bd9..0e038e700895fad567a295515e24e625bc259cfa 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -92,7 +92,7 @@ TEST(SensorOdom, creators_and_factories)
std::string name = "sensor_odom_" + toString(dim) + "d";
YAML::Node params;
- params["type"] = "SensorOdom" + std::to_string(dim) + "d";
+ params["type"] = "SensorOdom" + toString(dim) + "d";
params["name"] = name;
params["noise_p_std"] = noise_p_std;
params["noise_o_std"] = noise_o_std;
@@ -134,7 +134,7 @@ TEST(SensorOdom, creators_and_factories)
// factory node ------------------------------------------------------------------------
auto params_node = YAML::LoadFile(wolf_dir + "/test/yaml/" + name + ".yaml");
- auto S3 = FactorySensorNode::create("SensorOdom" + std::to_string(dim) + "d", params_node, {wolf_dir});
+ auto S3 = FactorySensorNode::create("SensorOdom" + toString(dim) + "d", params_node, {wolf_dir});
// checks
checkSensor(S3, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
@@ -142,7 +142,7 @@ TEST(SensorOdom, creators_and_factories)
// factory file ------------------------------------------------------------------------
auto S4 = FactorySensorFile::create(
- "SensorOdom" + std::to_string(dim) + "d", wolf_dir + "/test/yaml/" + name + ".yaml", {wolf_dir});
+ "SensorOdom" + toString(dim) + "d", wolf_dir + "/test/yaml/" + name + ".yaml", {wolf_dir});
// checks
checkSensor(S4, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 7a642d31fbb5194774a61c1fcd5c5a37e977e264..f6231b8a1e49ccb089cf3060830ad48fa2ad0b7c 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -87,7 +87,7 @@ TEST(SensorPose, creators_and_factories)
std::string name = "sensor_pose_" + toString(dim) + "d";
YAML::Node params;
- params["type"] = "SensorPose" + std::to_string(dim) + "d";
+ params["type"] = "SensorPose" + toString(dim) + "d";
params["name"] = name;
params["std_noise"] = dim == 2 ? std_noise_2D : std_noise_3D;
params["states"]["keys"] = "PO";
@@ -126,7 +126,7 @@ TEST(SensorPose, creators_and_factories)
// factory node ------------------------------------------------------------------------
auto params_node = YAML::LoadFile(wolf_dir + "/test/yaml/" + name + ".yaml");
- auto S3 = FactorySensorNode::create("SensorPose" + std::to_string(dim) + "d", params_node, {wolf_dir});
+ auto S3 = FactorySensorNode::create("SensorPose" + toString(dim) + "d", params_node, {wolf_dir});
// checks
checkSensor(S3, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
@@ -134,7 +134,7 @@ TEST(SensorPose, creators_and_factories)
// factory file ------------------------------------------------------------------------
auto S4 = FactorySensorFile::create(
- "SensorPose" + std::to_string(dim) + "d", wolf_dir + "/test/yaml/" + name + ".yaml", {wolf_dir});
+ "SensorPose" + toString(dim) + "d", wolf_dir + "/test/yaml/" + name + ".yaml", {wolf_dir});
// checks
checkSensor(S4, 'P', dim == 2 ? p_state_2D : p_state_3D, true, vector0, false, vector0);
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index ac48b773da7371e7d14396fa866835b0603b8d04..95da75d48493337c936f94a5330eb2c8ac6bf2c0 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -47,6 +47,24 @@ using namespace Eigen;
* (modifications should be applied to both tests)
*/
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+class SolverCeresTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SolverManagerPtr solver_ptr;
+
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 3);
+ solver_ptr =
+ FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
+ }
+};
+
NodeStateBlocksPtr createStateBlock()
{
auto node = std::make_shared<NodeStateBlocksDummy>();
@@ -65,13 +83,44 @@ FactorBasePtr createFactor(NodeStateBlocksPtr node_ptr)
nullptr,
false);
}
-TEST(SolverCeres, Create)
+
+TEST(SolverCeresTestFactories, FactoryNode)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ auto problem = Problem::create("PO", 3);
+ YAML::Node params;
+ 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["function_tolerance"] = 0.000001;
+ params["gradient_tolerance"] = 0.0000000001;
+ 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 = FactorySolverNode::create("SolverCeres", problem, params, {wolf_dir});
// check pointers
- EXPECT_EQ(P, solver_ptr->getProblem());
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeresTestFactories, FactoryFile)
+{
+ auto problem = Problem::create("PO", 3);
+ auto solver_ptr =
+ FactorySolverFile::create("SolverCeres", 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());
@@ -79,30 +128,27 @@ TEST(SolverCeres, Create)
////////////////////////////////////////////////////////
// FLOATING STATE BLOCKS
-TEST(SolverCeres, FloatingStateBlock_Add)
+TEST_F(SolverCeresTest, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -110,45 +156,42 @@ TEST(SolverCeres, FloatingStateBlock_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
+TEST_F(SolverCeresTest, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -156,23 +199,20 @@ TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddFix)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -183,8 +223,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -200,8 +240,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
sb_ptr->fix();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -212,8 +252,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -226,23 +266,20 @@ TEST(SolverCeres, FloatingStateBlock_AddFix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddFixed)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -261,8 +298,8 @@ TEST(SolverCeres, FloatingStateBlock_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -276,23 +313,20 @@ TEST(SolverCeres, FloatingStateBlock_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -313,16 +347,16 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -337,11 +371,8 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -356,20 +387,20 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
@@ -391,8 +422,8 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
@@ -401,179 +432,164 @@ TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_Remove)
+TEST_F(SolverCeresTest, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddRemove)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
+ problem->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
+TEST_F(SolverCeresTest, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FloatingStateBlock_AddUpdated)
+TEST_F(SolverCeresTest, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -591,13 +607,13 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
@@ -610,11 +626,12 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -622,42 +639,39 @@ TEST(SolverCeres, FloatingStateBlock_AddUpdated)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
0); // After solver_manager->update, notifications should be empty
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// STATE BLOCKS AND FACTOR
-TEST(SolverCeres, StateBlockAndFactor_Add)
+TEST_F(SolverCeresTest, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -666,13 +680,13 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock (floating for the moment)
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
@@ -682,19 +696,19 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
// notify factor (state block now not floating)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -703,11 +717,8 @@ TEST(SolverCeres, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
+TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -716,16 +727,16 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -733,27 +744,27 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -762,11 +773,8 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_Fix)
+TEST_F(SolverCeresTest, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -775,16 +783,16 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -796,8 +804,8 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -817,15 +825,15 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -837,11 +845,8 @@ TEST(SolverCeres, StateBlockAndFactor_Fix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddFixed)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -850,16 +855,16 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -879,8 +884,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -893,11 +898,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
+TEST_F(SolverCeresTest, StateBlockAndFactor_UpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -906,16 +908,16 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -939,8 +941,8 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -954,11 +956,8 @@ TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -976,24 +975,24 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check solver
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1023,11 +1022,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1036,40 +1032,40 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1077,11 +1073,8 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
+TEST_F(SolverCeresTest, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1090,39 +1083,39 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
+ problem->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1131,11 +1124,8 @@ TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1144,24 +1134,24 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -1169,8 +1159,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
// notification added)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1179,11 +1169,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1192,32 +1179,32 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1226,11 +1213,8 @@ TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1239,49 +1223,49 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // factor ADD is posponed
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update(); // repeated REMOVE should be ignored
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1290,11 +1274,8 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
+TEST_F(SolverCeresTest, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1303,39 +1284,39 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
// update solver
@@ -1348,11 +1329,8 @@ TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+TEST_F(SolverCeresTest, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1373,16 +1351,16 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// == Insert OTHER notifications ==
@@ -1395,11 +1373,12 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -1407,39 +1386,36 @@ TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
0); // After solver_manager->update, notifications should be empty
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// ONLY FACTOR
-TEST(SolverCeres, OnlyFactor_Add)
+TEST_F(SolverCeresTest, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1448,20 +1424,20 @@ TEST(SolverCeres, OnlyFactor_Add)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// checks
@@ -1470,11 +1446,8 @@ TEST(SolverCeres, OnlyFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, OnlyFactor_Remove)
+TEST_F(SolverCeresTest, OnlyFactor_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1483,35 +1456,35 @@ TEST(SolverCeres, OnlyFactor_Remove)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1519,11 +1492,8 @@ TEST(SolverCeres, OnlyFactor_Remove)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, OnlyFactor_AddRemove)
+TEST_F(SolverCeresTest, OnlyFactor_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1532,27 +1502,27 @@ TEST(SolverCeres, OnlyFactor_AddRemove)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index 8d670a40a8656f5413db56d33f26c9d782670c3a..360548f06683ee71cf3f5b15250c55726beed12a 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -46,11 +46,14 @@ using namespace Eigen;
* (modifications should be applied to both tests)
*/
+std::string wolf_dir = _WOLF_CODE_DIR;
+
TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
{
- double Dt = 5.0;
- ProblemPtr P = Problem::create("PO", 2);
- auto solver_ptr = std::make_shared<SolverCeres>(P);
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr =
+ FactorySolverFile::create("SolverCeres", P, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// loop updating (without sleep)
std::thread t([&]() {
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 60f18993d53ca1878df60cf80047fb22fd224dc7..55d4ae764c2bbb687bd2ccf19ce2a5bada6eae71 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -28,7 +28,7 @@
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/node_state_blocks_dummy.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h"
@@ -43,6 +43,24 @@ using namespace Eigen;
* (modifications should be applied to both tests)
*/
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+class SolverManagerTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SolverManagerPtr solver_ptr;
+
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 3);
+ solver_ptr =
+ FactorySolverFile::create("SolverDummy", problem, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
+ }
+};
+
NodeStateBlocksPtr createStateBlock()
{
auto node = std::make_shared<NodeStateBlocksDummy>();
@@ -62,13 +80,30 @@ FactorBasePtr createFactor(NodeStateBlocksPtr node_ptr)
false);
}
-TEST(SolverManager, Create)
+TEST(SolverManagerFactories, FactoryNode)
+{
+ auto problem = Problem::create("PO", 3);
+ YAML::Node params;
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ auto solver_ptr = FactorySolverNode::create("SolverDummy", problem, params, {wolf_dir});
+
+ // check pointers
+ EXPECT_EQ(problem, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManagerFactories, FactoryFile)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+ auto problem = Problem::create("PO", 3);
+ auto solver_ptr =
+ FactorySolverFile::create("SolverDummy", problem, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
// check pointers
- EXPECT_EQ(P, solver_ptr->getProblem());
+ EXPECT_EQ(problem, solver_ptr->getProblem());
// run solver check
EXPECT_TRUE(solver_ptr->check());
@@ -76,30 +111,28 @@ TEST(SolverManager, Create)
////////////////////////////////////////////////////////
// FLOATING STATE BLOCKS
-TEST(SolverManager, FloatingStateBlock_Add)
+TEST_F(SolverManagerTest, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
+ WOLF_INFO(solver_ptr->getPeriod())
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -107,45 +140,42 @@ TEST(SolverManager, FloatingStateBlock_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_DoubleAdd)
+TEST_F(SolverManagerTest, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -153,23 +183,20 @@ TEST(SolverManager, FloatingStateBlock_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddFix)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -180,8 +207,8 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -197,8 +224,8 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
sb_ptr->fix();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -209,8 +236,8 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -223,23 +250,20 @@ TEST(SolverManager, FloatingStateBlock_AddFix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddFixed)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -258,8 +282,8 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -273,23 +297,20 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -310,16 +331,16 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -334,11 +355,8 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -353,20 +371,20 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr, local_ptr));
@@ -388,8 +406,8 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock localparam
EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
@@ -398,179 +416,164 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_Remove)
+TEST_F(SolverManagerTest, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddRemove)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // should cancel out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancel out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // should cancell out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
+ problem->notifyStateBlock(sb_ptr, ADD); // again ADD in notification list
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check state block
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_DoubleRemove)
+TEST_F(SolverManagerTest, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// update solver
solver_ptr->update();
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddUpdated)
+TEST_F(SolverManagerTest, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -588,13 +591,13 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
@@ -607,11 +610,12 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -619,42 +623,39 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
0); // After solver_manager->update, notifications should be empty
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// STATE BLOCKS AND FACTOR
-TEST(SolverManager, StateBlockAndFactor_Add)
+TEST_F(SolverManagerTest, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -663,13 +664,13 @@ TEST(SolverManager, StateBlockAndFactor_Add)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock (floating for the moment)
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update();
@@ -679,19 +680,19 @@ TEST(SolverManager, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
// notify factor (state block now not floating)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -700,11 +701,8 @@ TEST(SolverManager, StateBlockAndFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
+TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -713,16 +711,16 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -730,27 +728,27 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// notify stateblock again
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check stateblock
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -759,11 +757,8 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_Fix)
+TEST_F(SolverManagerTest, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -772,16 +767,16 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -793,8 +788,8 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -814,15 +809,15 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -834,11 +829,8 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddFixed)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -847,16 +839,16 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -876,8 +868,8 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -890,11 +882,8 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+TEST_F(SolverManagerTest, StateBlockAndFactor_UpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -903,16 +892,16 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check flags
@@ -936,8 +925,8 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -951,11 +940,8 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -973,24 +959,24 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// check solver
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1020,11 +1006,8 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1033,40 +1016,40 @@ TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1074,11 +1057,8 @@ TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
+TEST_F(SolverManagerTest, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1087,39 +1067,39 @@ TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
+ problem->notifyFactor(fac_ptr, REMOVE); // state block should be automatically stored as floating
// check notifications
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// update solver
solver_ptr->update();
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1128,11 +1108,8 @@ TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1141,24 +1118,24 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
@@ -1166,8 +1143,8 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
// notification added)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1176,11 +1153,8 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1189,32 +1163,32 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD notification
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1223,11 +1197,8 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1236,49 +1207,49 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
+ problem->notifyStateBlock(sb_ptr, REMOVE); // cancells out the ADD
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // factor ADD is posponed
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
// check notifications
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(REMOVE, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver manager
solver_ptr->update(); // repeated REMOVE should be ignored
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// check state block
@@ -1287,11 +1258,8 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+TEST_F(SolverManagerTest, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1300,39 +1268,39 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
auto fac_ptr = createFactor(node_ptr);
// notify stateblock
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
+ problem->notifyFactor(fac_ptr, REMOVE); // cancells out ADD
// check notifications
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // state block should be automatically stored as floating
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(REMOVE, notif);
// update solver
@@ -1345,11 +1313,8 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
+TEST_F(SolverManagerTest, StateBlockAndFactor_AddUpdatedStateBlock)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1370,16 +1335,16 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an ADD is notified: a ADD notification should be stored ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// notify factor
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(ADD, notif);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// == Insert OTHER notifications ==
@@ -1392,11 +1357,12 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// Check flag has been set true
EXPECT_TRUE(sb_ptr->fixUpdated());
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
+ 1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ solver_ptr->update(); // it calls problem->consumeStateBlockNotificationMap();
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -1404,39 +1370,36 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
- P->notifyStateBlock(sb_ptr, REMOVE);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 1);
- EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 1);
+ EXPECT_TRUE(problem->getStateBlockNotification(sb_ptr, notif));
EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
// == UPDATES should clear the list of notifications ==
// notify state block
- P->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, ADD);
// update solver
solver_ptr->update();
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(),
0); // After solver_manager->update, notifications should be empty
// == ADD + REMOVE: notification map should be empty ==
- P->notifyStateBlock(sb_ptr, ADD);
- P->notifyStateBlock(sb_ptr, REMOVE);
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ problem->notifyStateBlock(sb_ptr, ADD);
+ problem->notifyStateBlock(sb_ptr, REMOVE);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
}
////////////////////////////////////////////////////////
// ONLY FACTOR
-TEST(SolverManager, OnlyFactor_Add)
+TEST_F(SolverManagerTest, OnlyFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1445,20 +1408,20 @@ TEST(SolverManager, OnlyFactor_Add)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// checks
@@ -1467,11 +1430,8 @@ TEST(SolverManager, OnlyFactor_Add)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, OnlyFactor_Remove)
+TEST_F(SolverManagerTest, OnlyFactor_Remove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1480,35 +1440,35 @@ TEST(SolverManager, OnlyFactor_Remove)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// update solver
solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
@@ -1516,11 +1476,8 @@ TEST(SolverManager, OnlyFactor_Remove)
EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, OnlyFactor_AddRemove)
+TEST_F(SolverManagerTest, OnlyFactor_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 3);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
-
// Create State block
auto node_ptr = createStateBlock();
auto sb_ptr = node_ptr->getStateBlock('I');
@@ -1529,27 +1486,27 @@ TEST(SolverManager, OnlyFactor_AddRemove)
auto fac_ptr = createFactor(node_ptr);
// notify factor (state block has not been notified)
- P->notifyFactor(fac_ptr, ADD);
+ problem->notifyFactor(fac_ptr, ADD);
// check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_TRUE(problem->getFactorNotification(fac_ptr, notif));
EXPECT_EQ(ADD, notif);
// notify factor
- P->notifyFactor(fac_ptr, REMOVE);
+ problem->notifyFactor(fac_ptr, REMOVE);
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// update solver
solver_ptr->update(); // nothing to update
// check notifications
- EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(problem->getFactorNotificationMapSize(), 0);
// checks
EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index aa8a2380de60aa8f68dfa6b6266a05fd5c6ff7bd..bbd9ae307467d67104599488af502f5b88be3acf 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -27,7 +27,7 @@
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_quaternion.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include <iostream>
#include <thread>
@@ -42,9 +42,13 @@ using namespace Eigen;
TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
{
- double Dt = 5.0;
- ProblemPtr P = Problem::create("PO", 2);
- auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ YAML::Node params;
+ params["period"] = 0;
+ params["verbose"] = 0;
+ params["compute_cov"] = false;
+ auto solver_ptr = FactorySolverNode::create("SolverDummy", P, params, {});
// loop updating (without sleep)
std::thread t([&]() {
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index d5782545551208bb1ce6ee48d1dde46713f03f30..816fa1cf692f60c1f0478cd8cbcfa8fd3ac81cef 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -24,7 +24,7 @@
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
#include "core/solver/solver_manager.h"
-#include "dummy/solver_manager_dummy.h"
+#include "dummy/solver_dummy.h"
#include <iostream>
@@ -33,6 +33,8 @@ using namespace wolf;
/// Set to true if you want debug info
bool debug = false;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
TEST(TrajectoryBase, ClosestKeyFrame)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -50,20 +52,20 @@ TEST(TrajectoryBase, ClosestKeyFrame)
FrameBasePtr F4 = std::make_shared<FrameBase>(
4, P->getFrameTypes(), VectorComposite("PO", {Eigen::Vector2d::Zero(), Eigen::Vector1d::Zero()}));
- FrameBasePtr KF; // closest key-frame queried
+ FrameBasePtr KF; // closest key-frame queried
KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
- ASSERT_EQ(KF->id(), F1->id()); // same id!
+ KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
+ ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
KF = T->closestFrameToTimeStamp(
2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
+ ASSERT_EQ(KF->id(), F2->id()); // same id!
KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
@@ -78,8 +80,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
-
- SolverManagerDummy N(P);
+ SolverManagerPtr N = SolverDummy::create(P, wolf_dir + "/test/yaml/solver_dummy.yaml", {wolf_dir});
// Trajectory status:
// KF1 KF2 F3 frames
@@ -113,7 +114,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(T->getLastFrame()->id(), F2->id());
std::cout << __LINE__ << std::endl;
- N.update();
+ N->update();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),
(SizeStd)0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
@@ -140,7 +141,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ(T->getFrameMap().size(), (SizeStd)0);
std::cout << __LINE__ << std::endl;
- N.update();
+ N->update();
ASSERT_EQ(P->getStateBlockNotificationMapSize(),
(SizeStd)0); // consumeStateBlockNotificationMap was called in update() so it should be empty
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 666fae18bb85167c8b6bfbc3af417baf94a4aa0c..f317a40fc4f22450c78ec71eb6773e7b142e295d 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -29,20 +29,6 @@ using namespace Eigen;
std::string wolf_dir = _WOLF_CODE_DIR;
-TEST(TreeManager, make_shared)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
- auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
-
- P->setTreeManager(GM);
-
- ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
-}
-
TEST(TreeManager, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -52,60 +38,60 @@ TEST(TreeManager, createNode)
WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog());
ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
- auto GM = TreeManagerDummy::create(yaml_server.getNode());
+ auto TM = TreeManagerDummy::create(yaml_server.getNode());
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerDummy::create(wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
+ auto TM = TreeManagerDummy::create(wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
-TEST(TreeManager, FactoryParam)
+TEST(TreeManager, FactoryNode)
{
ProblemPtr P = Problem::create("PO", 2);
auto yaml_server = yaml_schema_cpp::YamlServer({wolf_dir}, wolf_dir + "/test/yaml/tree_manager_dummy.yaml");
ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
- auto GM = FactoryTreeManager::create("TreeManagerDummy", yaml_server.getNode());
+ auto TM = FactoryTreeManagerNode::create("TreeManagerDummy", yaml_server.getNode(), {});
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create(
+ auto TM = FactoryTreeManagerFile::create(
"TreeManagerDummy", wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir});
- ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, autoConf)
@@ -129,20 +115,19 @@ TEST(TreeManager, keyFrameCallback)
{
ProblemPtr P = Problem::create("PO", 3);
- auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
- auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+ auto TMD = std::static_pointer_cast<TreeManagerDummy>(
+ TreeManagerDummy::create(wolf_dir + "/test/yaml/tree_manager_dummy.yaml", {wolf_dir}));
- P->setTreeManager(GM);
+ P->setTreeManager(TMD);
- ASSERT_EQ(GM->n_KF_, 0);
+ ASSERT_EQ(TMD->n_KF_, 0);
Vector7d x;
x << 1, 2, 3, 0, 0, 0, 1;
auto F0 = P->emplaceFrame(0, "PO", x);
P->keyFrameCallback(F0, nullptr);
- ASSERT_EQ(GM->n_KF_, 1);
+ ASSERT_EQ(TMD->n_KF_, 1);
}
int main(int argc, char **argv)
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index e92f2185848ad7b00ff739393f384c4261ec6af3..533634a86cb26c5bb280089f7714747bef0c2afa 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -34,19 +34,6 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-TEST(TreeManagerSlidingWindow, make_shared)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
-
- auto GM = std::make_shared<TreeManagerSlidingWindow>(ParamsGM);
-
- P->setTreeManager(GM);
-
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
TEST(TreeManagerSlidingWindow, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -55,31 +42,31 @@ TEST(TreeManagerSlidingWindow, createNode)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow"));
- auto GM = TreeManagerSlidingWindow::create(server.getNode());
+ auto TM = TreeManagerSlidingWindow::create(server.getNode());
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindow, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
+ auto TM = TreeManagerSlidingWindow::create(wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
-TEST(TreeManager, Factory)
+TEST(TreeManager, FactoryNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -87,29 +74,29 @@ TEST(TreeManager, Factory)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow"));
- auto GM = FactoryTreeManager::create("TreeManagerSlidingWindow", server.getNode());
+ auto TM = FactoryTreeManagerNode::create("TreeManagerSlidingWindow", server.getNode(),{});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- ASSERT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManager, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create(
+ auto TM = FactoryTreeManagerFile::create(
"TreeManagerSlidingWindow", wolf_dir + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindow, autoConf)
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index fb7a1399acf590578c16bd366337aa2b200ad548..16e98ae2475dbec13e801d02b57eb7f823e3ff8e 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -36,19 +36,6 @@ using namespace yaml_schema_cpp;
std::string wolf_dir = _WOLF_CODE_DIR;
-TEST(TreeManagerSlidingWindowDualRate, make_shared)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
-
- auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM);
-
- P->setTreeManager(GM);
-
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
TEST(TreeManagerSlidingWindowDualRate, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -57,32 +44,32 @@ TEST(TreeManagerSlidingWindowDualRate, createNode)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindowDualRate"));
- auto GM = TreeManagerSlidingWindowDualRate::create(server.getNode());
+ auto TM = TreeManagerSlidingWindowDualRate::create(server.getNode());
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindowDualRate, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerSlidingWindowDualRate::create(
+ auto TM = TreeManagerSlidingWindowDualRate::create(
wolf_dir + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml", {wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
-TEST(TreeManagerSlidingWindowDualRate, Factory)
+TEST(TreeManagerSlidingWindowDualRate, FactoryNode)
{
ProblemPtr P = Problem::create("PO", 2);
@@ -90,29 +77,29 @@ TEST(TreeManagerSlidingWindowDualRate, Factory)
ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindowDualRate"));
- auto GM = FactoryTreeManager::create("TreeManagerSlidingWindowDualRate", server.getNode());
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ auto TM = FactoryTreeManagerNode::create("TreeManagerSlidingWindowDualRate", server.getNode(), {});
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindowDualRate, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindowDualRate",
+ auto TM = FactoryTreeManagerFile::create("TreeManagerSlidingWindowDualRate",
wolf_dir + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml",
{wolf_dir});
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(TM) != nullptr);
- P->setTreeManager(GM);
+ P->setTreeManager(TM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ EXPECT_EQ(P->getTreeManager(), TM);
}
TEST(TreeManagerSlidingWindowDualRate, autoConf)
@@ -1149,13 +1136,13 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
ProblemPtr problem =
Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window_dual_rate3.yaml", {wolf_dir});
SolverManagerPtr solver =
- FactorySolverYaml::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
+ FactorySolverFile::create("SolverCeres", problem, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
// BASELINE
ProblemPtr problem_bl =
Problem::autoSetup(wolf_dir + "/test/yaml/problem_sliding_window_dual_rate_baseline.yaml", {wolf_dir});
SolverManagerPtr solver_bl =
- FactorySolverYaml::create("SolverCeres", problem_bl, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
+ FactorySolverFile::create("SolverCeres", problem_bl, wolf_dir + "/test/yaml/solver.yaml", {wolf_dir});
// aux variables
Vector7d data;
diff --git a/test/yaml/solver_ceres.yaml b/test/yaml/solver_ceres.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..53a698323208a61dc63a35084985c13fdcebdd98
--- /dev/null
+++ b/test/yaml/solver_ceres.yaml
@@ -0,0 +1,13 @@
+period: 1
+verbose: 2
+interrupt_on_problem_change: false
+max_num_iterations: 1000
+n_threads: 2
+function_tolerance: 0.000001
+gradient_tolerance: 0.0000000001
+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
+cov_enum: 2 #only if compute_cov
\ No newline at end of file
diff --git a/test/yaml/solver_dummy.yaml b/test/yaml/solver_dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b8ba9d35719a1447882d86e2ef8381731c825f98
--- /dev/null
+++ b/test/yaml/solver_dummy.yaml
@@ -0,0 +1,3 @@
+period: 1
+verbose: 2
+compute_cov: false
\ No newline at end of file