minor: more verbose warnings and errors

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)