diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 1a7230987849715583334ef1828bb55a08930b5d..f5c7a657f2305ae3d9f540865ef6c26b81f8ec8f 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -47,7 +47,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
                     StateBlockPtr _intr_ptr     = nullptr);
 
         virtual ~CaptureBase();
-        virtual void remove();
+        virtual void remove(bool viral_remove_empty_parent=false);
 
         // Type
         virtual bool isMotion() const { return false; }
@@ -65,7 +65,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
 
     public:
         virtual void setProblem(ProblemPtr _problem) final;
-        FeatureBasePtrList& getFeatureList();
+        const FeatureBasePtrList& getFeatureList() const;
 
         void getFactorList(FactorBasePtrList& _fac_list);
 
@@ -191,7 +191,7 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
     frame_ptr_ = _frm_ptr;
 }
 
-inline FeatureBasePtrList& CaptureBase::getFeatureList()
+inline const FeatureBasePtrList& CaptureBase::getFeatureList() const
 {
     return feature_list_;
 }
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 09a6c11b1cd0cd685fdb6db40ea0c3ad64a20142..210d7d84cf790a8dfae68155d231772f3d5a5b85 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -215,8 +215,9 @@ struct MatrixSizeCheck
 
 #define WOLF_LIST_TYPEDEFS(ClassName) \
         class ClassName; \
-        typedef std::list<ClassName##Ptr>          ClassName##PtrList; \
+        typedef std::list<ClassName##Ptr>             ClassName##PtrList; \
         typedef ClassName##PtrList::iterator          ClassName##Iter; \
+        typedef ClassName##PtrList::const_iterator    ClassName##ConstIter; \
         typedef ClassName##PtrList::reverse_iterator  ClassName##RevIter;
 
 #define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index fbc07e89af8850d9e4b4228817b5bbb6e5abac53..a6890dca220baa11ce1b33d5a85f17290a3e3626 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -78,7 +78,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
 
         virtual ~FactorBase() = default;
 
-        virtual void remove();
+        virtual void remove(bool viral_remove_empty_parent=false);
 
         unsigned int id() const;
 
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 2d3506fc96cea72a4db1ed6102edf60169e54682..2e3719dd6e58b9b7f473c66779ce4326b3747278 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -53,7 +53,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
 
         virtual ~FeatureBase();
-        virtual void remove();
+        virtual void remove(bool viral_remove_empty_parent=false);
 
         virtual void setProblem(ProblemPtr _problem) final;
 
@@ -86,7 +86,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
 
         CaptureBasePtr getCapture() const;
 
-        FactorBasePtrList& getFactorList();
+        const FactorBasePtrList& getFactorList() const;
 
         unsigned int getHits() const;
         const FactorBasePtrList& getConstrainedByList() const;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 23f3d0975565dfd2d9f123d4b78c291925bba810..853b8f64c8883fb307128d714d79b62d7c0a8905 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -71,7 +71,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
 
         virtual ~FrameBase();
-        virtual void remove();
+        virtual void remove(bool viral_remove_empty_parent=false);
 
         // Frame properties -----------------------------------------------
     public:
@@ -136,7 +136,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         FrameBasePtr getPreviousFrame() const;
         FrameBasePtr getNextFrame() const;
 
-        CaptureBasePtrList& getCaptureList();
+        const CaptureBasePtrList& getCaptureList() const;
         CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
         CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
         CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
@@ -265,7 +265,7 @@ inline TrajectoryBasePtr FrameBase::getTrajectory() const
     return trajectory_ptr_.lock();
 }
 
-inline CaptureBasePtrList& FrameBase::getCaptureList()
+inline const CaptureBasePtrList& FrameBase::getCaptureList() const
 {
     return capture_list_;
 }
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 99a4179542d3857ed670b4dd95ac8607c1e588b5..a309e7a03c8a5e27f459b772ce6bff5b0994f714 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -25,7 +25,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
         HardwareBase();
         virtual ~HardwareBase();
 
-        SensorBasePtrList& getSensorList();
+        const SensorBasePtrList& getSensorList() const;
 
     private:
         virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
