diff --git a/include/base/problem.h b/include/base/problem.h
index 0c82993f302c5ef5ca8f548e16a9247892eb9f6a..59d8aa3d1b9183a990fa69e83f72d1ce4b650a89 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -19,6 +19,7 @@ struct ProcessorParamsBase;
 #include "base/state_block.h"
 
 // std includes
+#include <mutex>
 
 namespace wolf {
 
@@ -42,6 +43,9 @@ class Problem : public std::enable_shared_from_this<Problem>
         SizeEigen state_size_, state_cov_size_, dim_;
         std::map<FactorBasePtr, Notification> factor_notification_map_;
         std::map<StateBlockPtr, Notification> state_block_notification_map_;
+        mutable std::mutex mut_factor_notifications_;
+        mutable std::mutex mut_state_block_notifications_;
+        mutable std::mutex mut_covariances_;
         bool prior_is_set_;
 
     private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -250,9 +254,9 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         void removeStateBlock(StateBlockPtr _state_ptr);
 
-        /** \brief Gets a map of factor notification to be handled by the solver
+        /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
          */
-        std::map<StateBlockPtr,Notification>& getStateBlockNotificationMap();
+        std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
 
         /** \brief Notifies a new factor to be added to the solver manager
          */
@@ -262,9 +266,9 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         void removeFactor(FactorBasePtr _factor_ptr);
 
-        /** \brief Gets a map of factor notification to be handled by the solver
+        /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
          */
-        std::map<FactorBasePtr, Notification>& getFactorNotificationMap();
+        std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
 
         // Print and check ---------------------------------------
         /**
@@ -303,14 +307,16 @@ inline ProcessorMotionPtr& Problem::getProcessorMotion()
     return processor_motion_ptr_;
 }
 
-inline std::map<StateBlockPtr,Notification>& Problem::getStateBlockNotificationMap()
+inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
 {
-    return state_block_notification_map_;
+    std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+    return std::move(state_block_notification_map_);
 }
 
-inline std::map<FactorBasePtr,Notification>& Problem::getFactorNotificationMap()
+inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
 {
-    return factor_notification_map_;
+    std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+    return std::move(factor_notification_map_);
 }
 
 } // namespace wolf
diff --git a/src/problem.cpp b/src/problem.cpp
index e88983fa90e6eedc7044bd8ddcd9831da7b53e78..1ab76dd56eb6f58b7afbe34cbb92a9b77c50929f 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -367,6 +367,7 @@ void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
 
 StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
 {
+    std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
     //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
 
     // Add add notification
@@ -384,6 +385,7 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
 
 void Problem::removeStateBlock(StateBlockPtr _state_ptr)
 {
+    std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
     //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
 
     // Check if there is already a notification for this state block
@@ -407,6 +409,7 @@ void Problem::removeStateBlock(StateBlockPtr _state_ptr)
 
 FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
 {
+    std::lock_guard<std::mutex> lock(mut_factor_notifications_);
     //std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
 
     // Add ADD notification
@@ -425,6 +428,7 @@ FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
 
 void Problem::removeFactor(FactorBasePtr _factor_ptr)
 {
+    std::lock_guard<std::mutex> lock(mut_factor_notifications_);
     //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
 
     // Check if there is already a notification for this state block
@@ -446,6 +450,7 @@ void Problem::removeFactor(FactorBasePtr _factor_ptr)
 
 void Problem::clearCovariance()
 {
+    std::lock_guard<std::mutex> lock(mut_covariances_);
     covariances_.clear();
 }
 
@@ -454,6 +459,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
     assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
     assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
 
+    std::lock_guard<std::mutex> lock(mut_covariances_);
     covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
 }
 
@@ -462,6 +468,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
     assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
     assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
 
+    std::lock_guard<std::mutex> lock(mut_covariances_);
     covariances_[std::make_pair(_state1, _state1)] = _cov;
 }
 
@@ -481,6 +488,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
 
     assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
 
+    std::lock_guard<std::mutex> lock(mut_covariances_);
+
     if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
         _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
                 covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
@@ -498,6 +507,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
 
 bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
 {
+    std::lock_guard<std::mutex> lock(mut_covariances_);
+
     // fill covariance
     for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
         for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index f8a1e25eb565672922e224bcb48606d2cf3de67c..22ca48386fefcb9bc42e5c18d3f3ec30e9ec6872 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -13,26 +13,30 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
 
 void SolverManager::update()
 {
+    // Consume notification maps
+    auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
+    auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
+
     // REMOVE CONSTRAINTS
-    auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
-    while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
+    for (auto fac_notification_it = fac_notification_map.begin();
+         fac_notification_it != fac_notification_map.end();
+         /* nothing, next is handled within the for */)
     {
         if (fac_notification_it->second == REMOVE)
         {
             removeFactor(fac_notification_it->first);
-            fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
+            fac_notification_it = fac_notification_map.erase(fac_notification_it);
         }
         else
             fac_notification_it++;
     }
 
     // ADD/REMOVE STATE BLOCS
