// Check if it was stored as a 'floating' state block
if(floating_state_blocks_.count(st)==1)
{
WOLF_DEBUG("SolverManager::addFactor(): Factor ",fac_ptr->id()," involves state block ",st," stored as 'floating'. Adding the state block to the solver.");
floating_state_blocks_.erase(st);// This line must be BEFORE addStateBlock()!