@@ -37,7 +37,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
 
 namespace wolf {
 
-inline SensorBasePtrList& HardwareBase::getSensorList()
+inline const SensorBasePtrList& HardwareBase::getSensorList() const
 {
     return sensor_list_;
 }
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 57d294acfe4404cb16fc3abdcc15e5225d2053ac..ce1ab0860d1767b396a302f7f8671aa09498e22a 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -48,7 +48,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
          **/
         LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
         virtual ~LandmarkBase();
-        virtual void remove();
+        virtual void remove(bool viral_remove_empty_parent=false);
         virtual YAML::Node saveToYaml() const;
 
         // Properties
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index d4d5f4fb7de8356cf53fe05283c53093842ebb6c..6d09191e54619d5a00fce39e1ecf002aace66d3a 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -33,7 +33,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
         virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
 
     public:
-        LandmarkBasePtrList& getLandmarkList();
+        const LandmarkBasePtrList& getLandmarkList() const;
         
         void load(const std::string& _map_file_yaml);
         void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
@@ -42,7 +42,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
         std::string dateTimeNow();
 };
 
-inline LandmarkBasePtrList& MapBase::getLandmarkList()
+inline const LandmarkBasePtrList& MapBase::getLandmarkList() const
 {
     return landmark_list_;
 }
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 3db21f9fe7245e54fcd6637adc8c5606ce9bd6a9..dcd73665457f2b78e1d9aed0678dabbc63e4da4c 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -109,7 +109,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         void removeProcessor(ProcessorBasePtr _proc_ptr);
 
     public:
-        ProcessorBasePtrList& getProcessorList();
+        const ProcessorBasePtrList& getProcessorList() const;
 
         CaptureBasePtr lastKeyCapture(void);
         CaptureBasePtr lastCapture(const TimeStamp& _ts);
@@ -229,7 +229,7 @@ inline unsigned int SensorBase::id()
     return sensor_id_;
 }
 
-inline ProcessorBasePtrList& SensorBase::getProcessorList()
+inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
 {
     return processor_list_;
 }
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index c41bf09830811009d415a748fbdca7a918a72f72..0645be841b4dd9c68cc2a6f557146101f199b26e 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
-        FrameBasePtrList& getFrameList();
         const FrameBasePtrList& getFrameList() const;
         FrameBasePtr getLastFrame() const;
         FrameBasePtr getLastKeyFrame() const;
@@ -57,15 +56,10 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         void getFactorList(FactorBasePtrList & _fac_list);
 
     protected:
-        FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
-        void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
+        FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr);
+        void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place);
 };
 
-inline FrameBasePtrList& TrajectoryBase::getFrameList()
-{
-    return frame_list_;
-}
-
 inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
 {
     return frame_list_;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index c4dcb6a0f55f7ed8dd51303135e5357faa4fcaa1..899327294dadc122747223403e26b6d0e58b67f4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -64,7 +64,7 @@ CaptureBase::~CaptureBase()
     removeStateBlocks();
 }
 
-void CaptureBase::remove()
+void CaptureBase::remove(bool viral_remove_empty_parent)
 {
     if (!is_removing_)
     {
@@ -78,19 +78,19 @@ void CaptureBase::remove()
         FrameBasePtr F = frame_ptr_.lock();
         if (F)
         {
-            F->getCaptureList().remove(this_C);
-            if (F->getCaptureList().empty() && F->getConstrainedByList().empty())
-                F->remove(); // remove upstream
+            F->removeCapture(this_C);
+            if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
+                F->remove(viral_remove_empty_parent); // remove upstream
         }
 
         // remove downstream
         while (!feature_list_.empty())
         {
-            feature_list_.front()->remove(); // remove downstream
+            feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
         }
         while (!constrained_by_list_.empty())
         {
-            constrained_by_list_.front()->remove(); // remove constrained by
+            constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
         }
     }
 }
