diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index 6224c83f27b594c23234449b781b5cf3f56bae77..f8fa7b85bcf3060d9665257c0e5121206ae67fbe 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -5,17 +5,21 @@
  *      \author: Dinesh Atchuthan
  */
 
-//Wolf
-#include "core/capture/capture_IMU.h"
-#include "core/factor/factor_pose_3D.h"
-#include "core/processor/processor_IMU.h"
-#include "core/sensor/sensor_IMU.h"
-#include "core/processor/processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
-
 #include <core/utils/utils_gtest.h>
-#include "core/utils/logging.h"
+#include <core/utils/logging.h>
 
+// IMU
+#include "imu/internal/config.h"
+#include "imu/capture/capture_IMU.h"
+#include "imu/processor/processor_IMU.h"
+#include "imu/sensor/sensor_IMU.h"
+
+//Wolf
+#include <core/factor/factor_pose_3D.h>
+#include <core/processor/processor_odom_3D.h>
+#include <core/ceres_wrapper/ceres_manager.h>
+
+// debug
 #include <iostream>
 #include <fstream>
 
@@ -38,15 +42,16 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr KF0;
         FrameBasePtr KF1;
         Eigen::Vector6s origin_bias;
         Eigen::VectorXs expected_final_state;
         Eigen::VectorXs x_origin;
+        Eigen::MatrixXs P_origin;
 
     virtual void SetUp()
     {
@@ -54,24 +59,24 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
 
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
 
@@ -80,13 +85,15 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
         expected_final_state.resize(10);
         x_origin.resize(10);
         x_origin << 0,0,0, 0,0,0,1, 0,0,0;
+        P_origin.setIdentity(9,9); P_origin *= 0.01;
         t.set(0);
 
         origin_bias << 0,0,0, 0,0,0;
         expected_final_state = x_origin; //null bias + static
 
         //set origin of the problem
-        KF0 = processor_ptr_imu->setOrigin(x_origin, t);
+        KF0 = problem->setPrior(x_origin, P_origin, t, 0.005);
+//        KF0 = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
 
@@ -108,7 +115,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test
 
         }
 
-        KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        KF1 = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
 
         //===================================================== END{PROCESS DATA}
@@ -124,10 +131,10 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr KF0;
         FrameBasePtr KF1;
         Eigen::Vector6s origin_bias;
@@ -140,24 +147,24 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
 
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -171,7 +178,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
         expected_final_state = x_origin; //null bias + static
 
         //set origin of the problem
-        KF0 = processor_ptr_imu->setOrigin(x_origin, t);
+        KF0 = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -192,7 +199,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        KF1 = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
 
         //===================================================== END{PROCESS DATA}
@@ -208,10 +215,10 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -224,24 +231,24 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
 
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
 //        ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
 //        ceres_options.max_line_search_step_contraction = 1e-3;
 //        ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -255,7 +262,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
         expected_final_state = x_origin; //null bias + static,
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -276,7 +283,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
 
         //===================================================== END{PROCESS DATA}
@@ -292,10 +299,10 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -308,24 +315,24 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
 
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -341,7 +348,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
         
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -362,7 +369,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
 
         //===================================================== END{PROCESS DATA}
@@ -378,10 +385,10 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -394,24 +401,24 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
 
@@ -431,7 +438,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
         expected_final_state = x_origin;
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
 
@@ -449,7 +456,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         last_KF->setState(expected_final_state);
 
         //===================================================== END{PROCESS DATA}
@@ -465,10 +472,10 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -481,24 +488,24 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -515,7 +522,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
         expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -532,7 +539,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         last_KF->setState(expected_final_state);
 
         //===================================================== END{PROCESS DATA}
@@ -548,10 +555,10 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -564,24 +571,24 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -605,7 +612,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
         data_imu = data_imu + origin_bias; // bias measurements
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -625,7 +632,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         last_KF->setState(expected_final_state);
 
         //===================================================== END{PROCESS DATA}
@@ -641,10 +648,10 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -657,24 +664,24 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -703,7 +710,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
         data_imu = data_imu + origin_bias; // bias measurements
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -724,7 +731,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
             sen_imu->process(imu_ptr);
         }
 
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t);
         last_KF->setState(expected_final_state);
 
         //===================================================== END{PROCESS DATA}
