From 70a8027c33638abb9e8805151c2195ba6039034c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 1 Oct 2020 17:50:18 +0200
Subject: [PATCH] more asserts and prints
---
src/solver/solver_manager.cpp | 13 ++++++++-----
1 file changed, 8 insertions(+), 5 deletions(-)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 123e69ef1..7ceb98c52 100644
--- a/src/solver/solver_manager.cpp
+++ b/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();
}
--
GitLab