diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 08751287b5109a81160c3e70aadc4ae9d6f33253..8732820b4db10cd2a561c83d63defd82108f5ee4 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -110,6 +110,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         SizeEigen computeCalibSize() const;
 
         FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
+        void removeFeature(FeatureBasePtr _ft_ptr);
     private:
         void updateCalibSize();
 };
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 796579ce398961dd8069b425d063e678855731ca..b4ea5b7c7d51b306e7c1aa0408419984ad476e75 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -113,7 +113,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
         /** \brief Returns a pointer to the feature constrained from
          **/
         FeatureBasePtr getFeature() const;
-        void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
 
         /** \brief Returns a pointer to its capture
          **/
@@ -177,6 +176,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
         template<typename classType, typename... T>
         static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
 
+    protected:
+
+        void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
 //    protected:
 //        template<typename D>
 //        void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index c3a709fd563cafed50de1a1bb393cef1ec36a9e5..5e4b6cc9d45367c6a611ff75436bfc764a29bdb8 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -85,7 +85,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         FrameBasePtr getFrame() const;
 
         CaptureBasePtr getCapture() const;
-        void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
 
         FactorBasePtrList& getFactorList();
 
@@ -103,7 +102,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
     protected:
 
         Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+
+        void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
         FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+        void removeFactor(FactorBasePtr _co_ptr);
         virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
         virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
 };
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 98f50bb773ae2edc949510483b73a09c0aac60d8..bc059138890b4817bf5fa7b69683e7154b05d6a9 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -130,7 +130,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         virtual void setProblem(ProblemPtr _problem) final;
 
         TrajectoryBasePtr getTrajectory() const;
-        void setTrajectory(TrajectoryBasePtr _trj_ptr);
 
         FrameBasePtr getPreviousFrame() const;
         FrameBasePtr getNextFrame() const;
@@ -154,6 +153,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
     protected:
 
         CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
+        void removeCapture(CaptureBasePtr _capt_ptr);
+        void setTrajectory(TrajectoryBasePtr _trj_ptr);
     public:
         static FrameBasePtr create_PO_2D (const FrameType & _tp,
                                           const TimeStamp& _ts,
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 5c27689da7891d8e031e20f4b038a173ceefcd9b..fc8b87c08b5a7dc6802ee6c48dadf65820b76eee 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -91,7 +91,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         unsigned int getHits() const;
         const FactorBasePtrList& getConstrainedByList() const;
 
-        void setMap(const MapBasePtr _map_ptr);
         MapBasePtr getMap();
         void link(MapBasePtr);
         template<typename classType, typename... T>
@@ -99,6 +98,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
     protected:
 
+        void setMap(const MapBasePtr _map_ptr);
         virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
         virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
 };
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index ee6d245283c625fd4f16538620c7b14868ce5850..136f42f80aab5c41063a8de140de9c707f2e1032 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -30,6 +30,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
         
     protected:
         virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
+        virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
 
     public:
         LandmarkBasePtrList& getLandmarkList();
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a6f8ac588195b3402ba2aebab998a8d8f6fcf1f7..7c171a11f3761fbb313655b314bdf35c090b6665 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -204,8 +204,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         SensorBasePtr getSensor();
         const SensorBasePtr getSensor() const;
+    protected:
         void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
 
+    public:
         virtual bool isMotion();
 
         void setTimeTolerance(Scalar _time_tolerance);
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 0f1678fd17cdd02aa8935c721ba3ceb48f5071c1..010d3fb78b1674a5407319d2932d4871ad8c4521 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -101,6 +101,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
     protected:
         void setHardware(const HardwareBasePtr _hw_ptr);
         ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
+        void removeProcessor(ProcessorBasePtr _proc_ptr);
 
     public:
         ProcessorBasePtrList& getProcessorList();
@@ -186,6 +187,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         Eigen::MatrixXs getNoiseCov();
         void setExtrinsicDynamic(bool _extrinsic_dynamic);
         void setIntrinsicDynamic(bool _intrinsic_dynamic);
