diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index 802ab7b49dabf93455f8f93fc8c30745415fe33e..95eed7b383c3f3e003edae1eaa5347469a51d00e 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -82,7 +82,7 @@ class CeresManager : public SolverManager
 
         ceres::Solver::Options& getSolverOptions();
 
-        void check();
+        virtual bool check() const override;
 
         const Eigen::SparseMatrixd computeHessian() const;
 
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index c2b2c161caa8c3ee8401e8d1b3426ac8f8f64db0..a410690155fc898411c715a142376047c852f6a3 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -117,11 +117,14 @@ public:
 
   virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
 
+  virtual bool check() const = 0;
+
 protected:
 
   std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
 
   virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+  const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
   virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
 
   virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0d0c6537fedfc21dec63760d877fb56386885adf..af6f468fb7756fe527c104e78f5807e736cd4d6e 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -52,7 +52,7 @@ std::string CeresManager::solveImpl(const ReportVerbosity report_level)
 {
     // Check
     #ifdef _WOLF_DEBUG
-        check();
+        assert(check());
     #endif
 
     // run Ceres Solver
@@ -410,26 +410,58 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa
         throw std::invalid_argument( "Wrong Jacobian Method!" );
 }
 
-void CeresManager::check()
+bool CeresManager::check() const
 {
+    bool ok = true;
+
     // Check numbers
-    assert(ceres_problem_->NumResidualBlocks() == fac_2_costfunction_.size());
-    assert(ceres_problem_->NumResidualBlocks() == fac_2_residual_idx_.size());
-    assert(ceres_problem_->NumParameterBlocks() == state_blocks_.size());
+    if (ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size())
+    {
+        WOLF_ERROR("CeresManager::check: number of residuals mismatch");
+        ok = false;
+    }
+    if (ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
+    {
+        WOLF_ERROR("CeresManager::check: number of residuals mismatch");
+        ok = false;
+    }
+    if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
+    {
+        WOLF_ERROR("CeresManager::check: number of parameters mismatch. ceres_problem_->NumParameterBlocks() = ", ceres_problem_->NumParameterBlocks(), " - state_blocks_.size() = ", state_blocks_.size());
+        ok = false;
+    }
 
     // Check parameter blocks
-    for (auto&& state_block_pair : state_blocks_)
-        assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
+    for (const auto& state_block_pair : state_blocks_)
+    {
+        if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
+        {
+            WOLF_ERROR("CeresManager::check: any state block is missing in ceres problem");
+            ok = false;
+        }
+    }
 
     // Check residual blocks
-    for (auto&& fac_res_pair : fac_2_residual_idx_)
+    for (const auto& fac_res_pair : fac_2_residual_idx_)
     {
         // costfunction - residual
-        assert(fac_2_costfunction_.find(fac_res_pair.first) != fac_2_costfunction_.end());
-        assert(fac_2_costfunction_[fac_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second));
+        if (fac_2_costfunction_.find(fac_res_pair.first) == fac_2_costfunction_.end())
+        {
+            WOLF_ERROR("CeresManager::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_");
+            ok = false;
+        }
+        if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+        {
+            WOLF_ERROR("CeresManager::check: fac_2_costfunction_ and ceres mismatch");
+            ok = false;
+        }
 
         // factor - residual
-        assert(fac_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor());
+        if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
+        {
+            WOLF_ERROR("CeresManager::check: fac_2_residual_idx_ and ceres mismatch");
+            ok = false;
+        }
 
         // parameter blocks - state blocks
         std::vector<double*> param_blocks;
@@ -437,10 +469,15 @@ void CeresManager::check()
         auto i = 0;
         for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
         {
-            assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
+            if (getAssociatedMemBlockPtr(st) != param_blocks[i])
+            {
+                WOLF_ERROR("CeresManager::check: parameters mismatch");
+                ok = false;
+            }
             i++;
         }
     }
+    return ok;
 }
 
 void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 6532dfc6a2b95d43451e00f8fad4797e7c53edf9..800201dd064d6a7fd5197972ee0a5f16e423ba2a 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -24,7 +24,9 @@ void SolverManager::update()
     {
         if (fac_notification_it->second == REMOVE)
         {
+            assert(check());
             removeFactor(fac_notification_it->first);
+            assert(check());
             fac_notification_it = fac_notification_map.erase(fac_notification_it);
         }
         else
@@ -41,12 +43,14 @@ void SolverManager::update()
             // only add if not added
             if (state_blocks_.find(state_ptr) == state_blocks_.end())
             {
+                assert(check());
                 state_blocks_.emplace(state_ptr, state_ptr->getState());
                 addStateBlock(state_ptr);
                 // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
                 state_ptr->resetStateUpdated();
                 state_ptr->resetFixUpdated();
                 state_ptr->resetLocalParamUpdated();
+                assert(check());
             }
             else
             {
@@ -58,8 +62,10 @@ void SolverManager::update()
             // only remove if it exists
             if (state_blocks_.find(state_ptr)!=state_blocks_.end())
             {
+                assert(check());
                 removeStateBlock(state_ptr);
                 state_blocks_.erase(state_ptr);
+                assert(check());
             }
             else
             {
@@ -76,7 +82,9 @@ void SolverManager::update()
         assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
 
         // add factor
+        assert(check());
         addFactor(fac_notification_map.begin()->first);
+        assert(check());
         // remove notification
         fac_notification_map.erase(fac_notification_map.begin());
     }
@@ -150,6 +158,16 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
     return it->second;
 }
 
+const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const
+{
+    auto it = state_blocks_.find(state_ptr);
+
+    if (it == state_blocks_.end())
+        throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+
+    return it->second.data();
+}
+
 double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
 {
     auto it = state_blocks_.find(state_ptr);
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9154bb38532ffa4cdcaae21a202103e24f16948
--- /dev/null
+++ b/test/dummy/solver_manager_dummy.h
@@ -0,0 +1,104 @@
+/*
+ * solver_manager_dummy.h
+ *
+ *  Created on: May 27, 2020
+ *      Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+#define TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+
+#include "core/solver/solver_manager.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(SolverManagerDummy);
+class SolverManagerDummy : public SolverManager
+{
+    public:
+        std::list<FactorBasePtr> factors_;
+        std::map<StateBlockPtr,bool> state_block_fixed_;
+        std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
+
+        SolverManagerDummy(const ProblemPtr& wolf_problem) :
+            SolverManager(wolf_problem)
+        {
+        };
+
+        bool isStateBlockRegistered(const StateBlockPtr& st)
+        {
+            return state_blocks_.find(st)!=state_blocks_.end();
+        };
+
+        bool isStateBlockFixed(const StateBlockPtr& st) const
+        {
+            return state_block_fixed_.at(st);
+        };
+
+        bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
+        {
+            return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+        };
+
+        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+        {
+            return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
+        };
+
+        bool hasLocalParametrization(const StateBlockPtr& st) const
+        {
+            return state_block_local_param_.find(st) != state_block_local_param_.end();
+        };
+
+        virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
+        virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
+        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
+        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
+
+        // The following are dummy implementations
+        bool    hasConverged()  { return true;      }
+        SizeStd iterations()    { return 1;         }
+        double  initialCost()   { return double(1); }
+        double  finalCost()     { return double(0); }
+        virtual bool check() const override {return true;}
+
+
+
+    protected:
+
+        virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
+        virtual void addFactor(const FactorBasePtr& fac_ptr)
+        {
+            factors_.push_back(fac_ptr);
+        };
+        virtual void removeFactor(const FactorBasePtr& fac_ptr)
+        {
+            factors_.remove(fac_ptr);
+        };
+        virtual void addStateBlock(const StateBlockPtr& state_ptr)
+        {
+            state_block_fixed_[state_ptr] = state_ptr->isFixed();
+            state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+        };
+        virtual void removeStateBlock(const StateBlockPtr& state_ptr)
+        {
+            state_block_fixed_.erase(state_ptr);
+            state_block_local_param_.erase(state_ptr);
+        };
+        virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
+        {
+            state_block_fixed_[state_ptr] = state_ptr->isFixed();
+        };
+        virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+        {
+            if (state_ptr->getLocalParametrization() == nullptr)
+                state_block_local_param_.erase(state_ptr);
+            else
+                state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+        };
+};
+
+}
+
+#endif /* TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_ */
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 7ead2b8f801602e664cae1e2ec4249f21ef916a5..a9824d43ff2709308ee44de0d23d4403460ef96e 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -88,7 +88,7 @@ TEST(CeresManager, Create)
     ASSERT_EQ(P, ceres_manager_ptr->getProblem());
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddStateBlock)
@@ -111,7 +111,7 @@ TEST(CeresManager, AddStateBlock)
     ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, DoubleAddStateBlock)
@@ -140,7 +140,7 @@ TEST(CeresManager, DoubleAddStateBlock)
     ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, UpdateStateBlock)
