diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 16551ab17572d474b889779b30d790fa98a99c65..8e0fbc786908a51c33b4612dc7930e31dc4e4fce 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -357,6 +357,9 @@ SET(SRCS_DTASSC
data_association/association_nnls.cpp
)
+# Add the solver sub-directory
+add_subdirectory(solver)
+
#optional HDRS and SRCS
IF (Ceres_FOUND)
SET(HDRS_WRAPPER
@@ -470,7 +473,8 @@ ADD_LIBRARY(${PROJECT_NAME}
SHARED
${SRCS_BASE}
${SRCS}
- #${SRCS_DTASSC}
+ #${SRCS_DTASSC}
+ ${SRCS_SOLVER}
${SRCS_WRAPPER}
)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
@@ -521,7 +525,7 @@ INSTALL(FILES ${HDRS}
# DESTINATION include/iri-algorithms/wolf/data_association)
INSTALL(FILES ${HDRS_WRAPPER}
DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/solver)
INSTALL(FILES ${HDRS_SERIALIZATION}
DESTINATION include/iri-algorithms/wolf/serialization)
diff --git a/src/solver/CMakeLists.txt b/src/solver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7263fb5e8ffab31a2b7dce2857fc840f2604b2b7
--- /dev/null
+++ b/src/solver/CMakeLists.txt
@@ -0,0 +1,9 @@
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+
+# Forward var to parent scope
+
+SET(HDRS_SOLVER ${HDRS_SOLVER}
+ ${CMAKE_CURRENT_SOURCE_DIR}/solver_manager.h PARENT_SCOPE)
+
+SET(SRCS_SOLVER ${SRCS_SOLVER}
+ ${CMAKE_CURRENT_SOURCE_DIR}/solver_manager.cpp PARENT_SCOPE)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f86bb35ed3305820e521e5ad178b0ae6398978a
--- /dev/null
+++ b/src/solver/solver_manager.cpp
@@ -0,0 +1,136 @@
+#include "solver_manager.h"
+#include "../trajectory_base.h"
+#include "../map_base.h"
+#include "../landmark_base.h"
+
+namespace wolf {
+
+SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
+ wolf_problem_(_wolf_problem)
+{
+ //
+}
+
+inline void SolverManager::update()
+{
+ StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
+
+ for (StateBlockPtr& state : states)
+ {
+ switch (state->notify())
+ {
+ case StateBlock::Notification::ADD:
+ {
+ state_blocks_[state] = state->getState();
+ addStateBlock(state);
+ state->notify(StateBlock::Notification::ENABLED);
+ break;
+ }
+ case StateBlock::Notification::UPDATE:
+ {
+ state_blocks_[state] = state->getState();
+ state->notify(StateBlock::Notification::ENABLED);
+ break;
+ }
+ case StateBlock::Notification::FIX_UPDATE:
+ {
+ // something to do ?
+ //state_blocks_
+
+ updateStateBlockStatus(state);
+ state->notify(StateBlock::Notification::ENABLED);
+ break;
+ }
+ case StateBlock::Notification::REMOVE:
+ {
+ if (state_blocks_.erase(state) > 0)
+ removeStateBlock(state);
+ break;
+ }
+ case StateBlock::Notification::ENABLED:
+ {
+ // do nothing
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update: State Block notification "
+ "must be ADD, UPDATE, FIX_UPDATE, REMOVE or ENABLED.");
+ }
+ }
+
+ states.clear();
+
+ // ADD CONSTRAINTS
+ while (!wolf_problem_->getConstraintNotificationList().empty())
+ {
+ switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+ {
+ case Notification::ADD:
+ {
+ addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+
+ break;
+ }
+ default:
+ throw std::runtime_error("SolverManager::update:"
+ " Constraint notification must be ADD or REMOVE.");
+ }
+
+ wolf_problem_->getConstraintNotificationList().pop_front();
+ }
+
+ assert(wolf_problem_->getConstraintNotificationList().empty() &&
+ "wolf problem's constraints notification list not empty after update");
+ assert(wolf_problem_->getNotifiedStateBlockList().empty() &&
+ "wolf problem's state_blocks notification list not empty after update");
+}
+
+inline wolf::ProblemPtr SolverManager::getProblemPtr()
+{
+ return wolf_problem_;
+}
+
+std::string SolverManager::solve(const ReportVerbosity report_level)
+{
+ // update problem
+ update();
+
+ std::string report = solveImpl(report_level);
+
+ // update StateBlocks with optimized state value.
+ /// @todo whatif someone has changed the state notification during opti ??
+// std::for_each(state_blocks_.begin(), state_blocks_.end(),
+// [](std::pair<StateBlockPtr, Eigen::VectorXs>& p)
+// { if (p.first->notify() == StateBlock::Notification::ENABLED) p.first->setState(p.second); });
+
+ std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
+ it_end = state_blocks_.end();
+
+ for(; it != it_end; ++it)
+ if (it->first->notify() == StateBlock::Notification::ENABLED)
+ it->first->setState(it->second);
+
+ return report;
+}
+
+Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
+{
+ auto it = state_blocks_.find(state_ptr);
+
+ if (it == state_blocks_.end())
+ throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+
+ return it->second;
+}
+
+Scalar* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
+{
+ auto it = state_blocks_.find(state_ptr);
+
+ if (it == state_blocks_.end())
+ throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+
+ return it->second.data();
+}
+
+} // namespace wolf
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..45a9b4760dd416b079308dd0b0b962865ec5ef51
--- /dev/null
+++ b/src/solver/solver_manager.h
@@ -0,0 +1,84 @@
+#ifndef _WOLF_SOLVER_MANAGER_H_
+#define _WOLF_SOLVER_MANAGER_H_
+
+//wolf includes
+#include "wolf.h"
+#include "state_block.h"
+#include "constraint_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(SolverManager)
+
+/**
+ * \brief Solver manager for WOLF
+ */
+class SolverManager
+{
+public:
+
+ /** \brief Enumeration of covariance blocks to be computed
+ *
+ * Enumeration of covariance blocks to be computed
+ *
+ */
+ enum class CovarianceBlocksToBeComputed : std::size_t
+ {
+ ALL, ///< All blocks and all cross-covariances
+ ALL_MARGINALS, ///< All marginals
+ ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
+ };
+
+ /**
+ * \brief Enumeration for the verbosity of the solver report.
+ */
+ enum class ReportVerbosity : std::size_t
+ {
+ QUIET = 0,
+ BRIEF,
+ FULL
+ };
+
+protected:
+
+ ProblemPtr wolf_problem_;
+
+public:
+
+ SolverManager(const ProblemPtr& wolf_problem);
+
+ virtual ~SolverManager() = default;
+
+ std::string solve(const ReportVerbosity report_level);
+
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+
+ virtual void computeCovariances(const StateBlockList& st_list) = 0;
+
+ virtual void update();
+
+ ProblemPtr getProblemPtr();
+
+protected:
+
+ std::map<StateBlockPtr, Eigen::VectorXs> state_blocks_;
+
+ virtual Eigen::VectorXs& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ virtual Scalar* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+
+ virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
+
+ virtual void addConstraint(const ConstraintBasePtr& ctr_ptr) = 0;
+
+ virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr) = 0;
+
+ virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
+
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr) = 0;
+
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) = 0;
+};
+
+} // namespace wolf
+
+#endif /* _WOLF_SOLVER_MANAGER_H_ */