diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 9f548fd325de45d8901f432975bf23563938c988..ebf11819669dd49a0660b0a318a3a59077aa9bb5 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -208,6 +208,10 @@ WOLF_PTR_TYPEDEFS(NodeBase);
// Problem
WOLF_PTR_TYPEDEFS(Problem);
+// Graph Manager
+WOLF_PTR_TYPEDEFS(GraphManagerBase);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsGraphManagerBase);
+
// 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
index 52a954b87f4a40f44ad0b4a37bc44b1900e5f585..cb23c03e5b9707f5a5acbf94c29f3f8a1fc0fd35 100644
--- a/include/core/graph_manager/factory_graph_manager.h
+++ b/include/core/graph_manager/factory_graph_manager.h
@@ -31,8 +31,7 @@ inline std::string FactoryParamsGraphManager::getClass()
// GraphManager factory
typedef Factory<GraphManagerBase,
const std::string&,
- const ParamsGraphManagerBasePtr,
- ProblemPtr> FactoryGraphManager;
+ const ParamsGraphManagerBasePtr> FactoryGraphManager;
template<>
inline std::string FactoryGraphManager::getClass()
{
@@ -45,8 +44,7 @@ inline std::string FactoryGraphManager::getClass()
typedef Factory<GraphManagerBase,
const std::string&,
- const ParamsServer&,
- ProblemPtr> AutoConfFactoryGraphManager;
+ const ParamsServer&> AutoConfFactoryGraphManager;
template<>
inline std::string AutoConfFactoryGraphManager::getClass()
{
diff --git a/include/core/graph_manager/graph_manager_base.h b/include/core/graph_manager/graph_manager_base.h
index fac9999c80c285513e65bbeb300a6b42638401d5..962bdf54adc63645426c328d3f89f01b90383ded 100644
--- a/include/core/graph_manager/graph_manager_base.h
+++ b/include/core/graph_manager/graph_manager_base.h
@@ -1,7 +1,8 @@
#ifndef INCLUDE_GRAPH_MANAGER_BASE_H_
#define INCLUDE_GRAPH_MANAGER_BASE_H_
-#include "core/problem/problem.h"
+#include "core/common/wolf.h"
+#include "core/common/params_base.h"
namespace wolf
{
@@ -18,31 +19,28 @@ namespace wolf
*/
#define WOLF_GRAPH_MANAGER_CREATE(GraphManagerClass, ParamsGraphManagerClass) \
static GraphManagerBasePtr create(const std::string& _unique_name, \
- const ParamsServer& _server, \
- ProblemPtr _problem) \
+ const ParamsServer& _server) \
{ \
auto params = std::make_shared<ParamsGraphManagerClass>(_unique_name, _server); \
\
- auto graph_manager = std::make_shared<GraphManagerClass>(params, _problem); \
+ 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, \
- ProblemPtr _problem) \
+ const ParamsGraphManagerBasePtr _params) \
{ \
auto params = std::static_pointer_cast<ParamsGraphManagerClass>(_params); \
\
- auto graph_manager = std::make_shared<GraphManagerClass>(params, _problem); \
+ auto graph_manager = std::make_shared<GraphManagerClass>(params); \
\
graph_manager ->setName(_unique_name); \
\
return graph_manager; \
} \
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsGraphManagerBase)
struct ParamsGraphManagerBase : public ParamsBase
{
std::string prefix = "graph_manager/";
@@ -64,23 +62,21 @@ struct ParamsGraphManagerBase : public ParamsBase
}
};
-WOLF_PTR_TYPEDEFS(GraphManagerBase)
class GraphManagerBase : public NodeBase
{
public:
- GraphManagerBase(const std::string& _type, ParamsGraphManagerBasePtr _params, ProblemPtr _problem) :
+ GraphManagerBase(const std::string& _type, ParamsGraphManagerBasePtr _params) :
NodeBase("GRAPH_MANAGER", _type),
- problem_(_problem),
params_(_params)
{}
virtual ~GraphManagerBase(){}
- virtual void addKF(FrameBasePtr _KF) = 0;
- virtual void addFactor(FactorBasePtr _factor) = 0;
+ virtual void newKeyFrame(FrameBasePtr _key_frame) = 0;
+ virtual void newAuxFrame(FrameBasePtr _aux_frame) = 0;
+ virtual void newFactor(FactorBasePtr _factor) = 0;
protected:
- ProblemPtr problem_;
ParamsGraphManagerBasePtr params_;
};
diff --git a/include/core/graph_manager/graph_manager_sliding_window.h b/include/core/graph_manager/graph_manager_sliding_window.h
index 50b58c32714e0c01d16f54b7a84aa4d24148ebfc..5252a432fe48d187771e732f6191b8871ca7cad5 100644
--- a/include/core/graph_manager/graph_manager_sliding_window.h
+++ b/include/core/graph_manager/graph_manager_sliding_window.h
@@ -1,5 +1,5 @@
-#ifndef INCLUDE_GRAPH_MANAGER_H_
-#define INCLUDE_GRAPH_MANAGER_H_
+#ifndef INCLUDE_GRAPH_MANAGER_SLIDING_WINDOW_H_
+#define INCLUDE_GRAPH_MANAGER_SLIDING_WINDOW_H_
#include "core/graph_manager/graph_manager_base.h"
@@ -29,16 +29,17 @@ struct ParamsGraphManagerSlidingWindow : public ParamsGraphManagerBase
class GraphManagerSlidingWindow : public GraphManagerBase
{
public:
- GraphManagerSlidingWindow(ParamsGraphManagerSlidingWindowPtr _params, ProblemPtr _problem) :
- GraphManagerBase("GraphManagerSlidingWindow", _params, _problem),
+ GraphManagerSlidingWindow(ParamsGraphManagerSlidingWindowPtr _params) :
+ GraphManagerBase("GraphManagerSlidingWindow", _params),
params_sw_(_params)
{};
WOLF_GRAPH_MANAGER_CREATE(GraphManagerSlidingWindow, ParamsGraphManagerSlidingWindow)
virtual ~GraphManagerSlidingWindow(){}
- virtual void addKF(FrameBasePtr _KF) override;
- virtual void addFactor(FactorBasePtr _factor) override {};
+ virtual void newKeyFrame(FrameBasePtr _key_frame) override;
+ virtual void newAuxFrame(FrameBasePtr _aux_frame) override {};
+ virtual void newFactor(FactorBasePtr _factor) override {};
protected:
ParamsGraphManagerSlidingWindowPtr params_sw_;
@@ -46,4 +47,4 @@ class GraphManagerSlidingWindow : public GraphManagerBase
} /* namespace wolf */
-#endif /* INCLUDE_GRAPH_MANAGER_H_ */
+#endif /* INCLUDE_GRAPH_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index e1c48e685010cea4ba3311a7b9aa3fc24eef70ed..319ba9b282b6bf8bdb8ad23d5dd68d3a5f4a85f3 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -43,6 +43,7 @@ class Problem : public std::enable_shared_from_this<Problem>
friend ProcessorMotion;
protected:
+ GraphManagerBasePtr graph_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
@@ -74,6 +75,9 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen getDim() const;
std::string getFrameStructure() const;
+ // Graph manager --------------------------------------
+ void setGraphManager(GraphManagerBasePtr _gm);
+
// Hardware branch ------------------------------------
HardwareBasePtr getHardware() const;
diff --git a/src/graph_manager/graph_manager_sliding_window.cpp b/src/graph_manager/graph_manager_sliding_window.cpp
index 6acfe83ca0532912f605dd729d0b29fc92965cd9..31e42f49283889d528db18cd489cf0e19326d604 100644
--- a/src/graph_manager/graph_manager_sliding_window.cpp
+++ b/src/graph_manager/graph_manager_sliding_window.cpp
@@ -3,7 +3,7 @@
namespace wolf
{
-void GraphManagerSlidingWindow::addKF(FrameBasePtr _KF)
+void GraphManagerSlidingWindow::newKeyFrame(FrameBasePtr _key_frame)
{
WOLF_INFO("GraphManagerSlidingWindow: new KF added!");
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b35ae1a865bd9c8c90a2e8b08f4330685251b5b5..b95e1538a0db3160fe5a1f20b8240729d5eca183 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -15,7 +15,7 @@
#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
@@ -451,6 +451,14 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
+void Problem::setGraphManager(GraphManagerBasePtr _gm)
+{
+ if (graph_manager_)
+ graph_manager_->setProblem(nullptr);
+ graph_manager_ = _gm;
+ graph_manager_->setProblem(shared_from_this());
+}
+
Eigen::VectorXd Problem::zeroState() const
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
@@ -496,6 +504,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
" microseconds: ", duration.count());
#endif
}
+
+ // notify graph manager
+ if (graph_manager_)
+ graph_manager_->newKeyFrame(_keyframe_ptr);
}
bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
@@ -521,6 +533,10 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces
// }
//
// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+
+ // notify graph manager
+ if (graph_manager_)
+ graph_manager_->newAuxFrame(_frame_ptr);
}
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
@@ -582,8 +598,14 @@ FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _not
}
// Add notification
else
+ {
factor_notification_map_[_factor_ptr] = _noti;
+ // notify graph manager
+ if (graph_manager_)
+ graph_manager_->newFactor(_factor_ptr);
+ }
+
return _factor_ptr;
}
diff --git a/test/dummy/graph_manager_dummy.h b/test/dummy/graph_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..fbab3cb4b1e7ca0ffb4e4e78b0b8a01f19b709d6
--- /dev/null
+++ b/test/dummy/graph_manager_dummy.h
@@ -0,0 +1,38 @@
+#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, ParamsGraphManagerBasePtr)
+
+ virtual ~GraphManagerDummy(){}
+
+ virtual void newKeyFrame(FrameBasePtr _KF) override
+ {
+ n_KF_++;
+ };
+
+ virtual void newFactor(FactorBasePtr _factor) override
+ {
+ n_factors_++;
+ };
+
+ int n_factors_, n_KF_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_GRAPH_MANAGER_DUMMY_H_ */