hotfix missing implementation closes #357

src/solver/solver_manager.cpp

@@ -236,6 +236,7 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
 
     assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
     assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+    assert(state_ptr->isValid() && "SolverManager::addStateBlock state block state not valid (local parameterization)");
 
     // stateblock maps
     state_blocks_.emplace(state_ptr, state_ptr->getState());