From e5efaf4f43b8f4f8c970277f17fb15ba021b75c7 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:51:00 +0200
Subject: [PATCH] isStateBlock..Derived not crashing because of
associatedMemBlock
---
include/core/ceres_wrapper/solver_ceres.h | 10 +++++++---
1 file changed, 7 insertions(+), 3 deletions(-)
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 210a1aad9..9d473358a 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -173,18 +173,22 @@ inline ceres::Solver::Options& SolverCeres::getSolverOptions()
inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() and
- fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
+ return fac_2_residual_idx_.count(fac_ptr) == 1 and
+ fac_2_costfunction_.count(fac_ptr) == 1;
}
inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
{
+ if (state_blocks_.count(state_ptr) == 0)
+ return false;
return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
{
- return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
+ if (state_blocks_.count(st) == 0)
+ return false;
+ return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
};
inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
--
GitLab