diff --git a/CMakeLists.txt b/CMakeLists.txt index fb83ccf5399baef48a2267f0657cf68d8bbd60db..575f38472916be64bee076e6a8708a18bf1ba0c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -254,6 +254,11 @@ 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 @@ -343,6 +348,9 @@ 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 ) @@ -417,6 +425,7 @@ ADD_LIBRARY(${PROJECT_NAME} ${SRCS_FACTOR} ${SRCS_FEATURE} ${SRCS_FRAME} + ${SRCS_GRAPH_MANAGER} ${SRCS_HARDWARE} ${SRCS_LANDMARK} ${SRCS_MAP} @@ -467,44 +476,46 @@ INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME}) #install headers -INSTALL(FILES ${HDRS_MATH} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/math) -INSTALL(FILES ${HDRS_UTILS} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils) -INSTALL(FILES ${HDRS_PROBLEM} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem) -INSTALL(FILES ${HDRS_HARDWARE} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware) -INSTALL(FILES ${HDRS_TRAJECTORY} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory) -INSTALL(FILES ${HDRS_MAP} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/map) -INSTALL(FILES ${HDRS_FRAME} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame) -INSTALL(FILES ${HDRS_STATE_BLOCK} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block) -INSTALL(FILES ${HDRS_COMMON} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/common) INSTALL(FILES ${HDRS_CAPTURE} DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture) +INSTALL(FILES ${HDRS_COMMON} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/common) INSTALL(FILES ${HDRS_FACTOR} DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor) INSTALL(FILES ${HDRS_FEATURE} DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature) -INSTALL(FILES ${HDRS_SENSOR} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor) -INSTALL(FILES ${HDRS_PROCESSOR} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor) +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} DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark) -INSTALL(FILES ${HDRS_WRAPPER} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper) -INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse) -INSTALL(FILES ${HDRS_SOLVER} - DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver) +INSTALL(FILES ${HDRS_MAP} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/map) +INSTALL(FILES ${HDRS_MATH} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/math) +INSTALL(FILES ${HDRS_PROBLEM} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem) +INSTALL(FILES ${HDRS_PROCESSOR} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor) +INSTALL(FILES ${HDRS_SENSOR} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor) INSTALL(FILES ${HDRS_SERIALIZATION} DESTINATION include/iri-algorithms/wolf/plugin_core/core/serialization) +INSTALL(FILES ${HDRS_SOLVER} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver) +INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse) +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_UTILS} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils) +INSTALL(FILES ${HDRS_WRAPPER} + DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper) INSTALL(FILES ${HDRS_YAML} DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml) diff --git a/include/core/graph_manager/factory_graph_manager.h b/include/core/graph_manager/factory_graph_manager.h new file mode 100644 index 0000000000000000000000000000000000000000..52a954b87f4a40f44ad0b4a37bc44b1900e5f585 --- /dev/null +++ b/include/core/graph_manager/factory_graph_manager.h @@ -0,0 +1,63 @@ +#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, + ProblemPtr> 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&, + ProblemPtr> 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 new file mode 100644 index 0000000000000000000000000000000000000000..fac9999c80c285513e65bbeb300a6b42638401d5 --- /dev/null +++ b/include/core/graph_manager/graph_manager_base.h @@ -0,0 +1,89 @@ +#ifndef INCLUDE_GRAPH_MANAGER_BASE_H_ +#define INCLUDE_GRAPH_MANAGER_BASE_H_ + +#include "core/problem/problem.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, \ + ProblemPtr _problem) \ +{ \ + auto params = std::make_shared<ParamsGraphManagerClass>(_unique_name, _server); \ + \ + auto graph_manager = std::make_shared<GraphManagerClass>(params, _problem); \ + \ + graph_manager ->setName(_unique_name); \ + \ + return graph_manager; \ +} \ +static GraphManagerBasePtr create(const std::string& _unique_name, \ + const ParamsGraphManagerBasePtr _params, \ + ProblemPtr _problem) \ +{ \ + auto params = std::static_pointer_cast<ParamsGraphManagerClass>(_params); \ + \ + auto graph_manager = std::make_shared<GraphManagerClass>(params, _problem); \ + \ + graph_manager ->setName(_unique_name); \ + \ + return graph_manager; \ +} \ + +WOLF_STRUCT_PTR_TYPEDEFS(ParamsGraphManagerBase) +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"; + } +}; + +WOLF_PTR_TYPEDEFS(GraphManagerBase) +class GraphManagerBase : public NodeBase +{ + public: + GraphManagerBase(const std::string& _type, ParamsGraphManagerBasePtr _params, ProblemPtr _problem) : + NodeBase("GRAPH_MANAGER", _type), + problem_(_problem), + params_(_params) + {} + + virtual ~GraphManagerBase(){} + + virtual void addKF(FrameBasePtr _KF) = 0; + virtual void addFactor(FactorBasePtr _factor) = 0; + + protected: + ProblemPtr problem_; + 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 new file mode 100644 index 0000000000000000000000000000000000000000..50b58c32714e0c01d16f54b7a84aa4d24148ebfc --- /dev/null +++ b/include/core/graph_manager/graph_manager_sliding_window.h @@ -0,0 +1,49 @@ +#ifndef INCLUDE_GRAPH_MANAGER_H_ +#define INCLUDE_GRAPH_MANAGER_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, ProblemPtr _problem) : + GraphManagerBase("GraphManagerSlidingWindow", _params, _problem), + params_sw_(_params) + {}; + WOLF_GRAPH_MANAGER_CREATE(GraphManagerSlidingWindow, ParamsGraphManagerSlidingWindow) + + virtual ~GraphManagerSlidingWindow(){} + + virtual void addKF(FrameBasePtr _KF) override; + virtual void addFactor(FactorBasePtr _factor) override {}; + + protected: + ParamsGraphManagerSlidingWindowPtr params_sw_; +}; + +} /* namespace wolf */ + +#endif /* INCLUDE_GRAPH_MANAGER_H_ */ diff --git a/src/graph_manager/graph_manager_sliding_window.cpp b/src/graph_manager/graph_manager_sliding_window.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6acfe83ca0532912f605dd729d0b29fc92965cd9 --- /dev/null +++ b/src/graph_manager/graph_manager_sliding_window.cpp @@ -0,0 +1,19 @@ +#include "core/graph_manager/graph_manager_sliding_window.h" + +namespace wolf +{ + +void GraphManagerSlidingWindow::addKF(FrameBasePtr _KF) +{ + WOLF_INFO("GraphManagerSlidingWindow: new KF added!"); +} + +} /* 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/test/gtest_graph_manager.cpp b/test/gtest_graph_manager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b682c41ce11f21473a107772be5b6152fa6e65f0 --- /dev/null +++ b/test/gtest_graph_manager.cpp @@ -0,0 +1,13 @@ +/* + * gtest_graph_manager.cpp + * + * Created on: Apr 2, 2020 + * Author: joanvallve + */ + +#include "../include/core/problem/graph_manager_base.h" + +namespace wolf +{ + +} /* namespace wolf */