diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 25d7fd4067c2401b46db2c247961b54a8213f71f..d50eaf29481885942bfaa1812d90e3fbc73d6aa3 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -47,7 +47,8 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
 
     // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
     CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
-    kf->addCapture(capture_rb);
+    // kf->addCapture(capture_rb);
+    capture_rb->link(kf);
 
     // 3. explore all observations in the capture
     for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
@@ -70,9 +71,10 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
         {
             // new landmark:
             // - create landmark
-            lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
+            // lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
+            lmk = std::static_pointer_cast<LandmarkPoint2D>(LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing)));
             WOLF_TRACE("new   lmk(", id, "): ", lmk->getP()->getState().transpose());
-            getProblem()->getMap()->addLandmark(lmk);
+            // getProblem()->getMap()->addLandmark(lmk);
             // - add to known landmarks
             known_lmks.emplace(id, lmk);
         }
@@ -81,17 +83,23 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
         Vector2s rb(range,bearing);
         auto ftr = std::make_shared<FeatureRangeBearing>(rb,
                                                          getSensor()->getNoiseCov());
-        capture_rb->addFeature(ftr);
+        // capture_rb->addFeature(ftr);
+        FeatureBase::emplace<FeatureRangeBearing>(capture_rb, rb, getSensor()->getNoiseCov());
 
         // 6. create factor
         auto prc = shared_from_this();
-        auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
-                                                            lmk,
-                                                            prc,
-                                                            false,
-                                                            FAC_ACTIVE);
-        ftr->addFactor(ctr);
-        lmk->addConstrainedBy(ctr);
+        // auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
+        //                                                     lmk,
+        //                                                     prc,
+        //                                                     false,
+        //                                                     FAC_ACTIVE);
+        auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb,
+                                                           lmk,
+                                                           prc,
+                                                           false,
+                                                           FAC_ACTIVE);
+        // ftr->addFactor(ctr);
+        // lmk->addConstrainedBy(ctr);
     }
 
 }
diff --git a/include/base/node_base.h b/include/base/node_base.h
index d1eafaf887043876c52bbd20d11ff8fb206d050b..5782d9fc7092593ac3f6b13821adcb89f119aed6 100644
--- a/include/base/node_base.h
+++ b/include/base/node_base.h
@@ -83,8 +83,6 @@ class NodeBase
         void setName(const std::string& _name);
         ProblemPtr getProblem() const;
         virtual void setProblem(ProblemPtr _prob_ptr);
-        // template<typename classReturn, typename classType, typename... T>
-        // static std::shared_ptr<classReturn> emplace(T&&... all);
 };
 
 } // namespace wolf
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index da1393232145b65cd46a2c23c27f964049ca04c6..d18c9591e5abd9932a7781144f172b9a72826487 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -80,19 +80,8 @@ class ProcessorOdom2D : public ProcessorMotion
         // Factory method
     public:
         static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-    void link(SensorBasePtr);
-    template<typename classType, typename... T>
-    static std::shared_ptr<ProcessorOdom2D> emplace(SensorBasePtr _sen_ptr, T&&... all);
 };
 
-template<typename classType, typename... T>
-std::shared_ptr<ProcessorOdom2D> ProcessorOdom2D::emplace(SensorBasePtr _sen_ptr, T&&... all)
-{
-    ProcessorOdom2DPtr prc = std::make_shared<classType>(std::forward<T>(all)...);
-    prc->link(_sen_ptr);
-    return prc;
-}
-
 inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const
 {
     return Eigen::VectorXs::Zero(delta_size_);
diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp
index ccf9b8631bd0759448ec6c20da7b00b9f534087d..e24a43d5a51459aa2583ebca9f9e794ec08f9360 100644
--- a/src/capture/capture_GPS_fix.cpp
+++ b/src/capture/capture_GPS_fix.cpp
@@ -25,10 +25,11 @@ CaptureGPSFix::~CaptureGPSFix()
 void CaptureGPSFix::process()
 {
 	// EXTRACT AND ADD FEATURES
-    addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
+    // addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
 
     // ADD CONSTRAINT
-    getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
+    // getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
+    FactorBase::emplace<FactorGPS2D>(getFeatureList().front(), getFeatureList().front());
 }
 
 } //namespace wolf
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 3d8a9fed3b5dd3570df735d7136902453390e33a..71b536f9c1780ef2a94a3f46898aadb152d79ccb 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -99,8 +99,6 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
 {
     //std::cout << "Adding feature" << std::endl;
     feature_list_.push_back(_ft_ptr);
-    _ft_ptr->setCapturePtr(shared_from_this());
-    _ft_ptr->setProblem(getProblem());
     return _ft_ptr;
 }
 
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 4f5cf88a4670649f8e61df9423353851797d424b..fc45eea814bc120ab3d7ec44867fcda407c3d4e5 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -18,14 +18,15 @@ CapturePose::~CapturePose()
 void CapturePose::emplaceFeatureAndFactor()
 {
     // Emplace feature
-    FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
-    addFeature(feature_pose);
+    // FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
+    // addFeature(feature_pose);
+    auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this(), data_, data_covariance_);
 
     // Emplace factor
     if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
-        feature_pose->addFactor(std::make_shared<FactorPose2D>(feature_pose));
+        FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose);
     else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
-        feature_pose->addFactor(std::make_shared<FactorPose3D>(feature_pose));
+        FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose);
     else
         throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
 }
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index 81900c7ef6077fe947f09c9f59a42d094c268a40..be1a73a7de7e5efe3be0c88677bae1f79455dcbd 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -62,18 +62,21 @@ int main()
 
     // Wolf problem
     ProblemPtr wolf_problem_ptr_ = Problem::create("PO 2D");
-    SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
-                                             std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
-                                             std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
-
+    // SensorBasePtr sensor_ptr_ = std::make_shared< SensorBase>("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+    //                                          std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+    //                                          std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+    auto sensor_ptr_ = SensorBase::emplace<SensorBase>(wolf_problem_ptr_->getHardware(), "ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+                                                       std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+                                                       std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
     ProcessorParamsTrackerLandmarkPtr params_trk = std::make_shared<ProcessorParamsTrackerLandmark>();
     params_trk->max_new_features = 5;
     params_trk->min_features_for_keyframe = 7;
     params_trk->time_tolerance = 0.25;
-    std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk);
-
-    wolf_problem_ptr_->addSensor(sensor_ptr_);
-    sensor_ptr_->addProcessor(processor_ptr_);
+    // std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ = std::make_shared< ProcessorTrackerLandmarkDummy>(params_trk);
+    std::shared_ptr<ProcessorTrackerLandmarkDummy> processor_ptr_ =
+        std::static_pointer_cast<ProcessorTrackerLandmarkDummy>(ProcessorBase::emplace<ProcessorTrackerLandmarkDummy>(sensor_ptr_, params_trk));
+    // wolf_problem_ptr_->addSensor(sensor_ptr_);
+    // sensor_ptr_->addProcessor(processor_ptr_);
 
     std::cout << "sensor & processor created and added to wolf problem" << std::endl;
 
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 3d9e5b49710a17fcfa26d7055a3ff4e50b4fed23..a6de1caf9f99b83539b91ea5f4ed74916d354cec 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -54,16 +54,6 @@ void FeatureBase::remove()
 FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
 {
     factor_list_.push_back(_co_ptr);
-    _co_ptr->setFeaturePtr(shared_from_this());
-    _co_ptr->setProblem(getProblem());
-    // add factor to be added in solver
-    if (getProblem() != nullptr)
-    {
-        if (_co_ptr->getStatus() == FAC_ACTIVE)
-            getProblem()->addFactor(_co_ptr);
-    }
-    else
-        WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
     return _co_ptr;
 }
 