@@ -315,6 +315,7 @@ void CaptureBase::move(FrameBasePtr _frm_ptr)
 
 void CaptureBase::link(FrameBasePtr _frm_ptr)
 {
+    assert(!is_removing_ && "linking a removed capture");
     assert(this->getFrame() == nullptr && "linking a capture already linked");
 
     if(_frm_ptr)
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index a653807ebbaf849ab8fea0dd81c07592f197fb14..f64a1464b0c441f43c731b1a4420cddc3d3794fd 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -44,54 +44,54 @@ FactorBase::FactorBase(const std::string&  _tp,
 //    std::cout << "constructed        +c" << id() << std::endl;
 }
 
-void FactorBase::remove()
+void FactorBase::remove(bool viral_remove_empty_parent)
 {
     if (!is_removing_)
     {
         is_removing_ = true;
-        FactorBasePtr this_c = shared_from_this(); // keep this alive while removing it
+        FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
         FeatureBasePtr f = feature_ptr_.lock();
         if (f)
         {
-            f->getFactorList().remove(shared_from_this()); // remove from upstream
-            if (f->getFactorList().empty() && f->getConstrainedByList().empty())
-                f->remove(); // remove upstream
+            f->removeFactor(this_fac); // remove from upstream
+            if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
+                f->remove(viral_remove_empty_parent); // remove upstream
         }
         // add factor to be removed from solver
         if (getProblem() != nullptr)
-            getProblem()->notifyFactor(shared_from_this(),REMOVE);
+            getProblem()->notifyFactor(this_fac,REMOVE);
 
         // remove other: {Frame, Capture, Feature, Landmark}
         FrameBasePtr frm_o = frame_other_ptr_.lock();
         if (frm_o)
         {
-            frm_o->removeConstrainedBy(shared_from_this());
+            frm_o->removeConstrainedBy(this_fac);
             if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
-                frm_o->remove();
+                frm_o->remove(viral_remove_empty_parent);
         }
 
         CaptureBasePtr cap_o = capture_other_ptr_.lock();
         if (cap_o)
         {
-            cap_o->removeConstrainedBy(shared_from_this());
+            cap_o->removeConstrainedBy(this_fac);
             if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
-                cap_o->remove();
+                cap_o->remove(viral_remove_empty_parent);
         }
 
         FeatureBasePtr ftr_o = feature_other_ptr_.lock();
         if (ftr_o)
         {
-            ftr_o->removeConstrainedBy(shared_from_this());
+            ftr_o->removeConstrainedBy(this_fac);
             if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
-                ftr_o->remove();
+                ftr_o->remove(viral_remove_empty_parent);
         }
 
         LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
         if (lmk_o)
         {
-            lmk_o->removeConstrainedBy(shared_from_this());
+            lmk_o->removeConstrainedBy(this_fac);
             if (lmk_o->getConstrainedByList().empty())
-                lmk_o->remove();
+                lmk_o->remove(viral_remove_empty_parent);
         }
 
         //        std::cout << "Removed             c" << id() << std::endl;
@@ -143,6 +143,7 @@ void FactorBase::setStatus(FactorStatus _status)
 
 void FactorBase::link(FeatureBasePtr _ftr_ptr)
 {
+    assert(!is_removing_ && "linking a removed factor");
     assert(this->getFeature() == nullptr && "linking an already linked factor");
 
     // not link if nullptr
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 2929eb17fc4bb44f7b6b6094c05590c62a9e6ceb..f2b4dfca9b65bd7cf0ad64bca7de55d8dc53d830 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -36,7 +36,7 @@ FeatureBase::~FeatureBase()
 //    std::cout << "destructed       -f" << id() << std::endl;
 }
 
-void FeatureBase::remove()
+void FeatureBase::remove(bool viral_remove_empty_parent)
 {
     if (!is_removing_)
     {
@@ -47,19 +47,19 @@ void FeatureBase::remove()
         CaptureBasePtr C = capture_ptr_.lock();
         if (C)
         {
-            C->getFeatureList().remove(this_f); // remove from upstream
-            if (C->getFeatureList().empty())
-                C->remove(); // remove upstream
+            C->removeFeature(this_f); // remove from upstream
+            if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
+                C->remove(viral_remove_empty_parent); // remove upstream
         }
 
         // remove downstream
         while (!factor_list_.empty())
         {
-            factor_list_.front()->remove(); // remove downstream
+            factor_list_.front()->remove(viral_remove_empty_parent); // remove downstream
         }
         while (!constrained_by_list_.empty())
         {
-            constrained_by_list_.front()->remove(); // remove constrained
+            constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
         }
     }
 }
@@ -91,7 +91,7 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
     constrained_by_list_.remove(_fac_ptr);
 }
 
-FactorBasePtrList& FeatureBase::getFactorList()
+const FactorBasePtrList& FeatureBase::getFactorList() const
 {
     return factor_list_;
 }
@@ -157,6 +157,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con
 
 void FeatureBase::link(CaptureBasePtr _cpt_ptr)
 {
+    assert(!is_removing_ && "linking a removed feature");
     assert(this->getCapture() == nullptr && "linking an already linked feature");
 
     if(_cpt_ptr)
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index aee1ecfaec2139251015f1469eddab6d859d3747..8a87eae58494bc5c8b56bdf4b068e5ffb42bfdcf 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -86,7 +86,7 @@ FrameBase::~FrameBase()
 {
 }
 
-void FrameBase::remove()
+void FrameBase::remove(bool viral_remove_empty_parent)
 {
     if (!is_removing_)
     {
@@ -97,27 +97,23 @@ void FrameBase::remove()
         TrajectoryBasePtr T = trajectory_ptr_.lock();
         if (T)
         {
-            T->getFrameList().remove(this_F); // remove from upstream
+            T->removeFrame(this_F); // remove from upstream
         }
 
         // remove downstream
         while (!capture_list_.empty())
         {
-            capture_list_.front()->remove(); // remove downstream
+            capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
         }
         while (!constrained_by_list_.empty())
         {
-            constrained_by_list_.front()->remove(); // remove constrained
+            constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
         }
 
         // Remove Frame State Blocks
         if ( isKeyOrAux() )
             removeStateBlocks();
 
-
-        if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastKeyOrAuxFrame()->id() == this_F->id())
-            getTrajectory()->updateLastFrames();
-
 //        std::cout << "Removed       F" << id() << std::endl;
     }
 }
@@ -408,6 +404,7 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
 
 void FrameBase::link(TrajectoryBasePtr _trj_ptr)
 {
+    assert(!is_removing_ && "linking a removed frame");
     assert(this->getTrajectory() == nullptr && "linking an already linked frame");
 
     if(_trj_ptr)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index cb3f7cb21d3c0172e99ee482018f48365206d15f..3da95fb8d36b218b1f91d10ce409638f256ab1ae 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -29,7 +29,7 @@ LandmarkBase::~LandmarkBase()
 //    std::cout << "destructed   -L" << id() << std::endl;
 }
 
-void LandmarkBase::remove()
+void LandmarkBase::remove(bool viral_remove_empty_parent)
 {
     if (!is_removing_)
     {
@@ -39,12 +39,12 @@ void LandmarkBase::remove()
         // remove from upstream
         auto M = map_ptr_.lock();
         if (M)
-            M->getLandmarkList().remove(shared_from_this());
+            M->removeLandmark(this_L);
 
         // remove constrained by
         while (!constrained_by_list_.empty())
         {
-            constrained_by_list_.front()->remove();
+            constrained_by_list_.front()->remove(viral_remove_empty_parent);
         }
 
         // Remove State Blocks
@@ -164,6 +164,7 @@ YAML::Node LandmarkBase::saveToYaml() const
 
 void LandmarkBase::link(MapBasePtr _map_ptr)
 {
+    assert(!is_removing_ && "linking a removed landmark");
     assert(this->getMap() == nullptr && "linking an already linked landmark");
 
     if(_map_ptr)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 6dcc3c3417a34f68ba7b56502b4b53f75bc996c9..4d9e937028360e651dfbb0377927ad2186244b8e 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -75,12 +75,13 @@ void ProcessorBase::remove()
         // remove from upstream
         SensorBasePtr sen = sensor_ptr_.lock();
         if(sen)
-            sen->getProcessorList().remove(this_p);
+            sen->removeProcessor(this_p);
     }
 }
 
 void ProcessorBase::link(SensorBasePtr _sen_ptr)
 {
+    assert(!is_removing_ && "linking a removed processor");
     assert(this->getSensor() == nullptr && "linking an already linked processor");
 
     if(_sen_ptr)
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 2f5a11c9b25ed40d9613f20d1f68d22d57b72bf1..accc54225043d1a4352596f65f4622d8c54464ca 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -409,6 +409,7 @@ void SensorBase::setProblem(ProblemPtr _problem)
 
 void SensorBase::link(HardwareBasePtr _hw_ptr)
 {
+    assert(!is_removing_ && "linking a removed sensor");
     assert(this->getHardware() == nullptr && "linking an already linked sensor");
     if(_hw_ptr)
     {
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 34921e3f468a0e4960f4b305978eb33ba01109f4..35a1f9997e7ab5bffa8f4dbf4f8ac7c44415b66b 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -55,7 +55,7 @@ void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
     moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
 }
 
-void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
+void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place)
 {
     if (*_place != _frm_ptr)
     {
@@ -65,7 +65,7 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
     }
 }
 
-FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
+FrameBaseConstIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
 {
     for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
         if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())