@@ -171,7 +171,7 @@ TEST(CeresManager, UpdateStateBlock)
     ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddUpdateStateBlock)
@@ -198,7 +198,7 @@ TEST(CeresManager, AddUpdateStateBlock)
     ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, RemoveStateBlock)
@@ -230,7 +230,7 @@ TEST(CeresManager, RemoveStateBlock)
     ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddRemoveStateBlock)
@@ -256,7 +256,7 @@ TEST(CeresManager, AddRemoveStateBlock)
     ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, RemoveUpdateStateBlock)
@@ -281,7 +281,7 @@ TEST(CeresManager, RemoveUpdateStateBlock)
     ceres_manager_ptr->update();
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, DoubleRemoveStateBlock)
@@ -309,7 +309,7 @@ TEST(CeresManager, DoubleRemoveStateBlock)
     ceres_manager_ptr->update();
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddFactor)
@@ -331,7 +331,7 @@ TEST(CeresManager, AddFactor)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, DoubleAddFactor)
@@ -356,7 +356,7 @@ TEST(CeresManager, DoubleAddFactor)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, RemoveFactor)
@@ -384,7 +384,7 @@ TEST(CeresManager, RemoveFactor)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddRemoveFactor)
@@ -411,7 +411,7 @@ TEST(CeresManager, AddRemoveFactor)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, DoubleRemoveFactor)
@@ -446,7 +446,7 @@ TEST(CeresManager, DoubleRemoveFactor)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddStateBlockLocalParam)
@@ -473,7 +473,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
     ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, RemoveLocalParam)
