diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index b1309f336c0f8979d5cc4fa8d57aad37de2a00dc..1089e4ae64f96db351e5f2aee74423b26642f0ed 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -234,7 +234,7 @@ int main()
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
     for (auto& kf : *problem->getTrajectory())
-        if (kf->isKeyOrAux())
+        if (kf->isKey())
         {
             Eigen::MatrixXd cov;
             kf->getCovariance(cov);
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index cad0474fa06cc72c6df41d8f95f20153c15fa06e..e974a19c11b3a271d63e3759d8a4735b13447cae 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -219,7 +219,7 @@ int main()
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
     for (auto& kf : *problem->getTrajectory())
-        if (kf->isKeyOrAux())
+        if (kf->isKey())
         {
             Eigen::MatrixXd cov;
             kf->getCovariance(cov);
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 14a8f14b799ab39abbccf2e11ded6da84bd2bfda..d3b4ae34704f3eb7bb6b2abc30eb15097c38acab 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -13,7 +13,7 @@ class StateBlock;
 typedef enum
 {
     KEY = 2,          ///< key frame. It plays at optimizations (estimated).
-    AUXILIARY = 1,    ///< auxiliary frame. It plays at optimizations (estimated).
+    // AUXILIARY = 1,    ///< auxiliary frame. It plays at optimizations (estimated).
     NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
 } FrameType;
 }
@@ -83,13 +83,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
 
         // get type
         bool isKey() const;
-        bool isAux() const;
-        bool isKeyOrAux() const;
 
         // set type
         void setNonEstimated();
         void setKey(ProblemPtr _prb);
-        void setAux();
 
         // Frame values ------------------------------------------------
     public:
@@ -195,16 +192,6 @@ inline bool FrameBase::isKey() const
     return (type_ == KEY);
 }
 
