Multi-threading patch

As explained in #188 (closed), some of the wolf ros nodes use different threads for each sensor callback and the call to solve() as well. Consequently, it rarely crashes due to some asserts in SolverManager::update().

They are there to guarantee that all notified factors have been updated and that all state blocks in the problem are registered. However, with this multithreading scheme, a new factor or state block is sometimes added during the update() execution.

This MR comments two asserts and adds a continue to avoid a third case which would produce an error. However, this is just a patch, we should discuss adding some mutex for some Problem attributes or whatever.. to deal with multi-threading.

src/solver/solver_manager.cpp

@@ -79,6 +79,9 @@ void SolverManager::update()
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
     for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
     {
+        if (state_blocks_.find(state_ptr)==state_blocks_.end())
+            continue;
+
         assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
 
         // state update
@@ -106,8 +109,8 @@ void SolverManager::update()
         }
     }
 
-    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");
+    //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()