diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 8fff62bb4fa57ac42e58ecad5cc53dbf818d7f20..123e69ef120a0a20fc045c657397c14802ca7b96 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -166,7 +166,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr) // Warning if adding an already added if (factors_.count(fac_ptr) != 0) { - WOLF_WARN("Tried to add a factor that was already added !"); + WOLF_WARN("Tried to add the factor ", fac_ptr->id(), " that was already added !"); return; } @@ -206,7 +206,7 @@ void SolverManager::removeFactor(const FactorBasePtr& fac_ptr) // Warning if removing a missing factor if (factors_.count(fac_ptr) == 0) { - WOLF_WARN("Tried to remove a factor that is missing !"); + WOLF_WARN("Tried to remove factor ", fac_ptr->id(), " that is missing !"); return; } @@ -230,7 +230,7 @@ void SolverManager::addStateBlock(const StateBlockPtr& state_ptr) // Warning if adding an already added state block if (state_blocks_.count(state_ptr) != 0) { - WOLF_WARN("Tried to add a StateBlock that was already added !"); + WOLF_WARN("Tried to add athe StateBloc ", state_ptr, " that was already added !"); return; } @@ -256,7 +256,7 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr) // Warning if removing a missing state block if (state_blocks_.count(state_ptr) == 0) { - WOLF_WARN("Tried to remove a StateBlock that was not added !"); + WOLF_WARN("Tried to remove the StateBlock ", state_ptr, " that was not added !"); return; } @@ -323,8 +323,10 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state auto it = state_blocks_.find(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 !"); - + } return it->second; } @@ -333,8 +335,10 @@ const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state auto it = state_blocks_.find(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 !"); - + } return it->second.data(); } @@ -343,8 +347,10 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) auto it = state_blocks_.find(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 !"); - + } return it->second.data(); } @@ -365,7 +371,7 @@ bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const { - return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr); + return factors_.count(fac_ptr) == 1 and isFactorRegisteredDerived(fac_ptr); } bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)