more asserts and prints

src/solver/solver_manager.cpp

@@ -301,9 +301,12 @@ void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
 
 void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
 {
+    assert(state_ptr && "SolverManager::updateStateBlockState null state block");
+    assert(state_blocks_.count(state_ptr) == 1 && "SolverManager::updateStateBlockState unregistered state block");
     assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
+    assert(state_ptr->getState().size() == getAssociatedMemBlock(state_ptr).size());
+
     Eigen::VectorXd new_state = state_ptr->getState();
-    // We assume the same size for the states in both WOLF and the solver.
     std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
     // reset flag
     state_ptr->resetStateUpdated();
@@ -336,8 +339,8 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state
 
     if (it == state_blocks_.end())
     {
-        WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
-        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+        WOLF_ERROR("Tried to retrieve the memory block const ptr of an unregistered StateBlock: ", state_ptr);
+        throw std::runtime_error("Tried to retrieve the memory block const ptr of an unregistered StateBlock !");
     }
     return it->second.data();
 }
@@ -348,8 +351,8 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
 
     if (it == state_blocks_.end())
     {
-        WOLF_ERROR("Tried to retrieve the memory block of an unregistered StateBlock: ", state_ptr);
-        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+        WOLF_ERROR("Tried to retrieve the memory block ptr of an unregistered StateBlock: ", state_ptr);
+        throw std::runtime_error("Tried to retrieve the memory block ptr of an unregistered StateBlock !");
     }
     return it->second.data();
 }