Multithreading incompatible with asserts in solver_manager

Some of the wolf ros nodes use different threads for each sensor callback and the call to solve() as well. It rarely crashes due to two 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, rarely a new factor or state block is added during the update() execution.

It is rare but it raises the issue that wolf is not thread safe yet. However we are using multithreading with wolf...