@@ -505,7 +505,7 @@ TEST(CeresManager, RemoveLocalParam)
     ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, AddLocalParam)
@@ -538,7 +538,7 @@ TEST(CeresManager, AddLocalParam)
     ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, FactorsRemoveLocalParam)
@@ -580,7 +580,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 TEST(CeresManager, FactorsUpdateLocalParam)
@@ -624,7 +624,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
     ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
 
     // run ceres manager check
-    ceres_manager_ptr->check();
+    ASSERT_TRUE(ceres_manager_ptr->check());
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index bf878e87c1cb5a277225ac20be11fce7f1b12c5d..ceb1145ec6243c3467c0338490f48b3d76423dbe 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -15,6 +15,7 @@
 #include "core/processor/processor_odom_3d.h"
 #include "dummy/processor_tracker_feature_dummy.h"
 #include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
 
 #include "core/sensor/sensor_diff_drive.h"
 #include "core/processor/processor_diff_drive.h"
@@ -34,36 +35,6 @@ using namespace Eigen;
 // Register in the FactoryProcessor
 #include "core/processor/factory_processor.h"
 
-
-WOLF_PTR_TYPEDEFS(DummySolverManager);
-
-class DummySolverManager : public SolverManager
-{
-public:
-  DummySolverManager(const ProblemPtr& _problem)
-    : SolverManager(_problem)
-  {
-    //
-  }
-  virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
-  virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
-  virtual bool hasConverged(){return true;};
-  virtual SizeStd iterations(){return 0;};
-  virtual double initialCost(){return 0;};
-  virtual double finalCost(){return 0;};
-  virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
-  virtual void addFactor(const FactorBasePtr& fac_ptr){};
-  virtual void removeFactor(const FactorBasePtr& fac_ptr){};
-  virtual void addStateBlock(const StateBlockPtr& state_ptr){};
-  virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
-  virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
-  virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
-  virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
-  virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-};
-
 TEST(Problem, create)
 {
     ProblemPtr P = Problem::create("POV", 3);
@@ -298,7 +269,7 @@ TEST(Problem, StateBlocks)
     //    P->print(4,1,1,1);
 
     // consume notifications
-    DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
+    SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
     SM->update(); // calls P->consumeStateBlockNotificationMap();
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 640dc71faefcdfcaa937bdc928c904ef618f05d9..d4ad57425d8dd859c0b5f7535dcf42f4ce58b976 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,104 +13,19 @@
 #include "core/state_block/state_block.h"
 #include "core/capture/capture_void.h"
 #include "core/factor/factor_pose_2d.h"
-#include "core/solver/solver_manager.h"
 #include "core/state_block/local_parametrization_base.h"
 #include "core/state_block/local_parametrization_angle.h"
+#include "dummy/solver_manager_dummy.h"
 
 #include <iostream>
 
 using namespace wolf;
 using namespace Eigen;
 
-WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
-class SolverManagerWrapper : public SolverManager
-{
-    public:
-        std::list<FactorBasePtr> factors_;
-        std::map<StateBlockPtr,bool> state_block_fixed_;
-        std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
-
-        SolverManagerWrapper(const ProblemPtr& wolf_problem) :
-            SolverManager(wolf_problem)
-        {
-        };
-
-        bool isStateBlockRegistered(const StateBlockPtr& st)
-        {
-            return state_blocks_.find(st)!=state_blocks_.end();
-        };
-
-        bool isStateBlockFixed(const StateBlockPtr& st) const
-        {
-            return state_block_fixed_.at(st);
-        };
-
-        bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
-        {
-            return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
-        };
-
-        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
-        {
-            return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
-        };
-
-        bool hasLocalParametrization(const StateBlockPtr& st) const
-        {
-            return state_block_local_param_.find(st) != state_block_local_param_.end();
-        };
-
-        virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
-        virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
-        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
-        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-
-        // The following are dummy implementations
-        bool    hasConverged()  { return true;      }
-        SizeStd iterations()    { return 1;         }
-        double  initialCost()   { return double(1); }
-        double  finalCost()     { return double(0); }
-
-
-
-    protected:
-
-        virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
-        virtual void addFactor(const FactorBasePtr& fac_ptr)
-        {
-            factors_.push_back(fac_ptr);
-        };
-        virtual void removeFactor(const FactorBasePtr& fac_ptr)
-        {
-            factors_.remove(fac_ptr);
-        };
-        virtual void addStateBlock(const StateBlockPtr& state_ptr)
-        {
-            state_block_fixed_[state_ptr] = state_ptr->isFixed();
-            state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
-        };
-        virtual void removeStateBlock(const StateBlockPtr& state_ptr)
-        {
-            state_block_fixed_.erase(state_ptr);
-            state_block_local_param_.erase(state_ptr);
-        };
-        virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
-        {
-            state_block_fixed_[state_ptr] = state_ptr->isFixed();
-        };
-        virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
-        {
-            if (state_ptr->getLocalParametrization() == nullptr)
-                state_block_local_param_.erase(state_ptr);
-            else
-                state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
-        };
-};
-
 TEST(SolverManager, Create)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // check double pointers to branches
     ASSERT_EQ(P, solver_manager_ptr->getProblem());
@@ -119,7 +34,7 @@ TEST(SolverManager, Create)
 TEST(SolverManager, AddStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -138,7 +53,7 @@ TEST(SolverManager, AddStateBlock)
 TEST(SolverManager, DoubleAddStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -163,7 +78,7 @@ TEST(SolverManager, DoubleAddStateBlock)
 TEST(SolverManager, UpdateStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -211,7 +126,7 @@ TEST(SolverManager, UpdateStateBlock)
 TEST(SolverManager, AddUpdateStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -249,7 +164,7 @@ TEST(SolverManager, AddUpdateStateBlock)
 TEST(SolverManager, AddUpdateLocalParamStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -292,7 +207,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
 TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -339,7 +254,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
 TEST(SolverManager, RemoveStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -364,7 +279,7 @@ TEST(SolverManager, RemoveStateBlock)
 TEST(SolverManager, AddRemoveStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -386,7 +301,7 @@ TEST(SolverManager, AddRemoveStateBlock)
 TEST(SolverManager, RemoveUpdateStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -405,7 +320,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
 TEST(SolverManager, DoubleRemoveStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -430,7 +345,7 @@ TEST(SolverManager, DoubleRemoveStateBlock)
 TEST(SolverManager, AddUpdatedStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -508,7 +423,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
 TEST(SolverManager, AddFactor)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -535,7 +450,7 @@ TEST(SolverManager, AddFactor)
 TEST(SolverManager, RemoveFactor)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -568,7 +483,7 @@ TEST(SolverManager, RemoveFactor)
 TEST(SolverManager, AddRemoveFactor)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
@@ -603,7 +518,7 @@ TEST(SolverManager, AddRemoveFactor)
 TEST(SolverManager, DoubleRemoveFactor)
 {
     ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
     Vector2d state; state << 1, 2;
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 82c65c284c5f06b518943679ac70ae3f2803bc42..8eeaf7c0bdc28846f0c4d09a7ed25dcd4cbe0a9c 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -12,37 +12,12 @@
 #include "core/trajectory/trajectory_base.h"
 #include "core/frame/frame_base.h"
 #include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
 
 #include <iostream>
 
 using namespace wolf;
 
-struct DummySolverManager : public SolverManager
-{
-  DummySolverManager(const ProblemPtr& _problem)
-    : SolverManager(_problem)
-  {
-    //
-  }
-  virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
-  virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
-  virtual bool hasConverged(){return true;};
-  virtual SizeStd iterations(){return 0;};
-  virtual double initialCost(){return 0;};
-  virtual double finalCost(){return 0;};
-  virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
-  virtual void addFactor(const FactorBasePtr& fac_ptr){};
-  virtual void removeFactor(const FactorBasePtr& fac_ptr){};
-  virtual void addStateBlock(const StateBlockPtr& state_ptr){};
-  virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
-  virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
-  virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
-  virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
-  virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-};
-
 /// Set to true if you want debug info
 bool debug = false;
 
@@ -123,7 +98,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     ProblemPtr P = Problem::create("PO", 2);
     TrajectoryBasePtr T = P->getTrajectory();
 
-    DummySolverManager N(P);
+    SolverManagerDummy N(P);
 
     // Trajectory status:
     //  KF1   KF2    F3      frames