@@ -148,7 +138,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con
         std::cout << "Linking FeatureBase" << std::endl;
         _cpt_ptr->addFeature(shared_from_this());
         this->setCapturePtr(_cpt_ptr);
-        this->setProblem(getProblem());
+        this->setProblem(_cpt_ptr->getProblem());
     }
 
 } // namespace wolf
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index aea78b7444c4478897affc1dc7af0dd099edd70c..cab2798a0106188e55e243eb5babb530fb3828ea 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -279,9 +279,6 @@ FrameBasePtr FrameBase::getNextFrame() const
 CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
 {
     capture_list_.push_back(_capt_ptr);
-    _capt_ptr->setFramePtr(shared_from_this());
-    _capt_ptr->setProblem(getProblem());
-    _capt_ptr->registerNewStateBlocks();
     return _capt_ptr;
 }
 
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
index 3349005129e275a5dd930cd0927d408149994680..a9ac36bf4d2c0b1a213348d7f9f58d2db2b2b1f8 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -17,10 +17,6 @@ HardwareBase::~HardwareBase()
 SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
 {
     sensor_list_.push_back(_sensor_ptr);
-    _sensor_ptr->setProblem(getProblem());
-    _sensor_ptr->setHardwarePtr(shared_from_this());
-
-    _sensor_ptr->registerNewStateBlocks();
 
     return _sensor_ptr;
 }
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index b31117566ac69542436e471dd1b1639492680a86..31051f68e2ab9b0869a06a4fedc267e62711e9ae 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -262,25 +262,41 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
 //            FactorPointToLine2DPtr fac_point_line_ptr = std::static_pointer_cast<FactorPointToLine2D>(fac_ptr);
 
             // If landmark point constrained -> new factor
+            // new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+            //                                                     std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+            //                                                     fac_point_line_ptr->getProcessor(),
+            //                                                     fac_point_line_ptr->getFeaturePointId(),
+            //                                                     _remain_id,
+            //                                                     fac_point_line_ptr->getLandmarkPointAuxId(),
+            //                                                     fac_point_ptr->getApplyLossFunction(),
+            //                                                     fac_point_line_ptr->getStatus());
+            // new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+            //                                                     std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+            //                                                     fac_point_line_ptr->getProcessor(),
+            //                                                     fac_point_line_ptr->getFeaturePointId(),
+            //                                                     fac_point_line_ptr->getLandmarkPointId(),
+            //                                                     _remain_id,
+            //                                                     fac_point_line_ptr->getApplyLossFunction(),
+            //                                                     fac_point_line_ptr->getStatus());
             if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
-                new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                                        std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                                        fac_point_line_ptr->getProcessor(),
-                                                                        fac_point_line_ptr->getFeaturePointId(),
-                                                                        _remain_id,
-                                                                        fac_point_line_ptr->getLandmarkPointAuxId(),
-                                                                        fac_point_ptr->getApplyLossFunction(),
-                                                                        fac_point_line_ptr->getStatus());
+                FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                                         std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                                         fac_point_line_ptr->getProcessor(),
+                                                         fac_point_line_ptr->getFeaturePointId(),
+                                                         _remain_id,
+                                                         fac_point_line_ptr->getLandmarkPointAuxId(),
+                                                         fac_point_ptr->getApplyLossFunction(),
+                                                         fac_point_line_ptr->getStatus());
             // If landmark point is aux point -> new factor
             else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
-                new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
-                                                                        std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
-                                                                        fac_point_line_ptr->getProcessor(),
-                                                                        fac_point_line_ptr->getFeaturePointId(),
-                                                                        fac_point_line_ptr->getLandmarkPointId(),
-                                                                        _remain_id,
-                                                                        fac_point_line_ptr->getApplyLossFunction(),
-                                                                        fac_point_line_ptr->getStatus());
+                FactorBase::emplace<FactorPointToLine2D>(fac_ptr->getFeature(), std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
+                                                         std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+                                                         fac_point_line_ptr->getProcessor(),
+                                                         fac_point_line_ptr->getFeaturePointId(),
+                                                         fac_point_line_ptr->getLandmarkPointId(),
+                                                         _remain_id,
+                                                         fac_point_line_ptr->getApplyLossFunction(),
+                                                         fac_point_line_ptr->getStatus());
         }
         else
             throw std::runtime_error ("polyline factor of unknown type");
@@ -292,7 +308,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
             std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
 
             // add new factor
-            fac_ptr->getFeature()->addFactor(new_fac_ptr);
+            // fac_ptr->getFeature()->addFactor(new_fac_ptr);
+            new_fac_ptr->link(fac_ptr->getFeature());
 
             // remove factor
             fac_ptr->remove();
diff --git a/src/map_base.cpp b/src/map_base.cpp
index fdfbf11fbb0965d1ebab126539cb4e5c27bb3a2c..dc9f6e2fd3eaee2c0e5c474409bb88185b5c696d 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -30,9 +30,6 @@ MapBase::~MapBase()
 LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
 {
     landmark_list_.push_back(_landmark_ptr);
-    _landmark_ptr->setMapPtr(shared_from_this());
-    _landmark_ptr->setProblem(getProblem());
-    _landmark_ptr->registerNewStateBlocks();
     return _landmark_ptr;
 }
 
diff --git a/src/problem.cpp b/src/problem.cpp
index c410886afb7395cb445c11b22287db4216f2b76a..5752f79e3cd991de967f8e78ffc71ef27f93a8af 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -79,7 +79,8 @@ Problem::~Problem()
 
 void Problem::addSensor(SensorBasePtr _sen_ptr)
 {
-    getHardware()->addSensor(_sen_ptr);
+    // getHardware()->addSensor(_sen_ptr);
+    _sen_ptr->link(getHardware());
 }
 
 SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
@@ -123,7 +124,8 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
 
     ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params, _corresponding_sensor_ptr);
     prc_ptr->configure(_corresponding_sensor_ptr);
-    _corresponding_sensor_ptr->addProcessor(prc_ptr);
+    // _corresponding_sensor_ptr->addProcessor(prc_ptr);
+    prc_ptr->link(_corresponding_sensor_ptr);
 
     // setting the origin in all processor motion if origin already setted
     if (prc_ptr->isMotion() && prior_is_set_)
@@ -216,7 +218,8 @@ FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
                                    const TimeStamp& _time_stamp)
 {
     FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state);
-    trajectory_ptr_->addFrame(frm);
+    // trajectory_ptr_->addFrame(frm);
+    frm->link(trajectory_ptr_);
     return frm;
 }
 
@@ -681,12 +684,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
         // create origin capture with the given state as data
         // Capture fix only takes 3D position and Quaternion orientation
         CapturePosePtr init_capture;
+        CaptureBasePtr init_capture_base;
+        // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+        // init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
         if (trajectory_ptr_->getFrameStructure() == "POV 3D")
-            init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+            init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
         else
-            init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
+            init_capture_base = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
 
-        origin_keyframe->addCapture(init_capture);
+        init_capture = std::static_pointer_cast<CapturePose>(init_capture_base);
+        // origin_keyframe->addCapture(init_capture);
 
         // emplace feature and factor
         init_capture->emplaceFeatureAndFactor();
diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp
index 8409f78c2572489083d36a6c9deb31c45ed53ee1..ecd1dd23bd1fe60ca1499f54db1bd9ab933da31e 100644
--- a/src/processor/processor_GPS.cpp
+++ b/src/processor/processor_GPS.cpp
@@ -37,14 +37,16 @@ void ProcessorGPS::process(CaptureBasePtr _capture_ptr)
     {
         Eigen::Vector3s sat_pos = obs.measurements_[i].sat_position_;
         Scalar pr = obs.measurements_[i].pseudorange_;
-        capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_));
+        // capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_));
+        FeatureBase::emplace<FeatureGPSPseudorange>(capture_gps_ptr_, sat_pos, pr, gps_covariance_);
     }
     //std::cout << "gps features extracted" << std::endl;
     //std::cout << "Establishing factors to gps features..." << std::endl;
     for (auto i_it = capture_gps_ptr_->getFeatureList().begin();
             i_it != capture_gps_ptr_->getFeatureList().end(); i_it++)
     {
-        capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this()));
+        // capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this()));
+        FactorBase::emplace<FactorGPSPseudorange2D>(capture_gps_ptr_->getFeatureList().front(), (*i_it), shared_from_this());
     }
     //std::cout << "Factors established" << std::endl;
 }
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index 3f1f5b2445ea42bda8b863beddaf52210bbf4c4c..dde8bc48263a8f492fdfb34535e17af68469c347 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -214,12 +214,13 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
 {
     CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
     FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
-    FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
+    // FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
+    auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
 
     // link ot wolf tree
-    _feature_motion->addFactor(fac_imu);
-    _capture_origin->addConstrainedBy(fac_imu);
-    _capture_origin->getFrame()->addConstrainedBy(fac_imu);
+    // _feature_motion->addFactor(fac_imu);
+    // _capture_origin->addConstrainedBy(fac_imu);
+    // _capture_origin->getFrame()->addConstrainedBy(fac_imu);
 
     return fac_imu;
 }
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 84923e65bde5b79627e58af16b6990e3f5f2ef1a..115c4fe9e73c755020bf6aa35102020ef734bbff 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -31,7 +31,8 @@ FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _captur
     std::cout << "Making " << (_type == KEY_FRAME? "key-" : "") << "frame" << std::endl;
 
     FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp());
-    new_frame_ptr->addCapture(_capture_ptr);
+    // new_frame_ptr->addCapture(_capture_ptr);
+    _capture_ptr->link(new_frame_ptr);
 
     return new_frame_ptr;
 }
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
index 2958739d7e6d0c45e8db4d2df31c5c1db2ae5ae0..5ea980e9ac72dad48bc820c37aced78e7cf2a348 100644
--- a/src/processor/processor_capture_holder.cpp
+++ b/src/processor/processor_capture_holder.cpp
@@ -52,7 +52,8 @@ void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
 
     if (frame_ptr != _keyframe_ptr)
     {
-      _keyframe_ptr->addCapture(existing_capture);
+      // _keyframe_ptr->addCapture(existing_capture);
+        existing_capture->link(_keyframe_ptr);
 
       //WOLF_INFO("Adding capture laser !");
 
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 4e307962e94a3fc2e205cf7cd1c80f480d2871c4..f6fc883f8f5a96ac2c213ab88fc0fdb5c76be8c8 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -212,12 +212,13 @@ CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
 FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
                                                         CaptureBasePtr _capture_origin)
 {
-  FactorOdom2DPtr fac_odom =
-      std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
-                                         shared_from_this());
-
-  _feature->addFactor(fac_odom);
-  _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+  // FactorOdom2DPtr fac_odom =
+  //     std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
+  //                                        shared_from_this());
+  auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
+                                    shared_from_this());
+  // _feature->addFactor(fac_odom);
+  // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
 
   return fac_odom;
 }
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4798ead84ec1c9ebdf1bea82bd2155bf9d17f454..0004da85f26d9e0b74fae44e19645a8ecf8702ea 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -571,14 +571,16 @@ CaptureMotionPtr ProcessorMotion::emplaceCapture(const FrameBasePtr& _frame_own,
     capture->setCalibrationPreint(_calib_preint);
 
     // add to wolf tree
-    _frame_own->addCapture(capture);
+    // _frame_own->addCapture(capture);
+    capture->link(_frame_own);
     return capture;
 }
 
 FeatureBasePtr ProcessorMotion::emplaceFeature(CaptureMotionPtr _capture_motion)
 {
     FeatureBasePtr feature = createFeature(_capture_motion);
-    _capture_motion->addFeature(feature);
+    // _capture_motion->addFeature(feature);
+    feature->link(_capture_motion);
     return feature;
 }
 
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index cf10be08a27ce9e9af902b86a2117a4bfbfa6c2f..8d6878daf68c9a2b9ff4ea33a7e8b35aed46ea5e 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -154,10 +154,12 @@ CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const Sens
 
 FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
 {
-    FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
-                                                                      shared_from_this());
-    _feature->addFactor(fac_odom);
-    _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+    // FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
+    //                                                           shared_from_this());
+    auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature, _feature, _capture_origin->getFrame(),
+                                                      shared_from_this());
+    // _feature->addFactor(fac_odom);
+    // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
     return fac_odom;
 }
 
@@ -189,13 +191,6 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const
     return prc_ptr;
 }
 
-void ProcessorOdom2D::link(SensorBasePtr _sen_ptr)
-{
-    std::cout << "Linking ProcessorOdom2D" << std::endl;
-    _sen_ptr->addProcessor(shared_from_this());
-    this->setSensorPtr(_sen_ptr);
-}
-
 }
 
 // Register in the ProcessorFactory
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 7415459711b62482df8a0de5d32bb56a496180d2..c2b03c307b0a5eaf970a55dbe0d6d4bdac2e5ee7 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -405,10 +405,12 @@ CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const Sens
 
 FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
 {
-    FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
-                                                                      shared_from_this());
-    _feature_motion->addFactor(fac_odom);
-    _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+    // FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
+    //                                                                   shared_from_this());
+    auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion, _feature_motion, _capture_origin->getFrame(),
+                                                      shared_from_this());
+    // _feature_motion->addFactor(fac_odom);
+    // _capture_origin->getFrame()->addConstrainedBy(fac_odom);
     return fac_odom;
 }
 
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index e502ba296a1a4fee6a355839c9d7a6d52871a54a..9693861d3630bbda063a2283bd899277344ea556 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -58,7 +58,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
             // Append incoming to KF
-            pack->key_frame->addCapture(incoming_ptr_);
+            // pack->key_frame->addCapture(incoming_ptr_);
+            incoming_ptr_->link(pack->key_frame);
 
             // Process info
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
@@ -76,7 +77,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         case FIRST_TIME_WITHOUT_PACK :
         {
             FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp());
-            kfrm->addCapture(incoming_ptr_);
+            // kfrm->addCapture(incoming_ptr_);
+            incoming_ptr_->link(kfrm);
 
             // Process info
             processKnown();
