From 52f2dbaaf9eb2d73eed8731212b5c7bfb177f36d 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:34:07 +0200
Subject: [PATCH] hotfix missing implementation closes #357
---
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 60b06fd1f..498d6ec35 100644
--- a/src/solver/solver_manager.cpp
+++ b/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());
--
GitLab