diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 9037c4ddd743f72186f0b1d54714978d7accdb97..92846f85b76e7b67286f35c257ce32727de6ccb8 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -114,12 +114,17 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
             }
             // double loop all against all (without repetitions)
             for (unsigned int i = 0; i < all_state_blocks.size(); i++)
+            {
+                assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
                 for  (unsigned int j = i; j < all_state_blocks.size(); j++)
                 {
+                    assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
+
                     state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
                     double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
                                               getAssociatedMemBlockPtr(all_state_blocks[j]));
                 }
+            }
             break;
         }
         case CovarianceBlocksToBeComputed::ALL_MARGINALS:
@@ -128,26 +133,36 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
             for(auto fr_ptr : *wolf_problem_->getTrajectory())
                 if (fr_ptr->isKey())
                     for (const auto& key1 : wolf_problem_->getFrameStructure())
+                    {
+                        const auto& sb1 = fr_ptr->getStateBlock(key1);
+                        assert(isStateBlockRegisteredDerived(sb1));
+
                         for (const auto& key2 : wolf_problem_->getFrameStructure())
                         {
-                            const auto& sb1 = fr_ptr->getStateBlock(key1);
                             const auto& sb2 = fr_ptr->getStateBlock(key2);
+                            assert(isStateBlockRegisteredDerived(sb2));
+
                             state_block_pairs.emplace_back(sb1, sb2);
                             double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
                             if (sb1 == sb2)
                                 break;
                         }
+                    }
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
                 for (auto sb : l_ptr->getUsedStateBlockVec())
+                {
+                    assert(isStateBlockRegisteredDerived(sb));
                     for(auto sb2 : l_ptr->getUsedStateBlockVec())
                     {
+                        assert(isStateBlockRegisteredDerived(sb2));
                         state_block_pairs.emplace_back(sb, sb2);
                         double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
                         if (sb == sb2)
                             break;
                     }
+                }
             //            // loop all marginals (PO marginals)
             //            for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
             //            {
@@ -166,6 +181,9 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
             //robot-robot
             auto last_key_frame = wolf_problem_->getLastFrame();
 
+            assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
+            assert(isStateBlockRegisteredDerived(last_key_frame->getO()));
+
             state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
             state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
             state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO());
@@ -186,6 +204,8 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
 
                 for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
                 {
+                    assert(isStateBlockRegisteredDerived(*state_it));
+
                     // robot - landmark
                     state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
                     state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
@@ -197,8 +217,10 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
                     // landmark marginal
                     for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
                     {
-                      state_block_pairs.emplace_back(*state_it, *next_state_it);
-                      double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
+                        assert(isStateBlockRegisteredDerived(*next_state_it));
+
+                        state_block_pairs.emplace_back(*state_it, *next_state_it);
+                        double_pairs.emplace_back(getAssociatedMemBlockPtr((*state_it)),
                                                 getAssociatedMemBlockPtr((*next_state_it)));
                     }
                 }
@@ -229,19 +251,19 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
 
             // Error
             if (not last_frame->hasStateBlock('P') or
-                not isStateBlockRegistered(last_frame->getStateBlock('P')) or
+                not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')) or
                 not last_frame->hasStateBlock('O') or
-                not isStateBlockRegistered(last_frame->getStateBlock('O')) or
+                not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')) or
                 not sb_gnss_T or
-                not isStateBlockRegistered(sb_gnss_T))
+                not isStateBlockRegisteredDerived(sb_gnss_T))
             {
                 WOLF_WARN("SolverCeres::computeCovariances: last KF have null or unregistered state blocks P O or T, returning...");
                 WOLF_WARN_COND(not last_frame->getStateBlock('P'), "SolverCeres::computeCovariances: KF state block 'P' not found");
-                WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegistered(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
+                WOLF_WARN_COND(last_frame->getStateBlock('P') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('P')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
                 WOLF_WARN_COND(not last_frame->getStateBlock('O'), "SolverCeres::computeCovariances: KF state block 'O' not found");
-                WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegistered(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'P' not registered ", last_frame->getStateBlock('P'));
+                WOLF_WARN_COND(last_frame->getStateBlock('O') and not isStateBlockRegisteredDerived(last_frame->getStateBlock('O')), "SolverCeres::computeCovariances: KF state block 'O' not registered ", last_frame->getStateBlock('O'));
                 WOLF_WARN_COND(not sb_gnss_T, "SolverCeres::computeCovariances: KF capture state block 'T' not found");
-                WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegistered(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T);
+                WOLF_WARN_COND(sb_gnss_T and not isStateBlockRegisteredDerived(sb_gnss_T), "SolverCeres::computeCovariances: KF capture state block 'T' not registered ", sb_gnss_T);
 
                 return false;
             }
@@ -253,12 +275,18 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
 
             // double loop all against all (without repetitions)
             for (unsigned int i = 0; i < gauss_state_blocks.size(); i++)
+            {
+                assert(isStateBlockRegisteredDerived(gauss_state_blocks[i]));
+
                 for  (unsigned int j = i; j < gauss_state_blocks.size(); j++)
                 {
+                    assert(isStateBlockRegisteredDerived(gauss_state_blocks[j]));
+
                     state_block_pairs.emplace_back(gauss_state_blocks[i],gauss_state_blocks[j]);
                     double_pairs.emplace_back(getAssociatedMemBlockPtr(gauss_state_blocks[i]),
                                               getAssociatedMemBlockPtr(gauss_state_blocks[j]));
                 }
+            }
 
             break;
         }
@@ -301,15 +329,20 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
     std::vector<std::pair<const double*, const double*>> double_pairs;
 
     // double loop all against all (without repetitions)
-    for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++){
+    for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
+    {
         if (*st_it1 == nullptr){
             continue;
         }
+        assert(isStateBlockRegisteredDerived(*st_it1));
+
         for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
         {
             if (*st_it2 == nullptr){
                 continue;
             }
+            assert(isStateBlockRegisteredDerived(*st_it2));
+
             state_block_pairs.emplace_back(*st_it1, *st_it2);
             double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
                                       getAssociatedMemBlockPtr((*st_it2)));
@@ -341,7 +374,7 @@ bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
 
 void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
 {
-    assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
+    assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
 
     auto cost_func_ptr = createCostFunction(fac_ptr);
     fac_2_costfunction_[fac_ptr] = cost_func_ptr;
@@ -350,6 +383,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
     res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
     for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
     {
+        assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
         res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
     }
 
@@ -381,7 +415,7 @@ void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
 void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
 {
     assert(state_ptr);
-    assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added");
+    assert(!isStateBlockRegisteredDerived(state_ptr) && "adding a state block already added");
     assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
 
     ceres::LocalParameterization* local_parametrization_ptr = nullptr;
@@ -408,7 +442,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
 {
     //std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
     assert(state_ptr);
-    assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres");
+    assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
+
     ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
     state_blocks_local_param_.erase(state_ptr);
 }
@@ -416,6 +451,8 @@ void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
 void SolverCeres::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
 {
     assert(state_ptr != nullptr);
+    assert(isStateBlockRegisteredDerived(state_ptr) && "updating status of a state block that is not in ceres");
+
     if (state_ptr->isFixed())
         ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
     else