diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c8774da932baa28806790fc8b1d8743f5caa4a0a..99e6d44b4271a7975e2570a3dc889d2c4d89989b 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -17,7 +17,7 @@ ENDIF()
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 option(BUILD_EXAMPLES "Build examples" OFF)
-
+set(BUILD_TESTS true)
 
 # Does this has any other interest
 # but for the examples ?
@@ -582,12 +582,10 @@ export(PACKAGE ${PROJECT_NAME})
 ## Testing ##
 #############
 IF (GLOG_FOUND)
-    if(BUILD_TESTS)
-    
+    IF(BUILD_TESTS)
         MESSAGE("Building tests.")
         add_subdirectory(test)
-
-    endif(BUILD_TESTS)
+    ENDIF(BUILD_TESTS)
 ENDIF (GLOG_FOUND)
 
 IF(BUILD_EXAMPLES)
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index f37b2377b391564d10a6163ca95d89a5823c480c..9cd617a40867511acb34f304f10b80a0d7be8c37 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -7,7 +7,7 @@
 
 namespace wolf {
 
-CeresManager::CeresManager(ProblemPtr& _wolf_problem,
+CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
                            const ceres::Solver::Options& _ceres_options) :
   SolverManager(_wolf_problem),
   ceres_options_(_ceres_options)
diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h
index 2e1e0cb998df70e5ac6e42968edc62a6b326d151..6486f200efd89a492cc0260258a542c2ac50823d 100644
--- a/src/ceres_wrapper/ceres_manager.h
+++ b/src/ceres_wrapper/ceres_manager.h
@@ -40,7 +40,7 @@ protected:
 
 public:
 
-  CeresManager(ProblemPtr& _wolf_problem,
+  CeresManager(const ProblemPtr& _wolf_problem,
                const ceres::Solver::Options& _ceres_options
                 = ceres::Solver::Options());
 
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index b26b977a519983f5d0e06b6cd177682dd78272a4..26fb4fd7fe7a3df9f51f489b5fb5cce6b265339d 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -92,8 +92,11 @@ void SolverManager::update()
         WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
                         "Tried to remove a StateBlock that was not added !");
 
-        if (state_blocks_.erase(state) > 0)
-          removeStateBlock(state);
+        if (state_blocks_.find(state)!=state_blocks_.end())
+        {
+            removeStateBlock(state);
+            state_blocks_.erase(state);
+        }
 
         break;
       }
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index cf4e6cbcdc39b083c506c0f519da489a42637726..078d304265420ff86f7c43d5c079fcd2aaae2779 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -114,6 +114,12 @@ target_link_libraries(gtest_trajectory ${PROJECT_NAME})
 
 # ------- Now Derived classes ----------
 
+IF (Ceres_FOUND)
+# CeresManager test
+wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp)
+target_link_libraries(gtest_ceres_manager ${PROJECT_NAME})
+ENDIF(Ceres_FOUND)
+
 # ConstraintAbs(P/O/V) classes test
 wolf_add_gtest(gtest_constraint_absolute gtest_constraint_absolute.cpp)
 target_link_libraries(gtest_constraint_absolute ${PROJECT_NAME})
