diff --git a/CMakeLists.txt b/CMakeLists.txt
index 575f38472916be64bee076e6a8708a18bf1ba0c5..6f9c6e3e2ca856c90b41f8e45e7981f221e0c440 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -254,11 +254,6 @@ SET(HDRS_FEATURE
include/core/feature/feature_odom_2d.h
include/core/feature/feature_pose.h
)
-SET(HDRS_GRAPH_MANAGER
- include/core/graph_manager/factory_graph_manager.h
- include/core/graph_manager/graph_manager_base.h
- include/core/graph_manager/graph_manager_sliding_window.h
- )
SET(HDRS_LANDMARK
include/core/landmark/landmark_base.h
include/core/landmark/landmark_match.h
@@ -290,7 +285,11 @@ SET(HDRS_SOLVER
include/core/solver/solver_manager.h
include/core/solver/solver_factory.h
)
-
+SET(HDRS_TREE_MANAGER
+ include/core/tree_manager/factory_tree_manager.h
+ include/core/tree_manager/tree_manager_base.h
+ include/core/tree_manager/tree_manager_sliding_window.h
+ )
SET(HDRS_YAML
include/core/yaml/parser_yaml.hpp
include/core/yaml/yaml_conversion.h
@@ -348,9 +347,6 @@ SET(SRCS_FEATURE
src/feature/feature_odom_2d.cpp
src/feature/feature_pose.cpp
)
-SET(SRCS_GRAPH_MANAGER
- src/graph_manager/graph_manager_sliding_window.cpp
- )
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
)
@@ -376,6 +372,9 @@ SET(SRCS_SENSOR
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
+SET(SRCS_TREE_MANAGER
+ src/tree_manager/tree_manager_sliding_window.cpp
+ )
SET(SRCS_YAML
src/yaml/processor_odom_3d_yaml.cpp
src/yaml/sensor_odom_2d_yaml.cpp
@@ -425,7 +424,6 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_FRAME}
- ${SRCS_GRAPH_MANAGER}
${SRCS_HARDWARE}
${SRCS_LANDMARK}
${SRCS_MAP}
@@ -436,6 +434,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
${SRCS_TRAJECTORY}
+ ${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
@@ -486,8 +485,6 @@ INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
INSTALL(FILES ${HDRS_FRAME}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
-INSTALL(FILES ${HDRS_GRAPH_MANAGER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/graph_manager)
INSTALL(FILES ${HDRS_HARDWARE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
INSTALL(FILES ${HDRS_LANDMARK}
@@ -512,6 +509,8 @@ INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
INSTALL(FILES ${HDRS_TRAJECTORY}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
+INSTALL(FILES ${HDRS_TREE_MANAGER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
INSTALL(FILES ${HDRS_UTILS}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
INSTALL(FILES ${HDRS_WRAPPER}
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index ebf11819669dd49a0660b0a318a3a59077aa9bb5..741ad7f155884175aafb1b8183f8c672842abc1e 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -208,9 +208,9 @@ WOLF_PTR_TYPEDEFS(NodeBase);
// Problem
WOLF_PTR_TYPEDEFS(Problem);
-// Graph Manager
-WOLF_PTR_TYPEDEFS(GraphManagerBase);
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsGraphManagerBase);
+// Tree Manager
+WOLF_PTR_TYPEDEFS(TreeManagerBase);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerBase);
// Hardware
WOLF_PTR_TYPEDEFS(HardwareBase);
diff --git a/include/core/graph_manager/factory_graph_manager.h b/include/core/graph_manager/factory_graph_manager.h
deleted file mode 100644
index cb23c03e5b9707f5a5acbf94c29f3f8a1fc0fd35..0000000000000000000000000000000000000000
--- a/include/core/graph_manager/factory_graph_manager.h
+++ /dev/null
@@ -1,61 +0,0 @@
-#ifndef FACTORY_GRAPH_MANAGER_H_
-#define FACTORY_GRAPH_MANAGER_H_
-
-namespace wolf
-{
-class GraphManagerBase;
-struct ParamsGraphManagerBase;
-}
-
-// wolf
-#include "core/common/factory.h"
-
-// std
-
-namespace wolf
-{
-/** \brief GraphManager factory class
- * TODO
- */
-
-// ParamsGraphManager factory
-struct ParamsGraphManagerBase;
-typedef Factory<ParamsGraphManagerBase,
- const std::string&> FactoryParamsGraphManager;
-template<>
-inline std::string FactoryParamsGraphManager::getClass()
-{
- return "FactoryParamsGraphManager";
-}
-
-// GraphManager factory
-typedef Factory<GraphManagerBase,
- const std::string&,
- const ParamsGraphManagerBasePtr> FactoryGraphManager;
-template<>
-inline std::string FactoryGraphManager::getClass()
-{
- return "FactoryGraphManager";
-}
-
-#define WOLF_REGISTER_GRAPH_MANAGER(GraphManagerType, GraphManagerName) \
- namespace{ const bool WOLF_UNUSED GraphManagerName##Registered = \
- wolf::FactoryGraphManager::get().registerCreator(GraphManagerType, GraphManagerName::create); } \
-
-typedef Factory<GraphManagerBase,
- const std::string&,
- const ParamsServer&> AutoConfFactoryGraphManager;
-template<>
-inline std::string AutoConfFactoryGraphManager::getClass()
-{
- return "AutoConfFactoryGraphManager";
-}
-
-
-#define WOLF_REGISTER_GRAPH_MANAGER_AUTO(GraphManagerType, GraphManagerName) \
- namespace{ const bool WOLF_UNUSED GraphManagerName##AutoConfRegistered = \
- wolf::AutoConfFactoryGraphManager::get().registerCreator(GraphManagerType, GraphManagerName::create); } \
-
-} /* namespace wolf */
-
-#endif /* FACTORY_GRAPH_MANAGER_H_ */
diff --git a/include/core/graph_manager/graph_manager_base.h b/include/core/graph_manager/graph_manager_base.h
deleted file mode 100644
index f491acc117bfc4678e9e7f664e27a7607cf775c8..0000000000000000000000000000000000000000
--- a/include/core/graph_manager/graph_manager_base.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#ifndef INCLUDE_GRAPH_MANAGER_BASE_H_
-#define INCLUDE_GRAPH_MANAGER_BASE_H_
-
-// Wolf includes
-#include "core/common/wolf.h"
-#include "core/common/node_base.h"
-#include "core/common/params_base.h"
-
-namespace wolf
-{
-/*
- * Macro for defining Autoconf graph manager creator for WOLF's high level API.
- *
- * Place a call to this macro inside your class declaration (in the graph_manager_class.h file),
- * preferably just after the constructors.
- *
- * In order to use this macro, the derived processor class, ProcessorClass,
- * must have a constructor available with the API:
- *
- * GraphManagerClass(const ParamsGraphManagerPtr _params);
- */
-#define WOLF_GRAPH_MANAGER_CREATE(GraphManagerClass, ParamsGraphManagerClass) \
-static GraphManagerBasePtr create(const std::string& _unique_name, \
- const ParamsServer& _server) \
-{ \
- auto params = std::make_shared<ParamsGraphManagerClass>(_unique_name, _server); \
- \
- auto graph_manager = std::make_shared<GraphManagerClass>(params); \
- \
- graph_manager ->setName(_unique_name); \
- \
- return graph_manager; \
-} \
-static GraphManagerBasePtr create(const std::string& _unique_name, \
- const ParamsGraphManagerBasePtr _params) \
-{ \
- auto params = std::static_pointer_cast<ParamsGraphManagerClass>(_params); \
- \
- auto graph_manager = std::make_shared<GraphManagerClass>(params); \
- \
- graph_manager ->setName(_unique_name); \
- \
- return graph_manager; \
-} \
-
-struct ParamsGraphManagerBase : public ParamsBase
-{
- std::string prefix = "graph_manager/";
- ParamsGraphManagerBase() = default;
- ParamsGraphManagerBase(std::string _unique_name, const ParamsServer& _server):
- ParamsBase(_unique_name, _server)
- {
- toy_param = _server.getParam<double>(prefix + _unique_name + "/toy_param");
- }
-
- virtual ~ParamsGraphManagerBase() = default;
-
- bool toy_param;
-
- std::string print() const
- {
- return ParamsBase::print() + "\n"
- + "toy_param: " + std::to_string(toy_param) + "\n";
- }
-};
-
-class GraphManagerBase : public NodeBase
-{
- public:
- GraphManagerBase(const std::string& _type, ParamsGraphManagerBasePtr _params) :
- NodeBase("GRAPH_MANAGER", _type),
- params_(_params)
- {}
-
- virtual ~GraphManagerBase(){}
-
- virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
-
- protected:
- ParamsGraphManagerBasePtr params_;
-};
-
-} /* namespace wolf */
-
-#endif /* INCLUDE_GRAPH_MANAGER_BASE_H_ */
diff --git a/include/core/graph_manager/graph_manager_sliding_window.h b/include/core/graph_manager/graph_manager_sliding_window.h
deleted file mode 100644
index 09120a75c8438bd7c28671a51092d0cadeb68736..0000000000000000000000000000000000000000
--- a/include/core/graph_manager/graph_manager_sliding_window.h
+++ /dev/null
@@ -1,48 +0,0 @@
-#ifndef INCLUDE_GRAPH_MANAGER_SLIDING_WINDOW_H_
-#define INCLUDE_GRAPH_MANAGER_SLIDING_WINDOW_H_
-
-#include "core/graph_manager/graph_manager_base.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsGraphManagerSlidingWindow)
-WOLF_PTR_TYPEDEFS(GraphManagerSlidingWindow)
-
-struct ParamsGraphManagerSlidingWindow : public ParamsGraphManagerBase
-{
- ParamsGraphManagerSlidingWindow() = default;
- ParamsGraphManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
- ParamsGraphManagerBase(_unique_name, _server)
- {
- window_length = _server.getParam<unsigned int>(prefix + _unique_name + "/window_length");
- }
- std::string print() const
- {
- return "\n" + ParamsGraphManagerBase::print() + "\n"
- + "window_length: " + std::to_string(window_length)+ "\n";
- }
-
- unsigned int window_length;
-};
-
-class GraphManagerSlidingWindow : public GraphManagerBase
-{
- public:
- GraphManagerSlidingWindow(ParamsGraphManagerSlidingWindowPtr _params) :
- GraphManagerBase("GraphManagerSlidingWindow", _params),
- params_sw_(_params)
- {};
- WOLF_GRAPH_MANAGER_CREATE(GraphManagerSlidingWindow, ParamsGraphManagerSlidingWindow)
-
- virtual ~GraphManagerSlidingWindow(){}
-
- virtual void keyFrameCallback(FrameBasePtr _key_frame) override;
-
- protected:
- ParamsGraphManagerSlidingWindowPtr params_sw_;
-};
-
-} /* namespace wolf */
-
-#endif /* INCLUDE_GRAPH_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 319ba9b282b6bf8bdb8ad23d5dd68d3a5f4a85f3..e0463b12f2d09b370375b9aa2bdb57894a134ca2 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -43,7 +43,7 @@ class Problem : public std::enable_shared_from_this<Problem>
friend ProcessorMotion;
protected:
- GraphManagerBasePtr graph_manager_;
+ TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
@@ -75,8 +75,9 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen getDim() const;
std::string getFrameStructure() const;
- // Graph manager --------------------------------------
- void setGraphManager(GraphManagerBasePtr _gm);
+ // Tree manager --------------------------------------
+ void setTreeManager(TreeManagerBasePtr _gm);
+ TreeManagerBasePtr getTreeManager() const;
// Hardware branch ------------------------------------
HardwareBasePtr getHardware() const;
@@ -366,6 +367,11 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
+inline TreeManagerBasePtr Problem::getTreeManager() const
+{
+ return tree_manager_;
+}
+
inline bool Problem::priorIsSet() const
{
return prior_is_set_;
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b421461c4c636a5a87ccc35b87fc83213d5b4d0
--- /dev/null
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -0,0 +1,61 @@
+#ifndef FACTORY_TREE_MANAGER_H_
+#define FACTORY_TREE_MANAGER_H_
+
+namespace wolf
+{
+class TreeManagerBase;
+struct ParamsTreeManagerBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+
+// std
+
+namespace wolf
+{
+/** \brief TreeManager factory class
+ * TODO
+ */
+
+// ParamsTreeManager factory
+struct ParamsTreeManagerBase;
+typedef Factory<ParamsTreeManagerBase,
+ const std::string&> FactoryParamsTreeManager;
+template<>
+inline std::string FactoryParamsTreeManager::getClass()
+{
+ return "FactoryParamsTreeManager";
+}
+
+// TreeManager factory
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsTreeManagerBasePtr> FactoryTreeManager;
+template<>
+inline std::string FactoryTreeManager::getClass()
+{
+ return "FactoryTreeManager";
+}
+
+#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType, TreeManagerName) \
+ namespace{ const bool WOLF_UNUSED TreeManagerName##Registered = \
+ wolf::FactoryTreeManager::get().registerCreator(TreeManagerType, TreeManagerName::create); } \
+
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsServer&> AutoConfFactoryTreeManager;
+template<>
+inline std::string AutoConfFactoryTreeManager::getClass()
+{
+ return "AutoConfFactoryTreeManager";
+}
+
+
+#define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType, TreeManagerName) \
+ namespace{ const bool WOLF_UNUSED TreeManagerName##AutoConfRegistered = \
+ wolf::AutoConfFactoryTreeManager::get().registerCreator(TreeManagerType, TreeManagerName::create); } \
+
+} /* namespace wolf */
+
+#endif /* FACTORY_TREE_MANAGER_H_ */
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..8cd42c07e543be9f6f690bf88581ee6b5e481ed0
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -0,0 +1,81 @@
+#ifndef INCLUDE_TREE_MANAGER_BASE_H_
+#define INCLUDE_TREE_MANAGER_BASE_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/common/node_base.h"
+#include "core/common/params_base.h"
+
+namespace wolf
+{
+/*
+ * Macro for defining Autoconf tree manager creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the tree_manager_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived processor class, ProcessorClass,
+ * must have a constructor available with the API:
+ *
+ * TreeManagerClass(const ParamsTreeManagerPtr _params);
+ */
+#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
+static TreeManagerBasePtr create(const std::string& _unique_name, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsTreeManagerClass>(_unique_name, _server); \
+ \
+ auto tree_manager = std::make_shared<TreeManagerClass>(params); \
+ \
+ tree_manager ->setName(_unique_name); \
+ \
+ return tree_manager; \
+} \
+static TreeManagerBasePtr create(const std::string& _unique_name, \
+ const ParamsTreeManagerBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \
+ \
+ auto tree_manager = std::make_shared<TreeManagerClass>(params); \
+ \
+ tree_manager ->setName(_unique_name); \
+ \
+ return tree_manager; \
+} \
+
+struct ParamsTreeManagerBase : public ParamsBase
+{
+ std::string prefix = "problem/tree_manager/";
+ ParamsTreeManagerBase() = default;
+ ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
+ ParamsBase(_unique_name, _server)
+ {
+ }
+
+ virtual ~ParamsTreeManagerBase() = default;
+
+ std::string print() const
+ {
+ return ParamsBase::print() + "\n";
+ }
+};
+
+class TreeManagerBase : public NodeBase
+{
+ public:
+ TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) :
+ NodeBase("TREE_MANAGER", _type),
+ params_(_params)
+ {}
+
+ virtual ~TreeManagerBase(){}
+
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
+
+ protected:
+ ParamsTreeManagerBasePtr params_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_BASE_H_ */
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
new file mode 100644
index 0000000000000000000000000000000000000000..229ff57900a1cc8bbb3ee8e136328175f6375af3
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -0,0 +1,51 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
+
+#include "../tree_manager/tree_manager_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
+
+struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
+{
+ ParamsTreeManagerSlidingWindow() = default;
+ ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerBase(_unique_name, _server)
+ {
+ n_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames");
+ fix_first_key_frame = _server.getParam<bool> (prefix + "/fix_first_key_frame");
+ }
+ std::string print() const
+ {
+ return "\n" + ParamsTreeManagerBase::print() + "\n"
+ + "n_key_frames: " + std::to_string(n_key_frames) + "\n"
+ + "fix_first_key_frame: " + std::to_string(fix_first_key_frame) + "\n";
+ }
+
+ unsigned int n_key_frames;
+ bool fix_first_key_frame;
+};
+
+class TreeManagerSlidingWindow : public TreeManagerBase
+{
+ public:
+ TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) :
+ TreeManagerBase("TreeManagerSlidingWindow", _params),
+ params_sw_(_params)
+ {};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
+
+ virtual ~TreeManagerSlidingWindow(){}
+
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowPtr params_sw_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/src/graph_manager/graph_manager_sliding_window.cpp b/src/graph_manager/graph_manager_sliding_window.cpp
deleted file mode 100644
index d796f906d0bee8ca3105c6a909e18fc5a1d48894..0000000000000000000000000000000000000000
--- a/src/graph_manager/graph_manager_sliding_window.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "core/graph_manager/graph_manager_sliding_window.h"
-
-namespace wolf
-{
-
-void GraphManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
-{
- WOLF_INFO("GraphManagerSlidingWindow: keyFrameCallback!");
-}
-
-} /* namespace wolf */
-
-// Register in the FactoryGraphManager
-#include "core/graph_manager/factory_graph_manager.h"
-namespace wolf {
-WOLF_REGISTER_GRAPH_MANAGER("GraphManagerSlidingWindow", GraphManagerSlidingWindow);
-WOLF_REGISTER_GRAPH_MANAGER_AUTO("GraphManagerSlidingWindow", GraphManagerSlidingWindow);
-} // namespace wolf
-
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index bc92752ebf047071d4b79c9e4dfda860c879b2c2..550118c396c94561ce398d4cc1b9f3b2f8b31ed4 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -11,15 +11,11 @@
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
+#include "core/tree_manager/factory_tree_manager.h"
#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
#include "core/utils/loader.hpp"
#include "core/utils/check_log.hpp"
-#include "core/graph_manager/graph_manager_base.h"
-
-// IRI libs includes
-
-// C++ includes
#include <algorithm>
#include <map>
#include <sstream>
@@ -28,10 +24,13 @@
#include <vector>
#include <unordered_set>
+#include "../../include/core/tree_manager/tree_manager_base.h"
+
namespace wolf
{
Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
+ tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
@@ -150,6 +149,12 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
}
+ // Tree manager
+ std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
+ WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
+ if (tree_manager_type != "None" and tree_manager_type != "none")
+ problem->setTreeManager(AutoConfFactoryTreeManager::get().create(tree_manager_type, "tree manager", _server));
+
// Prior
Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
Eigen::MatrixXd prior_cov = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
@@ -451,12 +456,12 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
-void Problem::setGraphManager(GraphManagerBasePtr _gm)
+void Problem::setTreeManager(TreeManagerBasePtr _gm)
{
- if (graph_manager_)
- graph_manager_->setProblem(nullptr);
- graph_manager_ = _gm;
- graph_manager_->setProblem(shared_from_this());
+ if (tree_manager_)
+ tree_manager_->setProblem(nullptr);
+ tree_manager_ = _gm;
+ tree_manager_->setProblem(shared_from_this());
}
Eigen::VectorXd Problem::zeroState() const
@@ -505,9 +510,9 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
#endif
}
- // notify graph manager
- if (graph_manager_)
- graph_manager_->keyFrameCallback(_keyframe_ptr);
+ // notify tree manager
+ if (tree_manager_)
+ tree_manager_->keyFrameCallback(_keyframe_ptr);
}
bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
@@ -883,6 +888,10 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
if ( !processor->isMotion() )
processor->keyFrameCallback(origin_keyframe, _time_tolerance);
+ // Notify tree manager
+ if (tree_manager_)
+ tree_manager_->keyFrameCallback(origin_keyframe);
+
return origin_keyframe;
}
else
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52d91021af6d492dfc7dea960ee5c2106bfa15c4
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -0,0 +1,41 @@
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
+{
+ WOLF_INFO("TreeManagerSlidingWindow: keyFrameCallback!");
+
+ int n_kf(0);
+ FrameBasePtr first_KF(nullptr), second_KF(nullptr);
+ for (auto frm : getProblem()->getTrajectory()->getFrameList())
+ {
+ if (frm->isKey())
+ {
+ n_kf++;
+ if (first_KF == nullptr)
+ first_KF = frm;
+ else if (second_KF == nullptr)
+ second_KF = frm;
+ }
+ }
+
+ // remove first KF if too many KF
+ if (n_kf > params_sw_->n_key_frames)
+ {
+ first_KF->remove();
+ if (params_sw_->fix_first_key_frame)
+ second_KF->fix();
+ }
+}
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER("TreeManagerSlidingWindow", TreeManagerSlidingWindow);
+WOLF_REGISTER_TREE_MANAGER_AUTO("TreeManagerSlidingWindow", TreeManagerSlidingWindow);
+} // namespace wolf
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4ade74101ff38785e6ea433e13415f9519523ddb..98d3c91cda3f6725fe9c458557ddfb4bd6ec2e9e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -79,10 +79,6 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PROJECT_NAME})
-# FrameBase classes test
-wolf_add_gtest(gtest_graph_manager gtest_graph_manager.cpp)
-target_link_libraries(gtest_graph_manager ${PROJECT_NAME})
-
# HasStateBlocks classes test
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PROJECT_NAME})
@@ -157,6 +153,10 @@ target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
target_link_libraries(gtest_trajectory ${PROJECT_NAME})
+# TreeManager class test
+wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
+target_link_libraries(gtest_tree_manager ${PROJECT_NAME})
+
# ------- Now Derived classes ----------
IF (Ceres_FOUND)
diff --git a/test/dummy/graph_manager_dummy.h b/test/dummy/graph_manager_dummy.h
deleted file mode 100644
index 83989de29ef1cea9145c54f1503e54b897660a28..0000000000000000000000000000000000000000
--- a/test/dummy/graph_manager_dummy.h
+++ /dev/null
@@ -1,33 +0,0 @@
-#ifndef INCLUDE_GRAPH_MANAGER_DUMMY_H_
-#define INCLUDE_GRAPH_MANAGER_DUMMY_H_
-
-#include "core/graph_manager/graph_manager_base.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(GraphManagerDummy)
-
-class GraphManagerDummy : public GraphManagerBase
-{
- public:
- GraphManagerDummy(ParamsGraphManagerBasePtr _params) :
- GraphManagerBase("GraphManagerDummy", _params),
- n_factors_(0),
- n_KF_(0)
- {};
- WOLF_GRAPH_MANAGER_CREATE(GraphManagerDummy, ParamsGraphManagerBase)
-
- virtual ~GraphManagerDummy(){}
-
- virtual void keyFrameCallback(FrameBasePtr _KF) override
- {
- n_KF_++;
- };
-
- int n_factors_, n_KF_;
-};
-
-} /* namespace wolf */
-
-#endif /* INCLUDE_GRAPH_MANAGER_DUMMY_H_ */
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..379fdd23902daaa5e3ba9d29a3f6a445f9e21373
--- /dev/null
+++ b/test/dummy/tree_manager_dummy.h
@@ -0,0 +1,61 @@
+#ifndef INCLUDE_TREE_MANAGER_DUMMY_H_
+#define INCLUDE_TREE_MANAGER_DUMMY_H_
+
+#include "core/tree_manager/tree_manager_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
+struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
+{
+ ParamsTreeManagerDummy() = default;
+ ParamsTreeManagerDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsTreeManagerBase(_unique_name, _server)
+ {
+ toy_param = _server.getParam<double>(prefix + "/toy_param");
+ }
+
+ virtual ~ParamsTreeManagerDummy() = default;
+
+ bool toy_param;
+
+ std::string print() const
+ {
+ return ParamsTreeManagerBase::print() + "\n"
+ + "toy_param: " + std::to_string(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)
+
+ virtual ~TreeManagerDummy(){}
+
+ virtual void keyFrameCallback(FrameBasePtr _KF) override
+ {
+ n_KF_++;
+ };
+
+ int n_KF_;
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER("TreeManagerDummy", TreeManagerDummy)
+WOLF_REGISTER_TREE_MANAGER_AUTO("TreeManagerDummy", TreeManagerDummy)
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_DUMMY_H_ */
diff --git a/test/gtest_graph_manager.cpp b/test/gtest_graph_manager.cpp
deleted file mode 100644
index 9be4c6edfa300c4f19b636a7de8673ba34a40c89..0000000000000000000000000000000000000000
--- a/test/gtest_graph_manager.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
-
-#include "core/problem/problem.h"
-#include "core/graph_manager/graph_manager_base.h"
-#include "dummy/graph_manager_dummy.h"
-#include "core/capture/capture_void.h"
-#include "core/feature/feature_base.h"
-#include "core/factor/factor_pose_2d.h"
-
-using namespace wolf;
-using namespace Eigen;
-
-TEST(GraphManager, keyFrameCallback)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- auto ParamsGM = std::make_shared<ParamsGraphManagerBase>();
-
- auto GM = std::make_shared<GraphManagerDummy>(ParamsGM);
-
- P->setGraphManager(GM);
-
- ASSERT_EQ(GM->n_KF_, 0);
-
- auto F0 = P->emplaceFrame("PO", 2, KEY, VectorXd(3), TimeStamp(0.0));
- P->keyFrameCallback(F0, nullptr, 0);
-
- ASSERT_EQ(GM->n_KF_, 1);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52f86aced4329f61c433942cc25fd06d3cbe6034
--- /dev/null
+++ b/test/gtest_tree_manager.cpp
@@ -0,0 +1,105 @@
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/problem/problem.h"
+#include "dummy/tree_manager_dummy.h"
+#include "core/yaml/parser_yaml.hpp"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_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, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = TreeManagerDummy::create("tree_manager", ParamsGM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerDummy::create("tree_manager", server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, autoConf)
+{
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
+}
+
+TEST(TreeManager, autoConfNone)
+{
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
+}
+
+TEST(TreeManager, keyFrameCallback)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(GM->n_KF_, 0);
+
+ auto F0 = P->emplaceFrame("PO", 3, KEY, VectorXd(7), TimeStamp(0.0));
+ P->keyFrameCallback(F0, nullptr, 0);
+
+ ASSERT_EQ(GM->n_KF_, 1);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c48b993490027545b5184cc6a271a82a53fe37fe
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -0,0 +1,143 @@
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/problem/problem.h"
+#include "core/tree_manager/tree_manager_sliding_window.h"
+#include "core/yaml/parser_yaml.hpp"
+#include"core/capture/capture_void.h"
+#include"core/feature/feature_base.h"
+#include"core/factor/factor_odom_3d.h"
+#include"core/factor/factor_pose_3d.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_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);
+
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
+
+ auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, autoConf)
+{
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+}
+
+TEST(TreeManagerSlidingWindow, slidingWindowFix)
+{
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ // FRAME 1 ----------------------------------------------------------
+ auto F1 = P->getTrajectory()->getLastKeyFrame();
+ ASSERT_TRUE(F1 != nullptr);
+
+ Vector7d state = F1->getState();
+ Vector7d zero_disp(state);
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(1));
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, state);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, zero_disp);
+ auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F2, nullptr, false);
+
+ // Check no frame removed
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 3 ----------------------------------------------------------
+ auto F3 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(1));
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, state);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, zero_disp);
+ auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F3, nullptr, false);
+
+ // Check no frame removed
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 4 ----------------------------------------------------------
+ auto F4 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(1));
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, state);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, zero_disp);
+ auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F4, nullptr, false);
+
+ // Check frame F1 (prior) removed
+ ASSERT_TRUE(F1->isRemoving());
+
+
+ ASSERT_EQ(GM->n_KF_, 1);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..25e8ac8a4417ffec910fee4cac96669a00ebdc4f
--- /dev/null
+++ b/test/yaml/params_tree_manager1.yaml
@@ -0,0 +1,41 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ state: [0,0,0,0,0,0,1,0,0,0]
+ cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ time_tolerance: 0.1
+ timestamp: 0
+ tree_manager:
+ type: "TreeManagerDummy"
+ toy_param: 0
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..913f5875bfd09750e024fcfeab910ff45d6058ee
--- /dev/null
+++ b/test/yaml/params_tree_manager2.yaml
@@ -0,0 +1,40 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ state: [0,0,0,0,0,0,1,0,0,0]
+ cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ time_tolerance: 0.1
+ timestamp: 0
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..db88862872617570a07902460f1e91fab6342b89
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -0,0 +1,42 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ state: [0,0,0,0,0,0,1]
+ cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ time_tolerance: 0.1
+ timestamp: 0
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_key_frames: 3
+ fix_first_key_frame: false
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e0bb4e191f5bc38a352d79aaa91d6d70be8dc1a9
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -0,0 +1,42 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ state: [0,0,0,0,0,0,1]
+ cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ time_tolerance: 0.1
+ timestamp: 0
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_key_frames: 3
+ fix_first_key_frame: true
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
\ No newline at end of file