Skip to content
Snippets Groups Projects
Commit dcefc922 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

WIP not compiling

parent 2533b8ca
No related branches found
No related tags found
1 merge request!349Resolve "New GraphManager class"
Pipeline #5085 failed
......@@ -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);
......
......@@ -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()
{
......
#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_;
};
......
#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_ */
......@@ -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;
......
......@@ -3,7 +3,7 @@
namespace wolf
{
void GraphManagerSlidingWindow::addKF(FrameBasePtr _KF)
void GraphManagerSlidingWindow::newKeyFrame(FrameBasePtr _key_frame)
{
WOLF_INFO("GraphManagerSlidingWindow: new KF added!");
}
......
......@@ -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;
}
......
#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_ */
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment