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.