-    auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
-    while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
+    while ( !sb_notification_map.empty() )
     {
-        StateBlockPtr state_ptr = sb_notification_it->first;
+        StateBlockPtr state_ptr = sb_notification_map.begin()->first;
 
-        if (sb_notification_it->second == ADD)
+        if (sb_notification_map.begin()->second == ADD)
         {
             // only add if not added
             if (state_blocks_.find(state_ptr) == state_blocks_.end())
@@ -63,17 +67,18 @@ void SolverManager::update()
             }
         }
         // next notification
-        sb_notification_it = wolf_problem_->getStateBlockNotificationMap().erase(sb_notification_it);
+        sb_notification_map.erase(sb_notification_map.begin());
     }
 
     // ADD CONSTRAINTS
-    fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
-    while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
+    while (!fac_notification_map.empty())
     {
-        assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
+        assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
 
-        addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
-        fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
+        // add factor
+        addFactor(fac_notification_map.begin()->first);
+        // remove notification
+        fac_notification_map.erase(fac_notification_map.begin());
     }
 
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
@@ -105,9 +110,6 @@ void SolverManager::update()
             state_ptr->resetLocalParamUpdated();
         }
     }
-
-    //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
-    //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
 }
 
 wolf::ProblemPtr SolverManager::getProblem()
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index cbe91a6fdf1c276f55bf50e9c32451f0618feb5b..a1d2901477c19835c231150c8dc98399aa4d0422 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -398,12 +398,10 @@ TEST(CeresManager, AddRemoveFactor)
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
 
-    ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
     // remove factor
     P->removeFactor(c);
 
-    ASSERT_TRUE(P->getFactorNotificationMap().empty());
+    ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // add+remove = empty
 
     // update solver
     ceres_manager_ptr->update();
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index dc1a90b80fdea318dd8a68629089b6535a688aa9..1e7b3698ce7bc18443f0e9099481a76857b87e43 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -207,11 +207,11 @@ TEST(Problem, StateBlocks)
 
     // 2 state blocks, fixed
     SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
+    ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
 
     // 3 state blocks, fixed
     SensorBasePtr    St = P->installSensor   ("CAMERA", "camera",   xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
+    ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (/*2 + */3)); // consume empties the notification map, so only should contain notification since last call
 
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
     params->time_tolerance            = 0.1;
@@ -224,13 +224,13 @@ TEST(Problem, StateBlocks)
 
     // 2 state blocks, estimated
     P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
+    ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
 
     //    P->print(4,1,1,1);
 
     // change some SB properties
     St->unfixExtrinsics();
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // No more notifications on since the same SB!
+    ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
 
     //    P->print(4,1,1,1);
 }
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03..72528dd43f6c781fc301e482577bbd7c3766a615 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -445,13 +445,14 @@ TEST(SolverManager, AddUpdatedStateBlock)
     ASSERT_TRUE(sb_ptr->fixUpdated());
     ASSERT_TRUE(sb_ptr->stateUpdated());
 
-    // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) ==
+    // == When an ADD is notified: a ADD notification should be stored ==
 
     // add state_block
     P->addStateBlock(sb_ptr);
 
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
-    ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second,ADD);
+    auto state_block_notification_map = P->consumeStateBlockNotificationMap();
+    ASSERT_EQ(state_block_notification_map.size(),1);
+    ASSERT_EQ(state_block_notification_map.begin()->second,ADD);
 
     // == Insert OTHER notifications ==
 