diff --git a/src/test/gtest_ceres_manager.cpp b/src/test/gtest_ceres_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11a782e0dd4eb829103d49e98e2bd659ac73c6b7
--- /dev/null
+++ b/src/test/gtest_ceres_manager.cpp
@@ -0,0 +1,404 @@
+/*
+ * gtest_ceres_manager.cpp
+ *
+ *  Created on: Jun, 2018
+ *      Author: jvallve
+ */
+
+#include "utils_gtest.h"
+#include "../src/logging.h"
+
+#include "../problem.h"
+#include "../sensor_base.h"
+#include "../state_block.h"
+#include "../capture_void.h"
+#include "../constraint_pose_2D.h"
+#include "../solver/solver_manager.h"
+#include "../ceres_wrapper/ceres_manager.h"
+
+#include "ceres/ceres.h"
+
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(CeresManagerWrapper);
+class CeresManagerWrapper : public CeresManager
+{
+    public:
+        CeresManagerWrapper(const ProblemPtr& wolf_problem) :
+            CeresManager(wolf_problem)
+        {
+        };
+
+        bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st)
+        {
+            return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
+        };
+
+        bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
+        {
+            return state_blocks_.find(st)!=state_blocks_.end();
+        };
+
+        bool isStateBlockFixed(const StateBlockPtr& st)
+        {
+            return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
+        };
+
+        int numStateBlocks()
+        {
+            return ceres_problem_->NumParameterBlocks();
+        };
+
+        bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const
+        {
+            return ctr_2_residual_idx_.find(ctr_ptr) != ctr_2_residual_idx_.end() && ctr_2_costfunction_.find(ctr_ptr) != ctr_2_costfunction_.end();
+        };
+
+};
+
+TEST(SolverManager, Create)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // check double ointers to branches
+    ASSERT_EQ(P, ceres_manager_ptr->getProblemPtr());
+}
+
+TEST(SolverManager, AddStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check stateblock
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
+}
+
+TEST(SolverManager, DoubleAddStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // add stateblock again
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check stateblock
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
+}
+
+TEST(SolverManager, UpdateStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check stateblock unfixed
+    ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
+
+    // Fix frame
+    sb_ptr->fix();
+
+    // update stateblock
+    P->updateStateBlockPtr(sb_ptr);
+
+    // update solver manager
+    ceres_manager_ptr->update();
+
+    // check stateblock fixed
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
+}
+
+TEST(SolverManager, AddUpdateStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // update stateblock
+    P->updateStateBlockPtr(sb_ptr);
+
+    // update solver manager
+    ceres_manager_ptr->update();
+
+    // check stateblock fixed
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
+}
+
+TEST(SolverManager, RemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check stateblock
+    ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->numStateBlocks() == 0);
+}
+
+TEST(SolverManager, AddRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check no stateblocks
+    ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr));
+    ASSERT_TRUE(ceres_manager_ptr->numStateBlocks() == 0);
+}
+
+TEST(SolverManager, RemoveUpdateStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // Fix state block
+    sb_ptr->fix();
+
+    ASSERT_DEATH({
+        // update stateblock
+        P->updateStateBlockPtr(sb_ptr);
+
+        // update solver manager
+        ceres_manager_ptr->update();
+    },"");
+}
+
+TEST(SolverManager, DoubleRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver manager
+    ceres_manager_ptr->update();
+}
+
+TEST(SolverManager, AddConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check constraint
+    ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c));
+}
+
+TEST(SolverManager, RemoveConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // add constraint
+    P->removeConstraintPtr(c);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check constraint
+    ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
+}
+
+TEST(SolverManager, AddRemoveConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    ASSERT_TRUE(P->getConstraintNotificationList().front().constraint_ptr_ == c);
+
+    // add constraint
+    P->removeConstraintPtr(c);
+
+    ASSERT_TRUE(P->getConstraintNotificationList().empty());
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // check constraint
+    ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
+}
+
+TEST(SolverManager, DoubleRemoveConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // remove constraint
+    P->removeConstraintPtr(c);
+
+    // update solver
+    ceres_manager_ptr->update();
+
+    // remove constraint
+    P->removeConstraintPtr(c);
+
+    ASSERT_DEATH({
+    // update solver
+    ceres_manager_ptr->update();},"");
+
+    // check constraint
+    ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/src/test/gtest_solver_manager.cpp b/src/test/gtest_solver_manager.cpp
index 16b3fad389ca792923e0fe1a80a24b91a45f4d88..7543ea18155e7fc80a79e079d3ac35d343335634 100644
--- a/src/test/gtest_solver_manager.cpp
+++ b/src/test/gtest_solver_manager.cpp
@@ -10,6 +10,9 @@
 
 #include "../problem.h"
 #include "../sensor_base.h"
+#include "../state_block.h"
+#include "../capture_void.h"
+#include "../constraint_pose_2D.h"
 #include "../solver/solver_manager.h"
 
 #include <iostream>
@@ -21,10 +24,12 @@ WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
 class SolverManagerWrapper : public SolverManager
 {
     public:
+        std::list<ConstraintBasePtr> constraints_;
+        std::map<StateBlockPtr,bool> state_block_fixed_;
+
         SolverManagerWrapper(const ProblemPtr& wolf_problem) :
-            SolverManagerWrapper(wolf_problem)
+            SolverManager(wolf_problem)
         {
-
         };
 
         bool isStateBlockRegistered(const StateBlockPtr& st) const
@@ -32,23 +37,47 @@ class SolverManagerWrapper : public SolverManager
             return state_blocks_.find(st)!=state_blocks_.end();
         };
 
+        bool isStateBlockFixed(const StateBlockPtr& st) const
+        {
+            return state_block_fixed_.at(st);
+        };
+
+        bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const
+        {
+            return std::find(constraints_.begin(), constraints_.end(), ctr_ptr) != constraints_.end();
+        };
+
         virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
         virtual void computeCovariances(const StateBlockList& st_list){};
 
     protected:
 
         virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
-        virtual void addConstraint(const ConstraintBasePtr& ctr_ptr){};
-        virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr){};
-        virtual void addStateBlock(const StateBlockPtr& state_ptr){};
-        virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
-        virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
+        virtual void addConstraint(const ConstraintBasePtr& ctr_ptr)
+        {
+            constraints_.push_back(ctr_ptr);
+        };
+        virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr)
+        {
+            constraints_.remove(ctr_ptr);
+        };
+        virtual void addStateBlock(const StateBlockPtr& state_ptr)
+        {
+            state_block_fixed_[state_ptr] = state_ptr->isFixed();
+        };
+        virtual void removeStateBlock(const StateBlockPtr& state_ptr)
+        {
+            state_block_fixed_.erase(state_ptr);
+        };
+        virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
+        {
+            state_block_fixed_[state_ptr] = state_ptr->isFixed();
+        };
 };
 
 TEST(SolverManager, Create)
 {
     ProblemPtr P = Problem::create("PO 2D");
-    //CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P);
     SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
 
     // check double ointers to branches
@@ -58,69 +87,318 @@ TEST(SolverManager, Create)
 TEST(SolverManager, AddStateBlock)
 {
     ProblemPtr P = Problem::create("PO 2D");
-    //CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P);
     SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
 
-    // Set prior -> frame + constraint
-    Vector3s F1_state; F1_state << 1, 2, 3;
-    FrameBasePtr F1 = P->emplaceFrame(KEY_FRAME, F1_state,TimeStamp());
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
-    // update solver manager
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
     solver_manager_ptr->update();
 
     // check stateblock
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(F1->getStateBlockPtr(0)));
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(F1->getStateBlockPtr(1)));
-    ASSERT_EQ(solver_manager_ptr->getAssociatedMemBlock(F1->getStateBlockPtr(0)), F1->getStateBlockPtr(0)->getState());
+    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
 }
 