-inline bool FrameBase::isAux() const
-{
-    return (type_ == AUXILIARY);
-}
-
-inline bool FrameBase::isKeyOrAux() const
-{
-    return (type_ == KEY || type_ == AUXILIARY);
-}
-
 inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
 {
     _ts = time_stamp_;
@@ -243,7 +230,7 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
 inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type,
                                               const StateBlockPtr& _sb)
 {
-    if (isKeyOrAux())
+    if (isKey())
         HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
     else
         HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index ef5bba9a7ad087b08eae31699556af520b71cd32..2c48230e80bcef84517c9fe001854c3164a37ff9 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -252,7 +252,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         FrameBasePtr getLastKeyFrame( ) const;
         FrameBasePtr getLastKeyOrAuxFrame( ) const;
         FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
-        FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
 
         /** \brief Give the permission to a processor to create a new key Frame
          *
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 9c3729a61c80e868a0b8af2b6421ffdfddc0731d..840c09c103c69def1ea0355926bdaa270ef81c52 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -60,8 +60,8 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
 
     protected:
         std::string frame_structure_;  // Defines the structure of the Frames in the Trajectory.
-        FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
-        FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
+        // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
+        // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
         
     public:
         TrajectoryBase(const std::string& _frame_sturcture);
@@ -75,14 +75,11 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         FrameBasePtr getLastFrame() const;
         FrameBasePtr getFirstFrame() const;
         FrameBasePtr getLastKeyFrame() const;
-        FrameBasePtr getLastKeyOrAuxFrame() const;
         FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
-        FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
         FrameListIter begin() const;
         FrameListIter end() const;
         FrameListRevIter rbegin() const;
         FrameListRevIter rend() const;
-        void updateLastFrames();
 
         virtual void printHeader(int depth, //
                                  bool constr_by, //
@@ -141,12 +138,8 @@ inline FrameListRevIter TrajectoryBase::rend() const
 
 inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
 {
-    return last_key_frame_ptr_;
-}
-
-inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
-{
-    return last_key_or_aux_frame_ptr_;
+    // return last_key_frame_ptr_;
+    return *(this->rbegin());
 }
 
 inline std::string TrajectoryBase::getFrameStructure() const
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 579223b5df36039f1de51542f34fb69ff6958bbc..f22a7f2ed2fd8ad124e8185fd52720020db1ac65 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -89,7 +89,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
             std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
             //frame state blocks
             for(auto fr_ptr : *wolf_problem_->getTrajectory())
-                if (fr_ptr->isKeyOrAux())
+                if (fr_ptr->isKey())
                     for (const auto& key : fr_ptr->getStructure())
                     {
                         const auto& sb = fr_ptr->getStateBlock(key);
@@ -116,7 +116,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
         {
             // first create a vector containing all state blocks
             for(auto fr_ptr : *wolf_problem_->getTrajectory())
-                if (fr_ptr->isKeyOrAux())
+                if (fr_ptr->isKey())
                     for (const auto& key1 : wolf_problem_->getFrameStructure())
                         for (const auto& key2 : wolf_problem_->getFrameStructure())
                         {
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index e39265816f31260a4a3c85dee679447f66788cb6..4e6bbf2507603ec9aed2050086878b186b21b5b8 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -145,7 +145,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
         }
 
         // Remove Frame State Blocks
-        if ( isKeyOrAux() )
+        if ( isKey() )
             removeStateBlocks(getProblem());
     }
 }
@@ -158,15 +158,11 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts)
 void FrameBase::setNonEstimated()
 {
     // unregister if previously estimated
-    if (isKeyOrAux())
+    if (isKey())
         for (const auto& sb : getStateBlockVec())
             getProblem()->notifyStateBlock(sb, REMOVE);
 
     type_ = NON_ESTIMATED;
-    if (getTrajectory())
-    {
-        getTrajectory()->updateLastFrames();
-    }
 }
 
 void FrameBase::setKey(ProblemPtr _prb)
@@ -174,24 +170,6 @@ void FrameBase::setKey(ProblemPtr _prb)
     assert(_prb != nullptr && "setting Key fram with a null problem pointer");
     type_ = KEY;
     this->link(_prb->getTrajectory());
-
-    if (getTrajectory())
-    {
-        getTrajectory()->updateLastFrames();
-    }
-}
-
-void FrameBase::setAux()
-{
-    if (!isKeyOrAux())
-        registerNewStateBlocks(getProblem());
-
-    type_ = AUXILIARY;
-
-    if (getTrajectory())
-    {
-        getTrajectory()->updateLastFrames();
-    }
 }
 
 bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
@@ -359,7 +337,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
 
 void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
-    _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C  " : "");
+    _stream << _tabs << (isKey() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C  " : "");
     if (_constr_by)
     {
         _stream << "  <-- ";
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d0cbfaef5b142e5b122413981fd02f4caabd9b0d..b3db058da027d07f9697ebc449f99d3b39434a19 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -374,11 +374,12 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
         getState(*min_it, _state);
         _ts = *min_it;
     }
-    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
+    else if (trajectory_ptr_->rbegin() != trajectory_ptr_->rend())
     {
         // kind of redundant with getState(_ts, _state)
-        trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
-        trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
+        auto last = trajectory_ptr_->rbegin();
+        (*last)->getState(_state);
+        (*last)->getTimeStamp(_ts);
     }
     else
         _state = zeroState();
@@ -389,7 +390,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
 {
     // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
-    FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
     if (processor_is_motion_list_.empty()){
         if (closest_frame != nullptr)
             _state = closest_frame->getState();
@@ -796,12 +797,6 @@ bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
     return getFrameCovariance(frm, cov);
 }
 
-bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const
-{
-    FrameBasePtr frm = getLastKeyOrAuxFrame();
-    return getFrameCovariance(frm, cov);
-}
-
 bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const
 {
     bool success(true);
@@ -887,21 +882,11 @@ FrameBasePtr Problem::getLastKeyFrame() const
     return trajectory_ptr_->getLastKeyFrame();
 }
 
-FrameBasePtr Problem::getLastKeyOrAuxFrame() const
-{
-    return trajectory_ptr_->getLastKeyOrAuxFrame();
-}
-
 FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
 {
     return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
 }
 
-FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
-{
-    return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
-}
-
 void Problem::setPriorOptions(const std::string& _mode,
                               const double _time_tolerance,
                               const Eigen::VectorXd& _state,
@@ -1159,7 +1144,7 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
     for (auto F : *(T))
     {
       if (_verbose) {
-        _stream << (F->isKeyOrAux() ? (F->isKey() ? "  KFrm" : " EFrm") : "  Frm")
+        _stream << (F->isKey() ? (F->isKey() ? "  KFrm" : " EFrm") : "  Frm")
              << F->id() << " @ " << F.get() << std::endl;
         _stream << "    -> Prb @ " << F->getProblem().get() << std::endl;
         _stream << "    -> Trj @ " << F->getTrajectory().get() << std::endl;
@@ -1728,7 +1713,7 @@ void Problem::perturb(double amplitude)
     // Frames and Captures
     for (auto& F : *(getTrajectory()))
     {
-        if (F->isKeyOrAux())
+        if (F->isKey())
         {
             F->perturb(amplitude);
             for (auto& C : F->getCaptureList())
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 233e3c0f1f3130511d7ebcf0debd70107ad45bc6..c1945df34593cea047b1b426dfce8b46402216e7 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -726,10 +726,10 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
 {
     _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
     if (getOrigin())
-        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? "  KFrm" : "  AFrm" ) : "  Frm")
+        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? "  KFrm" : "  Frm" )
                 << getOrigin()->getFrame()->id() << std::endl;
     if (getLast())
-        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
+        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? "  KFrm" : " Frm")
                 << getLast()->getFrame()->id() << std::endl;
     if (getIncoming())
         _stream << _tabs << "  " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 947ca38052ee625d2fbb8452c68b2b2dc565619c..24c81bbdc7527e797a58d276baba54f3e8af9a33 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -343,10 +343,10 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
 {
     _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
     if (getOrigin())
-        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
+        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? "  KFrm" : " Frm")
                 << getOrigin()->getFrame()->id() << std::endl;
     if (getLast())
-        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? "  KFrm" : " AFrm") : "  Frm")
+        _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? "  KFrm" : " Frm")
                 << getLast()->getFrame()->id() << std::endl;
     if (getIncoming())
         _stream << _tabs << "  " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 1cf3a913e95271cd82a90cbb3d139b301ad1deba..77340c7e7f8d9946d200a1a20fbb98915449d8b7 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -5,9 +5,9 @@ namespace wolf {
 
 TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
     NodeBase("TRAJECTORY", "TrajectoryBase"),
-    frame_structure_(_frame_structure),
-    last_key_frame_ptr_(nullptr),
-    last_key_or_aux_frame_ptr_(nullptr)
+    frame_structure_(_frame_structure)
+    // last_key_frame_ptr_(nullptr),
+    // last_key_or_aux_frame_ptr_(nullptr)
 {
 //    WOLF_DEBUG("constructed T");
 }
@@ -24,11 +24,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
     auto success = frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
     std::cout << "Sucess? " << success.second << std::endl;
     printDebug();
-    if (_frame_ptr->isKeyOrAux())
-    {
-        // update last_estimated and last_key
-        updateLastFrames();
-    }
 
     return _frame_ptr;
 }
@@ -38,10 +33,6 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
     // add to list
     // frame_list_.erase(_frame_ptr);
     frame_list_.erase(_frame_ptr->getTimeStamp());
-
-    // update last_estimated and last_key
-    if (_frame_ptr->isKeyOrAux())
-        updateLastFrames();
 }
 
 void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
@@ -51,27 +42,6 @@ void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
 }
 
 
-void TrajectoryBase::updateLastFrames()
-{
-    bool last_estimated_set = false;
-
-    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
-    for (auto frm_rit = rbegin(); frm_rit != rend(); ++frm_rit)
-        if ((*frm_rit)->isKeyOrAux())
-        {
-            if (!last_estimated_set)
-            {
-                last_key_or_aux_frame_ptr_ = (*frm_rit);
-                last_estimated_set = true;
-            }
-            if ((*frm_rit)->isKey())
-            {
-                last_key_frame_ptr_ = (*frm_rit);
-                break;
-            }
-        }
-}
-
 FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
 {
     // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
@@ -79,38 +49,16 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) co
     double min_dt = 1e9;
 
     for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++)
-        if ((*frm_rit)->isKey())
-        {
-            double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
-            if (dt < min_dt)
-            {
-                min_dt = dt;
-                closest_kf = *frm_rit;
-            }
-            else
-                break;
-        }
-    return closest_kf;
-}
-
-FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
-{
-    // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
-    FrameBasePtr closest_kf = nullptr;
-    double min_dt = 1e9;
-
-    for (auto frm_rit = rbegin(); frm_rit != rend(); frm_rit++)
-        if ((*frm_rit)->isKeyOrAux())
+    {
+        double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
+        if (dt < min_dt)
         {
-            double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
-            if (dt < min_dt)
-            {
-                min_dt = dt;
-                closest_kf = *frm_rit;
-            }
-            else
-                break;
+            min_dt = dt;
+            closest_kf = *frm_rit;
         }
+        else
+            break;
+    }
     return closest_kf;
 }
 
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index b8dceb6baa616fb4bced7733ed17da639e9c5082..da0ea77ae239314b8e6b16a94c27c05c9284e98d 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -32,7 +32,7 @@ TEST(FrameBase, GettersAndSetters)
     ASSERT_EQ(t, 1);
     ASSERT_FALSE(F->isFixed());
     ASSERT_EQ(F->isKey(), false);
-    ASSERT_EQ(F->isKeyOrAux(), false);
+    ASSERT_EQ(F->isKey(), false);
 }
 
 TEST(FrameBase, StateBlocks)
@@ -114,7 +114,7 @@ TEST(FrameBase, LinksToTree)
     // set key
     F1->setKey(P);
     ASSERT_TRUE(F1->isKey());
-    ASSERT_TRUE(F1->isKeyOrAux());
+    ASSERT_TRUE(F1->isKey());
 }
 
 #include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index e9951c270aab76a84ed56ff08ebfbf977da8afc7..04cede086256d544656095d2ce89ee5c0d9aef22 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -102,19 +102,19 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
 
     FrameBasePtr KF; // closest key-frame queried
 
-    KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8);          // before all keyframes    --> return f0
+    KF = T->closestKeyFrameToTimeStamp(-0.8);          // before all keyframes    --> return f0
     ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    KF = T->closestKeyOrAuxFrameToTimeStamp(1.1);           // between keyframes       --> return F1
+    KF = T->closestKeyFrameToTimeStamp(1.1);           // between keyframes       --> return F1
     ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    KF = T->closestKeyOrAuxFrameToTimeStamp(1.9);           // between keyframes       --> return F2
+    KF = T->closestKeyFrameToTimeStamp(1.9);           // between keyframes       --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    KF = T->closestKeyOrAuxFrameToTimeStamp(2.6);           // between keyframe and frame, but closer to frame --> return F2
+    KF = T->closestKeyFrameToTimeStamp(2.6);           // between keyframe and frame, but closer to frame --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    KF = T->closestKeyOrAuxFrameToTimeStamp(3.2);           // after the last frame    --> return F2
+    KF = T->closestKeyFrameToTimeStamp(3.2);           // after the last frame    --> return F2
     ASSERT_EQ(KF->id(), F2->id());                           // same id!
 }
 
@@ -208,25 +208,25 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     FrameBasePtr F2 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 2);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 3, Eigen::Vector3d::Zero()); // non-key-frame
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     FrameBasePtr F1 = P->emplaceKeyFrame(Eigen::Vector3d::Zero(), 1);
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     F3->setKey(P); // set KF3
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
     auto F4 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 1.5, Eigen::Vector3d::Zero());
@@ -236,7 +236,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F4->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
     F4->setKey(P);
@@ -246,7 +246,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F3->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
     F2->setTimeStamp(4);
@@ -256,7 +256,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     F4->setTimeStamp(0.5);
@@ -275,7 +275,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+-----+-----+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     F5->setTimeStamp(5);
@@ -285,7 +285,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+-----+-----+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F5->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     auto F6 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 6, Eigen::Vector3d::Zero());
@@ -295,7 +295,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+-----+-----+-----+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F6->id());
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
     auto F7 = FrameBase::createNonKeyFrame<FrameBase>(P->getFrameStructure(), P->getDim(), 5.5, Eigen::Vector3d::Zero());
@@ -305,7 +305,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     // --+-----+-----+-----+-----+-----+-----+--->    time
     if (debug) P->print(2,0,1,0);
     ASSERT_EQ(T->getLastFrame()         ->id(), F7->id()); //Only auxiliary and key-frames are sorted
-    ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F5->id());
     ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
 }