@@ -742,10 +749,10 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
     public:
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -758,24 +765,24 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION;
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -788,7 +795,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
         Eigen::Quaternions quat_current(Eigen::Quaternions::Identity());
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -823,7 +830,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
         }
 
         expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(ts);
         last_KF->setState(expected_final_state);
 
         //===================================================== END{PROCESS DATA}
@@ -860,7 +867,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
@@ -872,13 +879,13 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
         ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        sensor          = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
+        sensor          = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
         sensor_imu      = std::static_pointer_cast<SensorIMU>(sensor);
-        processor       = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
+        processor       = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         processor_imu   = std::static_pointer_cast<ProcessorIMU>(processor);
 
         // SENSOR + PROCESSOR ODOM 3D
-        sensor          = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
+        sensor          = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
         sensor_odo      = std::static_pointer_cast<SensorOdom3D>(sensor);
 
         sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
@@ -1024,11 +1031,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
         SensorOdom3DPtr sen_odom3D;
-        ProblemPtr wolf_problem_ptr_;
+        ProblemPtr problem;
         CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
-        ProcessorOdom3DPtr processor_ptr_odom3D;
+        ProcessorBasePtr processor_;
+        ProcessorIMUPtr processor_imu;
+        ProcessorOdom3DPtr processor_odom3D;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
@@ -1041,36 +1048,36 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION;
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = new CeresManager(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_);
 
         // SENSOR + PROCESSOR ODOM 3D
-        SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
+        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
         ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
         prc_odom3D_params->max_time_span = 0.9999;
         prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
         prc_odom3D_params->dist_traveled = 1000000000;
         prc_odom3D_params->angle_turned = 1000000000;
 
-        ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
+        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
         sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
-        processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
+        processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -1085,8 +1092,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-        processor_ptr_odom3D->setOrigin(origin_KF);
+        origin_KF = processor_imu->setOrigin(x_origin, t);
+        processor_odom3D->setOrigin(origin_KF);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -1100,7 +1107,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         Scalar dt(0.0010), dt_odom(1.0);
         TimeStamp ts(0.0), t_odom(0.0);
         wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-        wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr);
+        wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), nullptr);
         sen_odom3D->process(mot_ptr);
         //first odometry data will be processed at this timestamp
         t_odom.set(t_odom.get() + dt_odom);
@@ -1125,11 +1132,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
             imu_ptr->setData(data_imu);
             sen_imu->process(imu_ptr);
 
-            D_cor = processor_ptr_imu->getLast()->getDeltaCorrected(origin_bias);
+            D_cor = processor_imu->getLast()->getDeltaCorrected(origin_bias);
 
             if(ts.get() >= t_odom.get())
             {
-                WOLF_TRACE("X_preint(t)  : ", wolf_problem_ptr_->getState(ts).transpose());
+                WOLF_TRACE("X_preint(t)  : ", problem->getState(ts).transpose());
 
                 // PROCESS ODOM 3D DATA
                 data_odom3D.head(3) << 0,0,0;
@@ -1145,7 +1152,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
         }
 
         expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(ts);
         last_KF->setState(expected_final_state);
 
         WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose());
@@ -1165,16 +1172,17 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         wolf::TimeStamp t;
         SensorIMUPtr sen_imu;
         SensorOdom3DPtr sen_odom3D;
-        ProblemPtr wolf_problem_ptr_;
-        CeresManager* ceres_manager_wolf_diff;
-        ProcessorBasePtr processor_ptr_;
-        ProcessorIMUPtr processor_ptr_imu;
-        ProcessorOdom3DPtr processor_ptr_odom3D;
+        ProblemPtr problem;
+        CeresManagerPtr ceres_manager_wolf_diff;
+        ProcessorBasePtr processor;
+        ProcessorIMUPtr processor_imu;
+        ProcessorOdom3DPtr processor_odom3D;
         FrameBasePtr origin_KF;
         FrameBasePtr last_KF;
         Eigen::Vector6s origin_bias;
         Eigen::VectorXs expected_final_state;
         Eigen::VectorXs x_origin;