@@ -462,37 +463,30 @@ TEST(SolverManager, AddUpdatedStateBlock)
     // Fix --> FLAG
     sb_ptr->unfix();
 
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); // only ADD notification (fix and state are flags in sb)
+    ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // No new notifications (fix and set state are flags in sb)
 
-    // == REMOVE should clear the previous notification (ADD) in the stack ==
+    // == When an REMOVE is notified: a REMOVE notification should be stored ==
 
     // remove state_block
     P->removeStateBlock(sb_ptr);
 
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(),0);// ADD + REMOVE = EMPTY
+    state_block_notification_map = P->consumeStateBlockNotificationMap();
+    ASSERT_EQ(state_block_notification_map.size(),1);
+    ASSERT_EQ(state_block_notification_map.begin()->second,REMOVE);
 
-    // == UPDATES + REMOVE should clear the list of notifications ==
+    // == ADD + REMOVE: notification map should be empty ==
+    P->addStateBlock(sb_ptr);
+    P->removeStateBlock(sb_ptr);
+    ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty());
 
+    // == UPDATES should clear the list of notifications ==
     // add state_block
     P->addStateBlock(sb_ptr);
 
     // update solver
     solver_manager_ptr->update();
 
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(),0); // After solver_manager->update, notifications should be empty
-
-    // Fix
-    sb_ptr->fix();
-
-    // Set State
-    state_2 = 2*state;
-    sb_ptr->setState(state_2);
-
-    // remove state_block
-    P->removeStateBlock(sb_ptr);
-
-    ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
-    ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE);
+    ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // After solver_manager->update, notifications should be empty
 }
 
 TEST(SolverManager, AddFactor)
@@ -560,12 +554,10 @@ TEST(SolverManager, AddRemoveFactor)
     FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
     FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
 
-    ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
     // add factor
     P->removeFactor(c);
 
-    ASSERT_TRUE(P->getFactorNotificationMap().empty());
+    ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // ADD+REMOVE = empty
 
     // update solver
     solver_manager_ptr->update();
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index daafef0ac3d0c2038229bcc994ecb0fe2607b954..5591be0a15fbb470c8389b00064dc9fd7c47f7fd 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -31,10 +31,12 @@ struct DummyNotificationProcessor
       FAIL() << "problem_ is nullptr !";
     }
 
-    auto sb_noti_pair = problem_->getStateBlockNotificationMap().begin();
-    while (sb_noti_pair != problem_->getStateBlockNotificationMap().end())
+    auto sb_noti_map = problem_->consumeStateBlockNotificationMap();
+    ASSERT_TRUE(problem_->consumeStateBlockNotificationMap().empty()); // consume should empty the notification map
+
+    while (!sb_noti_map.empty())
     {
-        switch (sb_noti_pair->second)
+        switch (sb_noti_map.begin()->second)
         {
           case ADD:
           {
@@ -47,9 +49,9 @@ struct DummyNotificationProcessor
           default:
             throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
         }
-        sb_noti_pair = problem_->getStateBlockNotificationMap().erase(sb_noti_pair);
+        sb_noti_map.erase(sb_noti_map.begin());
     }
-    ASSERT_TRUE(problem_->getStateBlockNotificationMap().empty());
+    ASSERT_TRUE(sb_noti_map.empty());
   }
 
   ProblemPtr problem_;
@@ -118,19 +120,18 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     T->addFrame(f1); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 1);
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+    ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2);
     std::cout << __LINE__ << std::endl;
 
     T->addFrame(f2); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 2);
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f3); // F
+    T->addFrame(f3); // F (not KF so state blocks are not notified)
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 3);
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
+    ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
     std::cout << __LINE__ << std::endl;
 
     ASSERT_EQ(T->getLastFrame()->id(), f3->id());
@@ -138,13 +139,14 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 
     N.update();
+    ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
     std::cout << __LINE__ << std::endl;
 
     // remove frames and keyframes
     f2->remove(); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 2);
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+    ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
     std::cout << __LINE__ << std::endl;
 
     ASSERT_EQ(T->getLastFrame()->id(), f3->id());
@@ -154,7 +156,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     f3->remove(); // F
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 1);
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
     std::cout << __LINE__ << std::endl;
 
     ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
@@ -162,12 +163,11 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     f1->remove(); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 0);
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
     std::cout << __LINE__ << std::endl;
 
     N.update();
 
-    ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 0);
+    ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
     std::cout << __LINE__ << std::endl;
 }