@@ -102,7 +104,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         case SECOND_TIME_WITHOUT_PACK :
         {
             FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
-            frm->addCapture(incoming_ptr_);
+            // frm->addCapture(incoming_ptr_);
+            incoming_ptr_->link(frm);
 
             // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
 
@@ -134,11 +137,14 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             FrameBasePtr last_old_frame = last_ptr_->getFrame();
             last_old_frame->unlinkCapture(last_ptr_);
             last_old_frame->remove();
-            pack->key_frame->addCapture(last_ptr_);
+            // pack->key_frame->addCapture(last_ptr_);
+            last_ptr_->link(pack->key_frame);
 
             // Create new frame
             FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
-            frm->addCapture(incoming_ptr_);
+            // frm->addCapture(incoming_ptr_);
+            incoming_ptr_->link(frm);
+
 
             // Detect new Features, initialize Landmarks, create Factors, ...
             processNew(params_tracker_->max_new_features);
@@ -174,7 +180,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
 
                 // make F; append incoming to new F
                 FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
-                frm->addCapture(incoming_ptr_);
+                // frm->addCapture(incoming_ptr_);
+                incoming_ptr_->link(frm);
 
                 // process
                 processNew(params_tracker_->max_new_features);
@@ -203,7 +210,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 // We do not create a KF
 
                 // Advance this
-                last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+                // last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+                incoming_ptr_->link(last_ptr_->getFrame());
                 last_ptr_->remove();
                 incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
 
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 670f3aaf776d371bc1db000d84184f5643cc8e2e..4026f4324d08231190e098ea2d94d6a5803df360 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -148,19 +148,21 @@ void ProcessorTrackerFeature::establishFactors()
         FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
         FeatureBasePtr feature_in_last   = pair_trkid_pair.second.second;
 
-        auto fac_ptr  = createFactor(feature_in_last, feature_in_origin);
-        feature_in_last  ->addFactor(fac_ptr);
-        feature_in_origin->addConstrainedBy(fac_ptr);
 
-        if (fac_ptr != nullptr) // factor links
-        {
-            FrameBasePtr frm = fac_ptr->getFrameOther();
-            if (frm)
-                frm->addConstrainedBy(fac_ptr);
-            CaptureBasePtr cap = fac_ptr->getCaptureOther();
-            if (cap)
-                cap->addConstrainedBy(fac_ptr);
-        }
+        auto fac_ptr  = createFactor(feature_in_last, feature_in_origin);
+        // feature_in_last  ->addFactor(fac_ptr);
+        fac_ptr->link(feature_in_last);
+        // feature_in_origin->addConstrainedBy(fac_ptr);
+
+        // if (fac_ptr != nullptr) // factor links
+        // {
+        //     FrameBasePtr frm = fac_ptr->getFrameOther();
+        //     if (frm)
+        //         frm->addConstrainedBy(fac_ptr);
+        //     CaptureBasePtr cap = fac_ptr->getCaptureOther();
+        //     if (cap)
+        //         cap->addConstrainedBy(fac_ptr);
+        // }
 
 
         WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 4414d8caf499e187e28961b54f8a65e80455716a..c68ea29713f8977c86f0621852d767c846e1a500 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -132,7 +132,8 @@ FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _featur
                                                           _feature_ptr->getMeasurement()(3));
 
         // Add landmark factor to the other feature
-        _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this()));
+        // _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this()));
+        FactorBase::emplace<FactorCorner2D>(_feature_other_ptr, _feature_other_ptr, landmark_ptr, shared_from_this());
     }
 
 //    std::cout << "creating factor: last feature " << _feature_ptr->getMeasurement()
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 980c652af00ecb490250ac0baabb375ddf5ba98e..be79d8c9a60ca4b8670e1309498fed670b9b5b88 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -416,12 +416,13 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
                 FeatureBasePtr ftr_last = pair_trkid_match.second.second;
 
                 // make factor
-                FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE);
+                // FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE);
+                auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE);
 
                 // link to wolf tree
-                ftr_last->addFactor(ctr);
-                ftr_orig->addConstrainedBy(ctr);
-                ftr_prev->addConstrainedBy(ctr);
+                // ftr_last->addFactor(ctr);
+                // ftr_orig->addConstrainedBy(ctr);
+                // ftr_prev->addConstrainedBy(ctr);
             }
         }
     }
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 3d5b75c8fcc6e9ef70ef396c31a6960a5aa3a016..ba2bf986ef9065e0568ca630299c10728d7ffc07 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -135,14 +135,15 @@ void ProcessorTrackerLandmark::establishFactors()
                                                      lmk);
         if (fac_ptr != nullptr) // factor links
         {
-            last_feature->addFactor(fac_ptr);
-            lmk->addConstrainedBy(fac_ptr);
-            FrameBasePtr frm = fac_ptr->getFrameOther();
-            if (frm)
-                frm->addConstrainedBy(fac_ptr);
-            CaptureBasePtr cap = fac_ptr->getCaptureOther();
-            if (cap)
-                cap->addConstrainedBy(fac_ptr);
+            // last_feature->addFactor(fac_ptr);
+            fac_ptr->link(last_feature);
+            // lmk->addConstrainedBy(fac_ptr);
+            // FrameBasePtr frm = fac_ptr->getFrameOther();
+            // if (frm)
+            //     frm->addConstrainedBy(fac_ptr);
+            // CaptureBasePtr cap = fac_ptr->getCaptureOther();
+            // if (cap)
+            //     cap->addConstrainedBy(fac_ptr);
         }
     }
 }
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
index a8037d037e0cad2f3739a4c1766ae86dcdeccc89..e39e4633e731b06dd7d003e27c92cd1e54f9e4fd 100644
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ b/src/processor/processor_tracker_landmark_polyline.cpp
@@ -813,8 +813,10 @@ void ProcessorTrackerLandmarkPolyline::establishFactors()
                 if (lmk_next_point_id > polyline_landmark->getLastId())
                     lmk_next_point_id -= polyline_landmark->getNPoints();
                 //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
-                last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
-                                                                                      ftr_point_id, lmk_point_id, lmk_next_point_id));
+                // last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
+                //                                                                       ftr_point_id, lmk_point_id, lmk_next_point_id));
+                FactorBase::emplace<FactorPointToLine2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(),
+                                                         ftr_point_id, lmk_point_id, lmk_next_point_id);
                 //std::cout << "factor added" << std::endl;
             }
 
@@ -826,8 +828,10 @@ void ProcessorTrackerLandmarkPolyline::establishFactors()
                 if (lmk_prev_point_id < polyline_landmark->getFirstId())
                     lmk_prev_point_id += polyline_landmark->getNPoints();
                 //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
-                last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
-                                                                                      ftr_point_id, lmk_point_id, lmk_prev_point_id));
+                // last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
+                //                                                                       ftr_point_id, lmk_point_id, lmk_prev_point_id));
+                FactorBase::emplace<FactorPointToLine2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(),
+                                                         ftr_point_id, lmk_point_id, lmk_prev_point_id);
                 //std::cout << "factor added" << std::endl;
             }
 
@@ -839,8 +843,10 @@ void ProcessorTrackerLandmarkPolyline::establishFactors()
 				//std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
 				//std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
 				//std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
-                last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
-                                                                                ftr_point_id, lmk_point_id));
+                // last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
+                //                                                                 ftr_point_id, lmk_point_id));
+                FactorBase::emplace<FactorPoint2D>(last_feature, polyline_feature, polyline_landmark, shared_from_this(),
+                                                   ftr_point_id, lmk_point_id);
                 //std::cout << "factor added" << std::endl;
             }
         }
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index bd9cfe28031895a1da00c1574a64e9488035f2ae..55a5c8446248e61d2059d4a23600534a3d59fa98 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -177,10 +177,12 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
     ftr_prior->setProblem(getProblem());
 
     // create & add factor absolute
+    // ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
+    // ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
     if (is_quaternion)
-        ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
+        FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb);
     else
-        ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
+        FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size);
 
     // store feature in params_prior_map_
     params_prior_map_[_i] = ftr_prior;
@@ -334,8 +336,6 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr)
 ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
 {
     processor_list_.push_back(_proc_ptr);
-    _proc_ptr->setSensorPtr(shared_from_this());
-    _proc_ptr->setProblem(getProblem());
     return _proc_ptr;
 }
 
@@ -409,6 +409,8 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
     std::cout << "Linking SensorBase" << std::endl;
     this->setHardwarePtr(_hw_ptr);
     this->getHardware()->addSensor(shared_from_this());