+        Eigen::MatrixXs P_origin;
 
     virtual void SetUp()
     {
@@ -1182,36 +1190,36 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         using std::make_shared;
         using std::static_pointer_cast;
 
-        std::string wolf_root = _WOLF_ROOT_DIR;
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
         
         //===================================================== SETTING PROBLEM
         // WOLF PROBLEM
-        wolf_problem_ptr_ = Problem::create("POV", 3);
+        problem = Problem::create("POV", 3);
 
         // CERES WRAPPER
         ceres::Solver::Options ceres_options;
         ceres_options.minimizer_type = ceres::TRUST_REGION;
         ceres_options.max_line_search_step_contraction = 1e-3;
         ceres_options.max_num_iterations = 1e4;
-        ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
+        ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options);
 
         // SENSOR + PROCESSOR IMU
-        SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
-        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
+        processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
         sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
-        processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
+        processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
 
         // SENSOR + PROCESSOR ODOM 3D
-        SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
+        SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml");
         ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
         prc_odom3D_params->max_time_span = 0.9999;
         prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
         prc_odom3D_params->dist_traveled = 1000000000;
         prc_odom3D_params->angle_turned = 1000000000;
 
-        ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
+        ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
         sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
-        processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
+        processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom);
     
         //===================================================== END{SETTING PROBLEM}
         //===================================================== INITIALIZATION
@@ -1219,14 +1227,16 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         expected_final_state.resize(10);
         x_origin.resize(10);
         x_origin << 0,0,0, 0,0,0,1, 0,0,0;
+        P_origin.setIdentity(9,9); P_origin *= .01;
         origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
         t.set(0);
         Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
         Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
 
         //set origin of the problem
-        origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-        processor_ptr_odom3D->setOrigin(origin_KF);
+        origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005);
+//        origin_KF = processor_imu->setOrigin(x_origin, t);
+//        processor_odom3D->setOrigin(origin_KF);
 
         //===================================================== END{INITIALIZATION}
         //===================================================== PROCESS DATA
@@ -1239,7 +1249,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         Scalar dt(0.0010), dt_odom(1.0);
         TimeStamp ts(0.0), t_odom(1.0);
         wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-        wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
+        wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, nullptr);
         sen_odom3D->process(mot_ptr);
         //first odometry data will be processed at this timestamp
         //t_odom.set(t_odom.get() + dt_odom);
@@ -1321,7 +1331,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
         }
 
         expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
-        last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
+        last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(ts);
         last_KF->setState(expected_final_state);
 
         //===================================================== END{PROCESS DATA}
@@ -1352,7 +1362,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     KF1->getV()->fix();
     KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished());
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1386,7 +1396,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1404,7 +1414,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1422,7 +1432,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1442,7 +1452,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
         KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-        report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1468,7 +1478,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK
     KF1->getO()->fix();
     KF1->getV()->fix();
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1502,7 +1512,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
     KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1520,7 +1530,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
     KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1538,7 +1548,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
     KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1558,7 +1568,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia
         KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
         KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-        report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
@@ -1584,7 +1594,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO
     last_KF->getO()->fix();
     last_KF->getV()->fix();
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1616,7 +1626,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     last_KF  ->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1631,7 +1641,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1646,7 +1656,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
     last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1663,7 +1673,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
         origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
         last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
 
-        report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1695,7 +1705,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
 
@@ -1711,7 +1721,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1726,7 +1736,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1743,7 +1753,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         origin_KF->setState(x_origin);
         last_KF->setState(expected_final_state);
 
-        report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1765,7 +1775,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     last_KF->getO()->fix();
     last_KF->getV()->fix();
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1796,7 +1806,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1811,7 +1821,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1826,7 +1836,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1843,7 +1853,7 @@ TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         origin_KF->setState(x_origin);
         last_KF->setState(expected_final_state);
 
-        report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1864,7 +1874,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
     last_KF->getO()->fix();
     last_KF->getV()->fix();
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1895,7 +1905,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1910,7 +1920,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1925,7 +1935,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1942,7 +1952,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
         origin_KF->setState(x_origin);
         last_KF->setState(expected_final_state);
 
-        report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1964,7 +1974,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO
     last_KF->getO()->fix();
     last_KF->getV()->fix();
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -1995,7 +2005,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2010,7 +2020,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2025,7 +2035,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2042,7 +2052,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
         origin_KF->setState(x_origin);
         last_KF->setState(expected_final_state);
 
