add solver_manager

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)

src/solver/CMakeLists.txt

+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)

src/solver/solver_manager.cpp

+#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

src/solver/solver_manager.h

+#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_ */