+
         void link(HardwareBasePtr);
         template<typename classType, typename... T>
         static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 339a41b3dad5a1cab9743360a9f7e2bd6f55c380..5d22163797d38016d1a62f14f4bceeae9cd066b2 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -38,7 +38,6 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         std::string getFrameStructure() const;
 
         // Frames
-        FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
         FrameBasePtrList& getFrameList();
         const FrameBasePtrList& getFrameList() const;
         FrameBasePtr getLastFrame() const;
@@ -51,6 +50,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
 
     protected:
         FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
+        void removeFrame(FrameBasePtr _frame_ptr);
 
     public:
         // factors
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 5aa5ea3662707d42f7f34b4e85b0919564e39b3d..f531a781e5b2e3ffeda42966b1e1382d5e5c6d79 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -102,7 +102,9 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
     return _ft_ptr;
 }
 
+void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
 {
+    feature_list_.remove(_ft_ptr);
 }
 
 void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
@@ -114,7 +116,6 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
 FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
 {
     constrained_by_list_.push_back(_fac_ptr);
-    _fac_ptr->setCaptureOther(shared_from_this());
     return _fac_ptr;
 }
 
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 26b325da868670ac0c8849998d61a7fbcb678d02..33ee54e3dbf7591049f1e009835852bcf972b30e 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -70,6 +70,11 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
     return _co_ptr;
 }
 
+void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
+{
+    factor_list_.remove(_co_ptr);
+}
+
 FrameBasePtr FeatureBase::getFrame() const
 {
     return capture_ptr_.lock()->getFrame();
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 7fd6bf365bcbbcd42ba9ff39f8ea0da3cccbaed6..02914eb1a2e7910c5c720742d3e001ac59af092b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -323,6 +323,11 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
     return _capt_ptr;
 }
 
+void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
+{
+    capture_list_.remove(_capt_ptr);
+}
+
 CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
 {
     for (CaptureBasePtr capture_ptr : getCaptureList())
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 204fc7b8ffd3d1b1a8c49030c74c0e803e11b7c4..0da491faa394a88e791e1a9b85fa408279296152 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -33,20 +33,9 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
     return _landmark_ptr;
 }
 
-void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
+void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
 {
-    for (auto lmk : _landmark_list)
-        addLandmark(lmk);
-
-    //TEMPORARY FIX, should be made compliant with the new emplace methodology
-	LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
-   for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
-   {
-       landmark_ptr->setMap(shared_from_this());
-       landmark_ptr->setProblem(getProblem());
-       landmark_ptr->registerNewStateBlocks();
-   }
-   landmark_list_.splice(landmark_list_.end(), _landmark_list);
+    landmark_list_.remove(_landmark_ptr);
 }
 
 void MapBase::load(const std::string& _map_file_dot_yaml)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 87819e750035cc5062fb8ad0c4588a4b717987f9..455c72b996f9c7c1b519eb80345a2b196103a9d0 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -430,16 +430,6 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces
     processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
 }
 
-LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
-{
-    getMap()->addLandmark(_lmk_ptr);
-    return _lmk_ptr;
-}
-
-void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
-{
-    getMap()->addLandmarkList(_lmk_list);
-}
 
 StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
 {
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index c633261ade4a3835e38969eb122d59d5be014928..4986a3521f82f2e42fb1a627acb572e651d227f8 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -339,6 +339,11 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
     return _proc_ptr;
 }
 
+void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
+{
+    processor_list_.remove(_proc_ptr);
+}
+
 StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
 {
     CaptureBasePtr cap;
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index a2395ffae0c1512ca4157fc335bd02ed62855d6a..34921e3f468a0e4960f4b305978eb33ba01109f4 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -34,6 +34,16 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
     return _frame_ptr;
 }
 
+void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
+{
+    // add to list
+    frame_list_.remove(_frame_ptr);
+
+    // update last_estimated and last_key
+    if (_frame_ptr->isKeyOrAux())
+        updateLastFrames();
+}
+
 void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
 {
 	for(auto fr_ptr : getFrameList())