+    this->setProblem(_hw_ptr->getProblem());
+    this->registerNewStateBlocks();
 }
 
 } // namespace wolf
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index cf01f2d788ed6a28390a460ee526e566412c9bb6..04bbce792fce08bb662a998c3e2d89a1b0122c3e 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -18,13 +18,8 @@ TrajectoryBase::~TrajectoryBase()
 
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
-    // link up
-    _frame_ptr->setTrajectoryPtr(shared_from_this());
-    _frame_ptr->setProblem(getProblem());
-
     if (_frame_ptr->isKey())
     {
-        _frame_ptr->registerNewStateBlocks();
         if (last_key_frame_ptr_ == nullptr || last_key_frame_ptr_->getTimeStamp() < _frame_ptr->getTimeStamp())
             last_key_frame_ptr_ = _frame_ptr;
 
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index b6f183675277a9638a85189dbafc5940578a1c84..1d4d6b866eab77300671b0b3fa827372e613b35d 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -76,7 +76,8 @@ TEST(CaptureBase, Dynamic_sensor_params)
 TEST(CaptureBase, addFeature)
 {
     CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
-    FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
+    // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity());
     ASSERT_FALSE(C->getFeatureList().empty());
     ASSERT_EQ(C->getFeatureList().front(), f);
 }
@@ -84,7 +85,8 @@ TEST(CaptureBase, addFeature)
 TEST(CaptureBase, addFeatureList)
 {
     CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
-    FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
+    // FeatureBasePtr f_first = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
+    auto f_first = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity());
     ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
 
     // make a list to add
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 3f92fc0872e1f1869a998c0acda2fffaad25f3e3..773e5ce58adc2de0df639569d3c6bc9c847df182 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -319,9 +319,12 @@ TEST(CeresManager, AddFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
 
     // update solver
     ceres_manager_ptr->update();
@@ -341,9 +344,12 @@ TEST(CeresManager, DoubleAddFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
 
     // add factor again
     P->addFactor(c);
@@ -366,9 +372,12 @@ TEST(CeresManager, RemoveFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
 
     // update solver
     ceres_manager_ptr->update();
@@ -394,9 +403,12 @@ TEST(CeresManager, AddRemoveFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
 
     ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
 
@@ -423,9 +435,12 @@ TEST(CeresManager, DoubleRemoveFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f,f));
 
     // update solver
     ceres_manager_ptr->update();
@@ -550,10 +565,14 @@ TEST(CeresManager, FactorsRemoveLocalParam)
 
     // Create (and add) 2 factors quaternion
     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()));
-    FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
-    FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+    // CaptureBasePtr                  C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr                  f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+    FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
+    // FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+    FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
 
     // update solver
     ceres_manager_ptr->update();
@@ -592,10 +611,14 @@ TEST(CeresManager, FactorsUpdateLocalParam)
 
     // Create (and add) 2 factors quaternion
     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()));
