Some last API changes

src/solver/solver_manager.cpp

@@ -30,15 +30,19 @@ void SolverManager::update()
     auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
     while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
     {
-        StateBlockPtr state = sb_notification_it->first;
+        StateBlockPtr state_ptr = sb_notification_it->first;
 
         if (sb_notification_it->second == ADD)
         {
             // only add if not added
-            if (state_blocks_.find(state) == state_blocks_.end())
+            if (state_blocks_.find(state_ptr) == state_blocks_.end())
             {
-                state_blocks_.emplace(state, state->getState());
-                addStateBlock(state);
+                state_blocks_.emplace(state_ptr, state_ptr->getState());
+                addStateBlock(state_ptr);
+                // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
+                state_ptr->resetStateUpdated();
+                state_ptr->resetFixUpdated();
+                state_ptr->resetLocalParamUpdated();
             }
             else
             {
@@ -48,10 +52,10 @@ void SolverManager::update()
         else
         {
             // only remove if it exists
-            if (state_blocks_.find(state)!=state_blocks_.end())
+            if (state_blocks_.find(state_ptr)!=state_blocks_.end())
             {
-                removeStateBlock(state);
-                state_blocks_.erase(state);
+                removeStateBlock(state_ptr);
+                state_blocks_.erase(state_ptr);
             }
             else
             {
@@ -83,22 +87,23 @@ void SolverManager::update()
             Eigen::VectorXs new_state = state_ptr->getState();
             // We assume the same size for the states in both WOLF and the solver.
             std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
+            // reset flag
+            state_ptr->resetStateUpdated();
         }
-
         // fix update
         if (state_ptr->fixUpdated())
         {
             updateStateBlockStatus(state_ptr);
+            // reset flag
+            state_ptr->resetFixUpdated();
         }
-
         // local parameterization update
         if (state_ptr->localParamUpdated())
         {
             updateStateBlockLocalParametrization(state_ptr);
+            // reset flag
+            state_ptr->resetLocalParamUpdated();
         }
-
-        // reset all flags
-        state_ptr->resetFlags();
     }
 
     assert(wolf_problem_->getConstraintNotificationMap().empty() && "wolf problem's constraints notification map not empty after update");
@@ -119,6 +124,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
 
     // update StateBlocks with optimized state value.
     /// @todo whatif someone has changed the state notification during opti ??
+    /// JV: I do not see a problem here, the solver provides the optimal state given the constraints, if someone changed the state during optimization, it will be overwritten by the optimal one.
 
     std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
             it_end = state_blocks_.end();
@@ -126,7 +132,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
     {
         // Avoid usuless copies
         if (!it->first->isFixed())
-            it->first->setState(it->second, false);
+            it->first->setState(it->second, false); // false = do not raise the flag state_updated_
     }
 
     return report;

src/state_block.cpp

@@ -26,46 +26,14 @@ void StateBlock::setFixed(bool _fixed)
     fixed_.store(_fixed);
 }
 
-//void StateBlock::addNotification(const StateBlock::Notification _new_notification)
+//void StateBlock::addToProblem(ProblemPtr _problem_ptr)
 //{
-//    std::lock_guard<std::mutex> lock(notifictions_mut_);
-//    if (_new_notification == Notification::ADD)
-//    {
-//        // When an ADD arrives, the state is already the newest,
-//        // thus old instructions can be cleared
-//        if (shared_from_this()->notifications_.size() > 0)
-//            notifications_.clear();
-//
-//        // Push ADD notification to the front
-//        notifications_.emplace_front(Notification::ADD);
-//    }
-//    else if (_new_notification == Notification::REMOVE)
-//    {
-//        // If we want to remove a block that still has an ADD instruction
-//        // we can just clear all notifications and just keep the remove
-//        if (!notifications_.empty() && notifications_.front() == Notification::ADD)
-//            notifications_.clear();
-//        else
-//        {
-//            notifications_.clear();
-//            notifications_.emplace_back(Notification::REMOVE);
-//        }
-//    }
-//    else
-//        std::runtime_error("unknown notification");
-//}
-
-//StateBlock::Notifications StateBlock::consumeNotifications() const
-//{
-//    std::lock_guard<std::mutex> lock(notifictions_mut_);
-//    return std::move(notifications_);
+//    _problem_ptr->addStateBlock(shared_from_this());
 //}
 //
-//StateBlock::Notifications StateBlock::getNotifications() const
+//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
 //{
-//    std::lock_guard<std::mutex> lock(notifictions_mut_);
-//    return notifications_;
+//    _problem_ptr->removeStateBlockPtr(shared_from_this());
 //}
 
-
 }

src/state_block.h

@@ -10,13 +10,11 @@ class LocalParametrizationBase;
 
 //Wolf includes
 #include "wolf.h"
-#include "local_parametrization_base.h"
 
 //std includes
 #include <iostream>
 #include <mutex>
 
-
 namespace wolf {
 
 /** \brief class StateBlock
@@ -85,8 +83,6 @@ public:
         SizeEigen getSize() const;
 
         /**\brief Returns the size of the local parametrization
-         *
-         * @return the size of the local parametrization
          */
         SizeEigen getLocalSize() const;
 
@@ -102,27 +98,25 @@ public:
          **/
         void unfix();
 
+        /** \brief Sets the state status
+         **/
         void setFixed(bool _fixed);
 
+        /** \brief Returns if the state has a local parametrization
+         **/
         bool hasLocalParametrization() const;
 
+        /** \brief Returns the state local parametrization ptr
+         **/
         LocalParametrizationBasePtr getLocalParametrizationPtr() const;
 
-        void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
-
-        void removeLocalParametrization();
-
-        //void addNotification(const StateBlock::Notification _new_notification);
-
-        //StateBlock::Notifications consumeNotifications() const;
-
-        /** \brief Check if exist any notification
+        /** \brief Sets a local parametrization
          **/
-        //bool hasNotifications() const;
+        void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
 
-        /** \brief Return list of notifications
+        /** \brief Removes the state_block local parametrization
          **/
-        //StateBlock::Notifications getNotifications() const;
+        void removeLocalParametrization();
 
         /** \brief Return if state has been updated
          **/
@@ -136,7 +130,25 @@ public:
          **/
         bool localParamUpdated() const;
 
-        void resetFlags();
+        /** \brief Set state_updated_ to false
+         **/
+        void resetStateUpdated();
+
+        /** \brief Set fix_updated_ to false
+         **/
+        void resetFixUpdated();
+
+        /** \brief Set localk_param_updated_ to false
+         **/
+        void resetLocalParamUpdated();
+
+        /** \brief Add this state_block to the problem
+         **/
+        //void addToProblem(ProblemPtr _problem_ptr);
+
+        /** \brief Remove this state_block from the problem
+         **/
+        //void removeFromProblem(ProblemPtr _problem_ptr);
 };
 
 } // namespace wolf
@@ -144,6 +156,7 @@ public:
 // IMPLEMENTATION
 #include "local_parametrization_base.h"
 #include "node_base.h"
+#include "problem.h"
 
 namespace wolf {
 
@@ -251,19 +264,21 @@ inline bool StateBlock::localParamUpdated() const
     return local_param_updated_.load();
 }
 
-inline void StateBlock::resetFlags()
+inline void StateBlock::resetStateUpdated()
 {
     state_updated_.store(false);
+}
+
+inline void StateBlock::resetFixUpdated()
+{
     fix_updated_.store(false);
-    local_param_updated_.store(false);
 }
 
-//inline bool StateBlock::hasNotifications() const
-//{
-//  std::lock_guard<std::mutex> lock(notifictions_mut_);
-//  return !notifications_.empty();
-//}
+inline void StateBlock::resetLocalParamUpdated()
+{
+    local_param_updated_.store(false);
+}
 
-} // namespace wolf
+}// namespace wolf
 
 #endif