Merge branch 'master' into pruning

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 ?
@@ -596,12 +596,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)

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)

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());
 

src/constraint_GPS_pseudorange_2D.h

@@ -4,8 +4,8 @@
 #define LIGHT_SPEED_ 299792458
 
 //Wolf includes
-#include <sensor_GPS.h>
-#include "feature_gps_pseudorange.h"
+#include "sensor_GPS.h"
+#include "feature_GPS_pseudorange.h"
 #include "constraint_autodiff.h"
 
 //std

src/processor_GPS.cpp

@@ -6,7 +6,7 @@
 #include <feature_GPS_pseudorange.h>
 #include <processor_GPS.h>
 
-#include "capture_gps.h"
+#include "capture_GPS.h"
 
 namespace wolf
 {

src/processor_GPS.h

@@ -7,7 +7,7 @@
 
 // Wolf includes
 #include "processor_base.h"
-#include "capture_gps.h"
+#include "capture_GPS.h"
 
 // Std includes
 

src/solver/solver_manager.cpp

@@ -55,9 +55,7 @@ void SolverManager::update()
       }
       case StateBlock::Notification::STATE_UPDATE:
       {
-        const bool registered = state_blocks_.find(state)!=state_blocks_.end();
-
-        WOLF_DEBUG_COND(!registered,
+        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
                         "Updating the state of an unregistered StateBlock !");
 
         // This will throw if StateBlock wasn't registered
@@ -92,8 +90,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;
       }

src/test/CMakeLists.txt

@@ -96,6 +96,10 @@ target_link_libraries(gtest_rotation ${PROJECT_NAME})
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
 target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
 
+# SolverManager test
+wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
+target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
+
 # TimeStamp class test
 wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
 target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
@@ -110,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})

src/test/gtest_ceres_manager.cpp

+/*
+ * 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();
+}
+

src/test/gtest_solver_manager.cpp

+/*
+ * gtest_solver_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 <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+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) :
+            SolverManager(wolf_problem)
+        {
+        };
+
+        bool isStateBlockRegistered(const StateBlockPtr& st) const
+        {
+            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)
+        {
+            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");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // check double ointers to branches
+    ASSERT_EQ(P, solver_manager_ptr->getProblemPtr());
+}
+
+TEST(SolverManager, AddStateBlock)
+{
+    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();
+
+    // check stateblock
+    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+}
+
+TEST(SolverManager, DoubleAddStateBlock)
+{
+    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();
+
+    // 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");
+    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();
+
+    // 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
+    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_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);
+
+    // add stateblock
+    P->addStateBlock(sb_ptr);
+
+    // 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
+    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)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+