problem fix new state_block notifications

src/problem.cpp

@@ -371,8 +371,13 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
 
     // add the state unit to the list
     state_block_list_.push_back(_state_ptr);
+
     // queue for solver manager
-    state_block_notification_list_.push_back(StateBlockNotification({ADD,_state_ptr}));
+    _state_ptr->addNotification(StateBlock::Notification::ADD);
+    notified_state_block_list_.push_back(_state_ptr);
+    notified_state_block_list_.sort();
+    notified_state_block_list_.unique();
+
     return _state_ptr;
 }
 
@@ -381,7 +386,10 @@ void Problem::updateStateBlockPtr(StateBlockPtr _state_ptr)
     //std::cout << "Problem::updateStateBlockPtr " << _state_ptr.get() << std::endl;
 
     // queue for solver manager
-    state_block_notification_list_.push_back(StateBlockNotification({UPDATE,_state_ptr}));
+    _state_ptr->addNotification(StateBlock::Notification::FIX_UPDATE);
+    notified_state_block_list_.push_back(_state_ptr);
+    notified_state_block_list_.sort();
+    notified_state_block_list_.unique();
 }
 
 void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
@@ -391,19 +399,11 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
     // add the state unit to the list
     state_block_list_.remove(_state_ptr);
 
-    // Check if the state addition is still as a notification
-    auto state_notif_it = state_block_notification_list_.begin();
-    for (; state_notif_it != state_block_notification_list_.end(); state_notif_it++)
-        if (state_notif_it->notification_ == ADD && state_notif_it->state_block_ptr_ == _state_ptr)
-            break;
-
-    // Remove addition notification
-    if (state_notif_it != state_block_notification_list_.end())
-    	state_block_notification_list_.erase(state_notif_it);
     // Add remove notification
-    else
-    	state_block_notification_list_.push_back(StateBlockNotification({REMOVE, _state_ptr}));
-
+    _state_ptr->addNotification(StateBlock::Notification::REMOVE);
+    notified_state_block_list_.push_back(_state_ptr);
+    notified_state_block_list_.sort();
+    notified_state_block_list_.unique();
 }
 
 ConstraintBasePtr Problem::addConstraintPtr(ConstraintBasePtr _constraint_ptr)

src/problem.h

@@ -29,11 +29,7 @@ enum Notification
     REMOVE,
     UPDATE
 };
-struct StateBlockNotification
-{
-    Notification notification_;
-    StateBlockPtr state_block_ptr_;
-};
+
 struct ConstraintNotification
 {
     Notification notification_;
@@ -53,10 +49,10 @@ class Problem : public std::enable_shared_from_this<Problem>
         ProcessorMotionPtr  processor_motion_ptr_;
         StateBlockList      state_block_list_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
-        std::list<StateBlockNotification> state_block_notification_list_;
         std::list<ConstraintNotification> constraint_notification_list_;
         bool prior_is_set_;
         Size state_size_, state_cov_size_;
+        StateBlockList notified_state_block_list_;
 
     private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
         Problem(const std::string& _frame_structure); // USE create() below !!
@@ -278,10 +274,9 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         void removeStateBlockPtr(StateBlockPtr _state_ptr);
 
-        /** \brief Gets a queue of state blocks notification to be handled by the solver
+        /** \brief Gets a list of state blocks which state has been changed to be handled by the solver
          */
-        std::list<StateBlockNotification>& getStateBlockNotificationList();
-
+        StateBlockList& getNotifiedStateBlockList();
 
         /** \brief Gets a queue of constraint notification to be handled by the solver
          */
@@ -331,16 +326,15 @@ inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
     return processor_motion_ptr_;
 }
 
-inline std::list<StateBlockNotification>& Problem::getStateBlockNotificationList()
-{
-    return state_block_notification_list_;
-}
-
 inline std::list<ConstraintNotification>& Problem::getConstraintNotificationList()
 {
     return constraint_notification_list_;
 }
 
+inline StateBlockList& Problem::getNotifiedStateBlockList()
+{
+    return notified_state_block_list_;
+}
 
 } // namespace wolf