From 8c94abaf5c1635524f74fe8ff4dd799e3c196781 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Sat, 19 Sep 2020 16:38:27 +0200
Subject: [PATCH] hotfix checking isValid in SolverManager
---
src/solver/solver_manager.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 498d6ec35..8fff62bb4 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -301,6 +301,7 @@ void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
{
+ assert(state_ptr->isValid() && "SolverManager::updateStateBlockState state block state not valid (local parameterization)");
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));
--
GitLab