-    FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
-    FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+    // CaptureBasePtr                  C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr                  f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+    FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
+    // FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+    FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO()));
 
     // update solver
     ceres_manager_ptr->update();
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
index 6c8990c291af5bb43dc9e042f9ca60dc84854e75..1dd8cbb7d98f579d30e71aeb4306d45a0b38b052 100644
--- a/test/gtest_factor_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -2457,10 +2457,13 @@ 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));
-    FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("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)));
-    
+    // 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 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));
+
     //prepare problem for solving
     origin_KF->getP()->fix();
     origin_KF->getO()->unfix();
@@ -2515,10 +2518,13 @@ 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));
-    FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("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)));
-    
+    // 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 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));
+
     //prepare problem for solving
     origin_KF->getP()->fix();
     origin_KF->getO()->unfix();
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 204c6ce633a36380d46eacbc65d66abe1a7747fe..a651aae785de3864892d4f62652ebf5fcfc2c105 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -38,7 +38,8 @@ CeresManager ceres_mgr(problem_ptr);
 FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
+// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
 
 ////////////////////////////////////////////////////////
 /* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
@@ -48,8 +49,10 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS"
 
 TEST(FactorBlockAbs, fac_block_abs_p)
 {
-    FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
-    fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
+    // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
+    // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
+    FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP());
 
     ASSERT_TRUE(problem_ptr->check(0));
 
@@ -66,8 +69,10 @@ TEST(FactorBlockAbs, fac_block_abs_p)
 
 TEST(FactorBlockAbs, fac_block_abs_p_tail2)
 {
-    FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
-    fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
+    // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
+    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
+    // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
+    FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2);
 
     // Unfix frame 0, perturb frm0
     frm0->unfix();
@@ -82,8 +87,10 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
 
 TEST(FactorBlockAbs, fac_block_abs_v)
 {
-    FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
-    fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
+    // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
+    // fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
+    FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV());
 
     ASSERT_TRUE(problem_ptr->check(0));
     
@@ -100,8 +107,10 @@ TEST(FactorBlockAbs, fac_block_abs_v)
 
 TEST(FactorQuatAbs, fac_block_abs_o)
 {
-    FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
-    fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
+    // FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
+    // fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
+    FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO());
 
     ASSERT_TRUE(problem_ptr->check(0));
     
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index a053b3f82438b8ac9c358fc45b4b656dd9df65ff..640694b774c8e4febc008454319b95cfb72b5bbf 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -29,17 +29,20 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
     SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
-    CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
-    fr2_ptr->addCapture(capture_ptr);
+    // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+    // fr2_ptr->addCapture(capture_ptr);
+    auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
 
     // FEATURE
-    FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
-    capture_ptr->addFeature(feature_ptr);
+    // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
+    // capture_ptr->addFeature(feature_ptr);
+    auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
 
-    // CONSTRAINT
-    FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
-    feature_ptr->addFactor(factor_ptr);
-    fr1_ptr->addConstrainedBy(factor_ptr);
+    // FACTOR
+    // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+    // feature_ptr->addFactor(factor_ptr);
+    // fr1_ptr->addConstrainedBy(factor_ptr);
+    auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
 
     ASSERT_TRUE(factor_ptr->getFeature());
     ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -64,19 +67,24 @@ TEST(CaptureAutodiff, ResidualOdom2d)
     SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
-    CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
-    fr2_ptr->addCapture(capture_ptr);
+    // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+    // fr2_ptr->addCapture(capture_ptr);
+    auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
+
 
     // FEATURE
     Eigen::Vector3s d;
     d << -1, -4, M_PI/2;
-    FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
-    capture_ptr->addFeature(feature_ptr);
+    // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+    // capture_ptr->addFeature(feature_ptr);
+    auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+
+    // FACTOR
+    // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+    // feature_ptr->addFactor(factor_ptr);
+    // fr1_ptr->addConstrainedBy(factor_ptr);
 
-    // CONSTRAINT
-    FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
-    feature_ptr->addFactor(factor_ptr);
-    fr1_ptr->addConstrainedBy(factor_ptr);
+    auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
 
     // EVALUATE
 
@@ -111,19 +119,22 @@ TEST(CaptureAutodiff, JacobianOdom2d)
     SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
-    CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
-    fr2_ptr->addCapture(capture_ptr);
+    // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+    // fr2_ptr->addCapture(capture_ptr);
+    auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
 
     // FEATURE
     Eigen::Vector3s d;
     d << -1, -4, M_PI/2;
-    FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
-    capture_ptr->addFeature(feature_ptr);
+    // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+    // capture_ptr->addFeature(feature_ptr);
+    auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
 
-    // CONSTRAINT
-    FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
-    feature_ptr->addFactor(factor_ptr);
-    fr1_ptr->addConstrainedBy(factor_ptr);
+    // FACTOR
+    // FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+    // feature_ptr->addFactor(factor_ptr);
+    // fr1_ptr->addConstrainedBy(factor_ptr);
+    auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
 
     // COMPUTE JACOBIANS
 
@@ -193,22 +204,26 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
     SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
 
     // CAPTURE
-    CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
-    fr2_ptr->addCapture(capture_ptr);
+    // CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+    // fr2_ptr->addCapture(capture_ptr);
+    auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
 
     // FEATURE
     Eigen::Vector3s d;
     d << -1, -4, M_PI/2;
-    FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
-    capture_ptr->addFeature(feature_ptr);
-
-    // CONSTRAINTS
-    FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
-    feature_ptr->addFactor(fac_autodiff_ptr);
-    fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
-    FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
-    feature_ptr->addFactor(fac_analytic_ptr);
-    fr1_ptr->addConstrainedBy(fac_analytic_ptr);
+    // FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+    // capture_ptr->addFeature(feature_ptr);
+    auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+
+    // FACTOR
+    // FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+    // feature_ptr->addFactor(fac_autodiff_ptr);
+    // fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
+    auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+    // FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
+    // feature_ptr->addFactor(fac_analytic_ptr);
+    // fr1_ptr->addConstrainedBy(fac_analytic_ptr);
+    auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr);
 
     // COMPUTE JACOBIANS
 
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 7559bfa7582aa1449232391806e498fb294d03be..ac4ec5a9cc27bcecc67a368efedfcff1d2245a94 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -58,13 +58,16 @@ class FactorAutodiffDistance3D_Test : public testing::Test
             F1 = problem->emplaceFrame        (KEY_FRAME, pose1, 1.0);
 
             F2 = problem->emplaceFrame        (KEY_FRAME, pose2, 2.0);
-            C2 = std::make_shared<CaptureBase>("Base", 1.0);
-            F2->addCapture(C2);
-            f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
-            C2->addFeature(f2);
-            c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE);
-            f2->addFactor(c2);
-            F1->addConstrainedBy(c2);
+            // C2 = std::make_shared<CaptureBase>("Base", 1.0);
+            // F2->addCapture(C2);
+            C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
+            // f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
+            // C2->addFeature(f2);
+            f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
+            // c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE);
+            // f2->addFactor(c2);
+            // F1->addConstrainedBy(c2);
+            c2 = std::static_pointer_cast<FactorAutodiffDistance3D>(FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE));
 
         }
 
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 71401381fe35c768d047b8324e0b2e2f74df35c4..496a1ffdba3785a5891f0ad8bbfbb9d73d88da66 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -701,19 +701,26 @@ class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
             cv123.push_back(c123);
             for (size_t i=1; i<c1p_can.cols() ; i++)
             {
-                fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov));
-                I1->addFeature(fv1.at(i));
-
-                fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov));
-                I2->addFeature(fv2.at(i));
-
-                fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov));
-                I3->addFeature(fv3.at(i));
-
-                cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
-                fv3.at(i)->addFactor(cv123.at(i));
-                fv1.at(i)->addConstrainedBy(cv123.at(i));
-                fv2.at(i)->addConstrainedBy(cv123.at(i));
+                // fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov));
+                auto f  = FeatureBase::emplace<FeatureBase>(I1, "PIXEL", c1p_can.col(i), pix_cov);
+                fv1.push_back(f);
+                // I1->addFeature(fv1.at(i));
+
+                // fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov));
+                auto f2  = FeatureBase::emplace<FeatureBase>(I2, "PIXEL", c2p_can.col(i), pix_cov);
+                fv2.push_back(f2);
+                // I2->addFeature(fv2.at(i));
+
+                // fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov));
+                auto f3  = FeatureBase::emplace<FeatureBase>(I3, "PIXEL", c3p_can.col(i), pix_cov);
+                fv3.push_back(f3);
+                // I3->addFeature(fv3.at(i));
+
+                auto ff = std::static_pointer_cast<FactorAutodiffTrifocal>(FactorBase::emplace<FactorAutodiffTrifocal>(fv3.at(i), fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
+                cv123.push_back(ff);
+                // fv3.at(i)->addFactor(cv123.at(i));
+                // fv1.at(i)->addConstrainedBy(cv123.at(i));
+                // fv2.at(i)->addConstrainedBy(cv123.at(i));
             }
 
         }
@@ -884,13 +891,16 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
     Scalar distance     = sqrt(2.0);
     Scalar distance_std = 0.1;
 
-    CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp());
-    F3->addCapture(Cd);
-    FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std));
-    Cd->addFeature(fd);
-    FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE);
-    fd->addFactor(cd);
-    F1->addConstrainedBy(cd);
+    auto Cd = CaptureBase::emplace<CaptureBase>(F3, "DISTANCE", F3->getTimeStamp());
+    // CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp());
+    // F3->addCapture(Cd);
+    auto fd = FeatureBase::emplace<FeatureBase>(Cd, "DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std));
+    // FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std));
+    // Cd->addFeature(fd);
+    auto cd = FactorBase::emplace<FactorAutodiffDistance3D>(fd, fd, F1, nullptr, false, FAC_ACTIVE);
+    // FACTORAUTODIFFDISTANCE3DPTR cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE);
+    // fd->addFactor(cd);
+    // F1->addConstrainedBy(cd);
 
     cd->setStatus(FAC_INACTIVE);
     std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index c3f767d56ea04682d2e33651c4e4e1a89f865585..e4c754ddadacc39a5bf81086052ff0426d8d3dfa 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -41,10 +41,13 @@ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(
 FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
 
 // Capture, feature and factor from frm1 to frm0
-CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
-FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
-FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add
-FactorBasePtr dummy = frm0->addConstrainedBy(ctr1);
+// CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
+auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "ODOM 3D", 1, nullptr, delta, 7, 6, nullptr);
+// FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "ODOM 3D", delta, data_cov);
+// FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add
+FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr)); // create and add
+// FactorBasePtr dummy = frm0->addConstrainedBy(ctr1);
 
 ////////////////////////////////////////////////////////
 
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index d7bce0c84f734613f1a2f143bebf2690897149f2..fdcd2d624ce5841a10351cb1ff2b7d2e6b6fdf24 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -33,9 +33,12 @@ CeresManager ceres_mgr(problem);
 FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor from frm1 to frm0
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
-FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
+// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 2D", 0, nullptr, pose, 3, 3, nullptr);
+// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 2D", pose, data_cov);
+// FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
+FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(fea0, fea0));
 
 ////////////////////////////////////////////////////////
 
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 0af4c3c406f02c9c87cabb35b5e43fd7137fe28b..ab7f30ecf774254f715ee64a034949ec9dfb7fa7 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -39,9 +39,12 @@ CeresManager ceres_mgr(problem);
 FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor
-CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
-FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
-FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0)));
+// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr);
+// FEATUREBASEPTR fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "ODOM 3D", pose7, data_cov);
+// FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0)));
+FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(fea0, fea0));
 
 ////////////////////////////////////////////////////////
 
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 2e8245e1544c1a51e280a0874a24d400527a1466..6ba19ec9cec9d468625f3178d6877bfd9e160f5c 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -77,8 +77,9 @@ class FeatureIMU_test : public testing::Test
     //create Frame
         ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
         state_vec = problem->getProcessorMotion()->getCurrentState();
-   	    last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
-        problem->getTrajectory()->addFrame(last_frame);
+   	    // last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
+        last_frame = FrameBase::emplace<FrameBase>(problem->getTrajectory(), KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
+        // problem->getTrajectory()->addFrame(last_frame);
         
     //create a feature
         delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 06436282a934f4b61bcfe04e50c77b1f3f27eea8..41334dfe8ae0f1e4f97648cff841e3076308fff5 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -67,29 +67,37 @@ TEST(FrameBase, LinksToTree)
     IntrinsicsOdom2D intrinsics_odo;
     intrinsics_odo.k_disp_to_disp = 1;
     intrinsics_odo.k_rot_to_rot = 1;
-    SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
-    P->getHardware()->addSensor(S);
-    FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-    T->addFrame(F1);
-    FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-    T->addFrame(F2);
-    CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
-    F1->addCapture(C);
+    // SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
+    // P->getHardware()->addSensor(S);
+    auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo);
+    // FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    // T->addFrame(F1);
+    auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    // FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    // T->addFrame(F2);
+    auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    // CaptureMotionPtr C = make_shared<CaptureMotion>("MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
+    // F1->addCapture(C);
+    auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
     /// @todo link sensor & proccessor
     ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
-    FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
-    C->addFeature(f);
-    FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p);
-    f->addFactor(c);
-
+    // FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
+    // C->addFeature(f);
+    auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
+    // FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p);
+    // f->addFactor(c);
+    auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p);
+
+    //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
     // c-by link F2 -> c not yet established
-    ASSERT_TRUE(F2->getConstrainedByList().empty());
+    // ASSERT_TRUE(F2->getConstrainedByList().empty());
+    ASSERT_FALSE(F2->getConstrainedByList().empty());
 
     // tree is inconsistent since we are missing the constrained_by link
-    ASSERT_FALSE(P->check(0));
+    // ASSERT_FALSE(P->check(0));
 
     // establish constrained_by link F2 -> c
-    F2->addConstrainedBy(c);
+    // F2->addConstrainedBy(c);
 
     // tree is now consistent
     ASSERT_TRUE(P->check(0));
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 5f479b19ec09a04c604aaca010c98cfd6bd37c6f..78f5ee81cb8b882640b78753c9ebf82c8015b3e5 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -130,18 +130,24 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
     // KF1 and motion from KF0
     t += dt;
     FrameBasePtr        F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
-    CaptureBasePtr      C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
-    FeatureBasePtr      f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
-    FactorBasePtr   c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr));
-    F0->addConstrainedBy(c1);
+    // CaptureBasePtr      C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
+    auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t);
+    // FeatureBasePtr      f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
+    auto f1 = FeatureBase::emplace<FeatureBase>(C1, "ODOM 2D", delta, delta_cov);
+    // FactorBasePtr   c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr));
+    // F0->addConstrainedBy(c1);
+    auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr);
 
     // KF2 and motion from KF1
     t += dt;
     FrameBasePtr        F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
-    CaptureBasePtr      C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
-    FeatureBasePtr      f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
-    FactorBasePtr   c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr));
-    F1->addConstrainedBy(c2);
+    // CaptureBasePtr      C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
+    auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t);
+    // FeatureBasePtr      f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
+    auto f2 = FeatureBase::emplace<FeatureBase>(C2, "ODOM 2D", delta, delta_cov);
+    // FactorBasePtr   c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr));
+    // F1->addConstrainedBy(c2);
+    auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr);
 
     ASSERT_TRUE(Pr->check(0));
 
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 8ee7623dc68b7f637677fd24ceb6f90b083a4b39..09d758e651873862fbfd746dbf3cb351dceb8e5f 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -37,8 +37,9 @@ TEST(Problem, Sensors)
     ProblemPtr P = Problem::create("POV 3D");
 
     // add a dummy sensor
-    SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
-    P->addSensor(S);
+    // SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
+    // P->addSensor(S);
+    auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
 
     // check pointers
     ASSERT_EQ(P, S->getProblem());
@@ -54,12 +55,14 @@ TEST(Problem, Processor)
     ASSERT_FALSE(P->getProcessorMotion());
 
     // add a motion sensor and processor
-    SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
-    P->addSensor(Sm);
+    // SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
+    // P->addSensor(Sm);
+    auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D());
 
     // add motion processor
-    ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
-    Sm->addProcessor(Pm);
+    // ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
+    // Sm->addProcessor(Pm);
+    auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()));
 
     // check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers
     ASSERT_FALSE(P->getProcessorMotion());
@@ -83,8 +86,9 @@ TEST(Problem, Installers)
     params->time_tolerance = 0.1;
     params->max_new_features = 5;
     params->min_features_for_keyframe = 10;
-    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
-    S->addProcessor(pt);
+    // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
+    auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(S, ProcessorTrackerFeatureDummy(params));
+    // S->addProcessor(pt);
 
     // check motion processor IS NOT set
     ASSERT_FALSE(P->getProcessorMotion());
@@ -219,9 +223,9 @@ TEST(Problem, StateBlocks)
     params->time_tolerance            = 0.1;
     params->max_new_features          = 5;
     params->min_features_for_keyframe = 10;
-    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
-
-    St->addProcessor(pt);
+    // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
+    auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
+    // St->addProcessor(pt);
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
     // 2 state blocks, estimated
@@ -253,9 +257,10 @@ TEST(Problem, Covariances)
     params->time_tolerance            = 0.1;
     params->max_new_features          = 5;
     params->min_features_for_keyframe = 10;
-    ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
+    // ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(params));
+    auto pt = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(St, ProcessorTrackerFeatureDummy(params));
 
-    St->addProcessor(pt);
+    // St->addProcessor(pt);
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
     // 4 state blocks, estimated
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 7479ca984eda6dff7504595c66a959ae1d3aebfa..754229c69d1dc25cede623abcc5f66528ceb75ea 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -38,18 +38,22 @@ TEST(ProcessorBase, KeyFrameCallback)
     ProblemPtr problem = Problem::create("PO 2D");
 
     // Install tracker (sensor and processor)
-    SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
-                                                     std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
-                                                     std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+    // SensorBasePtr sens_trk = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+    //                                                  std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+    //                                                  std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
 
+    auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(), "FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+                                                    std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+                                                    std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
     params->time_tolerance            = dt/2;
     params->max_new_features          = 5;
     params->min_features_for_keyframe = 5;
-    shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params);
+    // shared_ptr<ProcessorTrackerFeatureDummy> proc_trk = make_shared<ProcessorTrackerFeatureDummy>(params);
+    auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, params);
 
-    problem->addSensor(sens_trk);
-    sens_trk->addProcessor(proc_trk);
+    // problem->addSensor(sens_trk);
+    // sens_trk->addProcessor(proc_trk);
 
     // Install odometer (sensor and processor)
     SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index a3e18e59c74962936a906681143f9003fa9d99f0..b37be3fa117325cf8f9b5c227ca44a5e5a62b01b 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -72,23 +72,39 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
   auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>());
 
   // create Keyframes
-  F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
-                                         1,
-                                         stateblock_pptr1,
-                                         stateblock_optr1);
-  F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
-                                         1,
-                                         stateblock_pptr2,
-                                         stateblock_optr2);
-  F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
-                                         1,
-                                         stateblock_pptr3,
-                                         stateblock_optr3);
-  F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
-                                         1,
-                                         stateblock_pptr4,
-                                         stateblock_optr4);
-
+  // F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  //                                        1,
+  //                                        stateblock_pptr1,
+  //                                        stateblock_optr1);
+  // F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  //                                        1,
+  //                                        stateblock_pptr2,
+  //                                        stateblock_optr2);
+  // F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  //                                        1,
+  //                                        stateblock_pptr3,
+  //                                        stateblock_optr3);
+  // F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME,
+  //                                        1,
+  //                                        stateblock_pptr4,
+  //                                        stateblock_optr4);
+
+  F1 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME,
+                                                 1,
+                                                 stateblock_pptr1,
+                                                 stateblock_optr1);
+  F2 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME,
+                                                 1,
+                                                 stateblock_pptr2,
+                                                 stateblock_optr2);
+  F3 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME,
+                                                 1,
+                                                 stateblock_pptr3,
+                                                 stateblock_optr3);
+  F4 = wolf::FrameBase::emplace<wolf::FrameBase>(problem->getTrajectory(),wolf::KEY_FRAME,
+                                                 1,
+                                                 stateblock_pptr4,
+                                                 stateblock_optr4);
   // add dummy capture
   capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
                                                       1.0,
@@ -99,10 +115,10 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
   F4->addCapture(capture_dummy);
 
   // Add first three states to trajectory
-  problem->getTrajectory()->addFrame(F1);
-  problem->getTrajectory()->addFrame(F2);
-  problem->getTrajectory()->addFrame(F3);
-  problem->getTrajectory()->addFrame(F4);
+  // problem->getTrajectory()->addFrame(F1);
+  // problem->getTrajectory()->addFrame(F2);
+  // problem->getTrajectory()->addFrame(F3);
+  // problem->getTrajectory()->addFrame(F4);
 
   // Add same covariances for all states
   Eigen::Matrix2s position_covariance_matrix;
@@ -216,19 +232,21 @@ int main(int argc, char **argv)
   processor_params_point2d->buffer_size_ = 0;         // just exclude identical frameIDs
   processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE;
 
-  processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d);
+  // processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d);
+  processor_ptr_point2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_point2d));
   processor_ptr_point2d->setName("processor2Dpoint");
 
-  sensor_ptr->addProcessor(processor_ptr_point2d);
+  // sensor_ptr->addProcessor(processor_ptr_point2d);
 
   processor_params_ellipse2d->probability_ = 0.95;
   processor_params_ellipse2d->buffer_size_ = 0;         // just exclude identical frameIDs
   processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
 
-  processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d);
+  // processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d);
+  processor_ptr_ellipse2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_ellipse2d));
   processor_ptr_ellipse2d->setName("processor2Dellipse");
 
-  sensor_ptr->addProcessor(processor_ptr_ellipse2d);
+  // sensor_ptr->addProcessor(processor_ptr_ellipse2d);
 
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 6819cba18e0ec605a6272e8ac6e05e44cf58a6d3..71154fd5fc333a498c9e4c2e4ea9ffe7a30e0918 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -77,9 +77,11 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
     IntrinsicsCameraPtr intr = make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
     intr->width  = 640;
     intr->height = 480;
-    SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(),
-                                                         intr);
+    // SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(),
+    //                                                      intr);
 
+    auto sens_trk = SensorBase::emplace<SensorCamera>(problem->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(),
+                                                      intr);
     ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
 //    params_tracker_feature_trifocal->name                           = "trifocal";
     params_tracker_feature_trifocal->pixel_noise_std                = 1.0;
@@ -93,11 +95,12 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
     params_tracker_feature_trifocal->max_euclidean_distance         = 20;
     params_tracker_feature_trifocal->yaml_file_params_vision_utils  = wolf_root + "/src/examples/ACTIVESEARCH.yaml";
 
-    ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal);
+    // ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal);
+    auto proc_trk = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(ProcessorBase::emplace<ProcessorTrackerFeatureTrifocal>(sens_trk, params_tracker_feature_trifocal));
     proc_trk->configure(sens_trk);
 
-    problem->addSensor(sens_trk);
-    sens_trk->addProcessor(proc_trk);
+    // problem->addSensor(sens_trk);
+    // sens_trk->addProcessor(proc_trk);
 
     // Install odometer (sensor and processor)
     IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();
@@ -121,7 +124,8 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
     // Track
     cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols)
     image *= 0; // TODO see if we want to use a real image
-    CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
+    SensorCameraPtr sens_trk_cam = std::static_pointer_cast<SensorCamera>(sens_trk);
+    CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
     proc_trk->process(capt_trk);
 
     for (size_t ii=0; ii<32; ii++ )
@@ -134,7 +138,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
         proc_odo->process(capt_odo);
 
         // Track
-        capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
+        capt_trk = make_shared<CaptureImage>(t, sens_trk_cam, image);
         proc_trk->process(capt_trk);
 
         CaptureBasePtr prev = proc_trk->getPrevOrigin();
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index b6b5f2b85717df13c864551845bdf6c1725c0b39..be165e8ca382eb6cd5a9e19294f2b6b15efff184 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -506,9 +506,12 @@ TEST(SolverManager, AddFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
 
     // update solver
     solver_manager_ptr->update();
@@ -528,9 +531,12 @@ TEST(SolverManager, RemoveFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
 
     // update solver
     solver_manager_ptr->update();
@@ -556,9 +562,13 @@ TEST(SolverManager, AddRemoveFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
 
     ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
 
@@ -585,9 +595,13 @@ TEST(SolverManager, DoubleRemoveFactor)
 
     // Create (and add) factor 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()));
-    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+    // CaptureBasePtr      C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
+    // FeatureBasePtr      f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
+    // FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+
+    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+    auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+    FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(FactorBase::emplace<FactorPose2D>(f, f));
 
     // update solver
     solver_manager_ptr->update();
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index d77defcbc697d87cf827a465d1ef99e9221293a6..e70f26c025b28587e99a1b393189f75c9577d7c4 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -69,12 +69,15 @@ TEST(TrajectoryBase, ClosestKeyFrame)
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME,     1, nullptr, nullptr);
-    FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME,     2, nullptr, nullptr);
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr);
-    T->addFrame(f1);
-    T->addFrame(f2);
-    T->addFrame(f3);
+    // FrameBasePtr f1 = std::make_shared<FrameBase>(KEY_FRAME,     1, nullptr, nullptr);
+    // FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME,     2, nullptr, nullptr);
+    // FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, nullptr, nullptr);
+    FrameBasePtr f1 = FrameBase::emplace<FrameBase>(T, KEY_FRAME,     1, nullptr, nullptr);
+    FrameBasePtr f2 = FrameBase::emplace<FrameBase>(T, KEY_FRAME,     2, nullptr, nullptr);
+    FrameBasePtr f3 = FrameBase::emplace<FrameBase>(T, NON_KEY_FRAME, 3, nullptr, nullptr);
+    // T->addFrame(f1);
+    // T->addFrame(f2);
+    // T->addFrame(f3);
 
     FrameBasePtr kf; // closest key-frame queried
 
@@ -112,24 +115,31 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     FrameBasePtr f2 = std::make_shared<FrameBase>(KEY_FRAME,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
     FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
 
+    // FrameBasePtr f1 = FrameBase::emplace<FrameBase>(T, KEY_FRAME,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+    // FrameBasePtr f2 = FrameBase::emplace<FrameBase>(T, KEY_FRAME,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+    // FrameBasePtr f3 = FrameBase::emplace<FrameBase>(T, NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+
     std::cout << __LINE__ << std::endl;
 
     // add frames and keyframes
-    T->addFrame(f1); // KF
+    // T->addFrame(f1); // KF
+    f1->link(T);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 1);
     ASSERT_EQ(P->getStateBlockPtrList().            size(), (unsigned int) 2);
     ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f2); // KF
+    // T->addFrame(f2); // KF
+    f2->link(T);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 2);
     ASSERT_EQ(P->getStateBlockPtrList().            size(), (unsigned int) 4);
     ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f3); // F
+    // T->addFrame(f3); // F
+    f3->link(T);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 3);
     ASSERT_EQ(P->getStateBlockPtrList().            size(), (unsigned int) 4);
@@ -194,17 +204,20 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     FrameBasePtr f3 = std::make_shared<FrameBase>(NON_KEY_FRAME, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
 
     // add frames and keyframes in random order --> keyframes must be sorted after that
-    T->addFrame(f2); // KF2
+    // T->addFrame(f2); // KF2
+    f2->link(T);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()   ->id(), f2->id());
     ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
 
-    T->addFrame(f3); // F3
+    // T->addFrame(f3); // F3
+    f3->link(T);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
     ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
 
-    T->addFrame(f1); // KF1
+    // T->addFrame(f1); // KF1
+    f1->link(T);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
     ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
@@ -215,7 +228,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
 
     FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-    T->addFrame(f4);
+    // T->addFrame(f4);
+    f4->link(T);
     // Trajectory status:
     //  kf1   kf2   kf3     f4       frames
     //   1     2     3     1.5       time stamps