-        report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2064,7 +2074,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
     last_KF->getO()->fix();
     last_KF->getV()->fix();
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2095,7 +2105,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2110,7 +2120,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2125,7 +2135,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
     origin_KF->setState(x_origin);
     last_KF->setState(expected_final_state);
 
-    report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2142,7 +2152,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
         origin_KF->setState(x_origin);
         last_KF->setState(expected_final_state);
 
-        report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+        report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
         //Only biases are unfixed
         ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2164,7 +2174,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
 
     last_KF->setState(expected_final_state);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2185,7 +2195,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_
 //
 //    last_KF->setState(expected_final_state);
 //
-//    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+//    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 //
 //    //Only biases are unfixed
 //    ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
@@ -2211,8 +2221,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager_wolf_diff->computeCovariances(ALL);
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2222,7 +2232,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i
     Eigen::MatrixXs covX(10,10);
         
     //get data from covariance blocks
-    wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+    problem->getFrameCovariance(last_KF, covX);
 
     for(int i = 0; i<10; i++)
         cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -2256,8 +2266,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager_wolf_diff->computeCovariances(ALL);
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2268,7 +2278,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i
     Eigen::MatrixXs covX(10,10);
         
     //get data from covariance blocks
-    wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+    problem->getFrameCovariance(last_KF, covX);
 
     for(int i = 0; i<10; i++)
         cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -2303,8 +2313,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    //ceres_manager_wolf_diff->computeCovariances(ALL);
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    //ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2332,7 +2342,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2362,7 +2372,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2390,7 +2400,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2419,8 +2429,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager_wolf_diff->computeCovariances(ALL);
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2435,7 +2445,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i
     Eigen::MatrixXs covX(10,10);
         
     //get data from covariance blocks
-    wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+    problem->getFrameCovariance(last_KF, covX);
 
     for(int i = 0; i<10; i++)
         cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -2457,12 +2467,9 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
     Eigen::MatrixXs featureFix_cov(6,6);
     featureFix_cov = Eigen::MatrixXs::Identity(6,6); 
     featureFix_cov(5,5) = 0.1;
-    // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
-    auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr);
-    // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
+    auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr);
     auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
-    // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-    FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
+    FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
 
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2480,8 +2487,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager_wolf_diff->computeCovariances(ALL);
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2496,7 +2503,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i
     Eigen::MatrixXs covX(10,10);
         
     //get data from covariance blocks
-    wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+    problem->getFrameCovariance(last_KF, covX);
 
     for(int i = 0; i<10; i++)
         cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -2518,12 +2525,12 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
     Eigen::MatrixXs featureFix_cov(6,6);
     featureFix_cov = Eigen::MatrixXs::Identity(6,6); 
     featureFix_cov(5,5) = 0.1;
-    // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
-    auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr);
+    // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3D>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr));
+    auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr);
     // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
     auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov);
-    // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-    FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
+    // FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
+    FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix));
 
     //prepare problem for solving
     origin_KF->getP()->fix();
@@ -2541,8 +2548,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
     origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager_wolf_diff->computeCovariances(ALL);
+    std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2557,7 +2564,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_
     Eigen::MatrixXs covX(10,10);
         
     //get data from covariance blocks
-    wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
+    problem->getFrameCovariance(last_KF, covX);
 
     for(int i = 0; i<10; i++)
         cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -2596,9 +2603,9 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in
 //    WOLF_TRACE("last   bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
 //    WOLF_TRACE("jacob  bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
     std::cout << report << std::endl;
-    ceres_manager->computeCovariances(ALL);
+    ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     //Only biases are unfixed
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2642,8 +2649,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
     origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager->computeCovariances(ALL);
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
     ASSERT_MATRIX_APPROX(last_KF  ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2688,7 +2695,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_in
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
     origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2715,7 +2722,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
     origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2744,7 +2751,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_in
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
     origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2771,7 +2778,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
     origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2800,8 +2807,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
     Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
     origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
 
-    std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-    ceres_manager->computeCovariances(ALL);
+    std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
+    ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);
 
     ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
     ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
@@ -2837,7 +2844,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
+//  ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
 //  ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
 //  ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
 //  ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";