-/*TEST(SolverManager, AddPrior)
+TEST(SolverManager, DoubleAddStateBlock)
 {
     ProblemPtr P = Problem::create("PO 2D");
-    CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P);
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
 
-    // Set prior -> frame + constraint
-    Vector3s F1_state; F1_state << 1, 2, 3;
-    FrameBasePtr F1 = P->setPrior(Vector3s::Zero(),Matrix3s::Identity(),TimeStamp());
-    ConstraintBasePtr C1 = F1->getCaptureList().front()->getFeatureList().front()->getConstraintList().front();
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
-    // update solver manager
-    ceres_manager_ptr->update();
+    // add stateblock
+    P->addStateBlock(sb_ptr);
 
-    // check stateblock
-    ASSERT_EQ(ceres_manager_ptr->getAssociatedMemBlock(F1->getStateBlockPtr(0)), F1->getStateBlockPtr(0)->getState());
+    // update solver
+    solver_manager_ptr->update();
 
-    // solve
-    ceres_manager_ptr->solve();
+    // add stateblock again
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check stateblock
+    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
 }
 
 TEST(SolverManager, UpdateStateBlock)
 {
     ProblemPtr P = Problem::create("PO 2D");
-    CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P);
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
 
-    // Set prior -> frame + constraint
-    Vector3s F1_state; F1_state << 1, 2, 3;
-    FrameBasePtr F1 = P->setPrior(F1_state,Matrix3s::Identity(),TimeStamp());
-    ConstraintBasePtr C1 = F1->getCaptureList().front()->getFeatureList().front()->getConstraintList().front();
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check stateblock unfixed
+    ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+
+    // Fix frame
+    sb_ptr->fix();
+
+    // update stateblock
+    P->updateStateBlockPtr(sb_ptr);
 
     // update solver manager
-    ceres_manager_ptr->update();
+    solver_manager_ptr->update();
+
+    // check stateblock fixed
+    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+}
+
+TEST(SolverManager, AddUpdateStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // update stateblock
+    P->updateStateBlockPtr(sb_ptr);
+
+    // update solver manager
+    solver_manager_ptr->update();
+
+    // check stateblock fixed
+    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+}
+
+TEST(SolverManager, RemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
 
     // check stateblock
-    ASSERT_EQ(ceres_manager_ptr->getAssociatedMemBlock(F1->getStateBlockPtr(0)), F1->getStateBlockPtr(0)->getState());
+    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+}
+
+TEST(SolverManager, AddRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
-    // solve
-    ceres_manager_ptr->solve();
+    // add stateblock
+    P->addStateBlock(sb_ptr);
 
-    // Change state and fix
-    F1->setState(Vector3s::Zero());
-    F1->fix();
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check stateblock
+    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+}
+
+TEST(SolverManager, RemoveUpdateStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // Fix state block
+    sb_ptr->fix();
+
+    ASSERT_DEATH({
+        // update stateblock
+        P->updateStateBlockPtr(sb_ptr);
+
+        // update solver manager
+        solver_manager_ptr->update();
+    },"");
+}
+
+TEST(SolverManager, DoubleRemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
 
     // update solver manager
-    ceres_manager_ptr->update();
+    solver_manager_ptr->update();
+}
+
+TEST(SolverManager, AddConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check constraint
+    ASSERT_TRUE(solver_manager_ptr->isConstraintRegistered(c));
+}
+
+TEST(SolverManager, RemoveConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
 
-}*/
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // add constraint
+    P->removeConstraintPtr(c);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check constraint
+    ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c));
+}
+
+TEST(SolverManager, AddRemoveConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    ASSERT_TRUE(P->getConstraintNotificationList().front().constraint_ptr_ == c);
+
+    // add constraint
+    P->removeConstraintPtr(c);
+
+    ASSERT_TRUE(P->getConstraintNotificationList().empty());
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check constraint
+    ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c));
+}
+
+TEST(SolverManager, DoubleRemoveConstraint)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Create (and add) constraint point 2d
+    FrameBasePtr        F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
+    CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // remove constraint
+    P->removeConstraintPtr(c);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // remove constraint
+    P->removeConstraintPtr(c);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check constraint
+    ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c));
+}
 
 int main(int argc, char **argv)
 {