diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 03417063241c103308552200b9e56552261e9970..25d7fd4067c2401b46db2c247961b54a8213f71f 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -41,7 +41,7 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
     if (!kf)
     {
         // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
-        kf = getProblem()->closestImportantFrameToTimeStamp(_capture->getTimeStamp());
+        kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp());
         assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
     }
 
diff --git a/include/base/frame/frame_base.h b/include/base/frame/frame_base.h
index e440355e866c9f99f4d49c989aa4755fc873ac87..fbe3914ab8b76c290d0bcdb13680e6cbea6493cd 100644
--- a/include/base/frame/frame_base.h
+++ b/include/base/frame/frame_base.h
@@ -22,9 +22,9 @@ namespace wolf {
  */
 typedef enum
 {
-    IMPORTANT = 2,     ///< estimated important frame. It plays at optimizations.
-    ESTIMATED = 1,     ///< estimated frame. It plays at optimizations.
-    NON_ESTIMATED = 0  ///< Regular frame. It does not play at optimization.
+    KEY = 2,          ///< key 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;
 
 //class FrameBase
@@ -72,13 +72,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
     public:
         unsigned int id();
 
-        // Important / Non Important
-        bool isImportant() const;
-        void setImportant();
-
-        // Estimated / Non estimated
+        // get type
+        bool isKey() const;
         bool isEstimated() const;
-        void setEstimated();
+
+        // set type
+        void setKey();
+        void setAuxiliary();
 
         // Frame values ------------------------------------------------
     public:
@@ -172,14 +172,14 @@ inline unsigned int FrameBase::id()
     return frame_id_;
 }
 
-inline bool FrameBase::isImportant() const
+inline bool FrameBase::isKey() const
 {
-    return (type_ == IMPORTANT);
+    return (type_ == KEY);
 }
 
 inline bool FrameBase::isEstimated() const
 {
-    return (type_ == IMPORTANT || type_ == ESTIMATED);
+    return (type_ == KEY || type_ == AUXILIARY);
 }
 
 inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index b7070994829419d35c20b3e602fd37b1f6390d45..82df6bc2665f9f8e14d9b6846a7f7c4be60c784f 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -138,7 +138,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Emplace frame from string frame_structure
          * \param _frame_structure String indicating the frame structure.
-         * \param _frame_key_type Either IMPORTANT, ESTIMATED or NON_ESTIMATED
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          * \param _time_stamp Time stamp of the frame
          *
@@ -154,7 +154,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Emplace frame from string frame_structure without state
          * \param _frame_structure String indicating the frame structure.
-         * \param _frame_key_type Either IMPORTANT, ESTIMATED or NON_ESTIMATED
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -167,7 +167,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                                   const TimeStamp& _time_stamp);
 
         /** \brief Emplace frame from string frame_structure without structure
-         * \param _frame_key_type Either IMPORTANT, ESTIMATED or NON_ESTIMATED
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          * \param _time_stamp Time stamp of the frame
          *
@@ -181,7 +181,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                                   const TimeStamp& _time_stamp);
 
         /** \brief Emplace frame from string frame_structure without structure nor state
-         * \param _frame_key_type Either IMPORTANT, ESTIMATED or NON_ESTIMATED
+         * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
          * \param _time_stamp Time stamp of the frame
          *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
@@ -194,42 +194,42 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         // Frame getters
         FrameBasePtr getLastFrame( ) const;
-        FrameBasePtr getLastImportantFrame( ) const;
+        FrameBasePtr getLastKeyFrame( ) const;
         FrameBasePtr getLastEstimatedFrame( ) const;
-        FrameBasePtr closestImportantFrameToTimeStamp(const TimeStamp& _ts) const;
+        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
         FrameBasePtr closestEstimatedFrameToTimeStamp(const TimeStamp& _ts) const;
 
-        /** \brief Give the permission to a processor to create a new important Frame
+        /** \brief Give the permission to a processor to create a new key Frame
          *
-         * This should implement a black list of processors that have forbidden important frame creation.
+         * This should implement a black list of processors that have forbidden key frame creation.
          *   - This decision is made at problem level, not at processor configuration level.
-         *   - Therefore, if you want to permanently configure a processor for not creating important frames, see Processor::voting_active_ and its accessors.
+         *   - Therefore, if you want to permanently configure a processor for not creating key frames, see Processor::voting_active_ and its accessors.
          */
-        bool permitImportantFrame(ProcessorBasePtr _processor_ptr);
+        bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
 
-        /** \brief New important frame callback
+        /** \brief New key frame callback
          *
-         * New important frame callback: It should be called by any processor that creates a new important frame. It calls the importantFrameCallback of the rest of processors.
+         * New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
          */
-        void importantFrameCallback(FrameBasePtr _frame_ptr, //
+        void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
                               ProcessorBasePtr _processor_ptr, //
                               const Scalar& _time_tolerance);
 
-        /** \brief Give the permission to a processor to create a new estimated Frame
+        /** \brief Give the permission to a processor to create a new auxiliary Frame
          *
-         * This should implement a black list of processors that have forbidden estimated frame creation.
+         * This should implement a black list of processors that have forbidden auxiliary frame creation.
          *   - This decision is made at problem level, not at processor configuration level.
          *   - Therefore, if you want to permanently configure a processor for not creating estimated frames, see Processor::voting_active_ and its accessors.
          */
-        bool permitEstimatedFrame(ProcessorBasePtr _processor_ptr);
+        bool permitAuxFrame(ProcessorBasePtr _processor_ptr);
 
-        /** \brief New estimated frame callback
+        /** \brief New auxiliary frame callback
          *
-         * New estimated frame callback: It should be called by any processor that creates a new estimated frame. It calls the estimatedFrameCallback of the processor motion.
+         * New auxiliary frame callback: It should be called by any processor that creates a new auxiliary frame. It calls the auxiliaryFrameCallback of the processor motion.
          */
-        void estimatedFrameCallback(FrameBasePtr _frame_ptr, //
-                                    ProcessorBasePtr _processor_ptr, //
-                                    const Scalar& _time_tolerance);
+        void auxFrameCallback(FrameBasePtr _frame_ptr, //
+                              ProcessorBasePtr _processor_ptr, //
+                              const Scalar& _time_tolerance);
 
         // State getters
         Eigen::VectorXs getCurrentState         ( );
@@ -257,7 +257,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov);
         bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0);
         bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance);
-        bool getLastImportantFrameCovariance(Eigen::MatrixXs& _covariance);
+        bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance);
+        bool getLastEstimatedFrameCovariance(Eigen::MatrixXs& _covariance);
         bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance);
 
         // Solver management ----------------------------------------
diff --git a/include/base/trajectory/trajectory_base.h b/include/base/trajectory/trajectory_base.h
index fe7d201e6cd565ca69f3cf5cb3c6e15a60e691c4..4ce1a6e744305bd03181ee52991c0b1b027633d0 100644
--- a/include/base/trajectory/trajectory_base.h
+++ b/include/base/trajectory/trajectory_base.h
@@ -25,7 +25,7 @@ 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_important_frame_ptr_; // keeps pointer to the last important frame
+        FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
         FrameBasePtr last_estimated_frame_ptr_; // keeps pointer to the last estimated frame
         
     public:
@@ -40,9 +40,9 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         FrameBasePtrList& getFrameList();
         const FrameBasePtrList& getFrameList() const;
         FrameBasePtr getLastFrame() const;
-        FrameBasePtr getLastImportantFrame() const;
+        FrameBasePtr getLastKeyFrame() const;
         FrameBasePtr getLastEstimatedFrame() const;
-        FrameBasePtr closestImportantFrameToTimeStamp(const TimeStamp& _ts) const;
+        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
         FrameBasePtr closestEstimatedFrameToTimeStamp(const TimeStamp& _ts) const;
         void sortFrame(FrameBasePtr _frm_ptr);
         void updateLastFrames();
@@ -70,9 +70,9 @@ inline FrameBasePtr TrajectoryBase::getLastFrame() const
     return frame_list_.back();
 }
 
-inline FrameBasePtr TrajectoryBase::getLastImportantFrame() const
+inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
 {
-    return last_important_frame_ptr_;
+    return last_key_frame_ptr_;
 }
 
 inline FrameBasePtr TrajectoryBase::getLastEstimatedFrame() const
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index a237f5ee029eb3a888329ddf310891e81a1df61c..590f8de1d0ff37e6e9c27d11f79f0f54dc5d4372 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -144,7 +144,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
         case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
         {
             //robot-robot
-            auto last_key_frame = wolf_problem_->getLastImportantFrame();
+            auto last_key_frame = wolf_problem_->getLastKeyFrame();
 
             state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
             state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 077f5324b027296a0244885212ce10bf221a9459..456367a2c67b1c98ca91e9631c3f3da47751882a 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -247,7 +247,7 @@ int main(int argc, char** argv)
     odom_trajectory.head(3) = ground_truth_pose;
 
     // Origin Key Frame
-    FrameBasePtr origin_frame = problem.createFrame(ESTIMATED, ground_truth_pose, ts);
+    FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
 
     // Prior covariance
     CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 200945f345247f84ab2fd27c7cec795ae586c926..62ad277f98576b1bf17e17c7515d8dfb428c93be 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -254,7 +254,7 @@ int main(int argc, char** argv)
     odom_trajectory.head(3) = ground_truth_pose;
 
     // Origin Key Frame
-    FrameBasePtr origin_frame = problem.createFrame(ESTIMATED, ground_truth_pose, ts);
+    FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
 
     // Prior covariance
     CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index e3ace5f555f7d64a6f79db6047edc5df9ef10e93..4040194b382e9db0698b2a5d8acf0f852af1286b 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -80,7 +80,7 @@ int main(int argc, char** argv)
     //create FrameIMU
     ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
     state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(ESTIMATED, ts, state_vec);
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
     wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
 
         //create a feature
@@ -141,7 +141,7 @@ int main(int argc, char** argv)
     //create FrameIMU
     ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
     state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(ESTIMATED, ts, state_vec);
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
     wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
 
         //create a feature
@@ -199,7 +199,7 @@ int main(int argc, char** argv)
         //create FrameIMU
     ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
     state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
-    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(ESTIMATED, ts, state_vec);
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec);
     wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
 
         //create a feature
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index ed3d8c13d78a8dbc7347ecffad7341e541292637..4b923de89990eac72ffc20df61b13e28dbb76b66 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -215,7 +215,7 @@ int main(int argc, char** argv)
     wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
     for(FrameBasePtr frm_ptr : frame_list)
     {
-        if(frm_ptr->isEstimated())
+        if(frm_ptr->isKey())
         {   
             //prepare needed variables
             FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index 81be64b485fff8ef6bbea5bbd754af9ba7e117ef..e542e18e0ffac35b9c398ba01ab23bd7fc2e13a1 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -278,7 +278,7 @@ int main(int argc, char** argv)
     wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
     for(FrameBasePtr frm_ptr : frame_list)
     {
-        if(frm_ptr->isEstimated())
+        if(frm_ptr->isKey())
         {   
             //prepare needed variables
             FrameIMUPtr frmIMU_ptr = std::static_pointer_cast<FrameIMU>(frm_ptr);
@@ -324,7 +324,7 @@ int main(int argc, char** argv)
 
     for(FrameBasePtr frm_ptr : frame_list)
     {
-        if(frm_ptr->isEstimated())
+        if(frm_ptr->isKey())
         {
             FactorBasePtrList fac_list =  frm_ptr->getConstrainedByList();
             for(FactorBasePtr fac_ptr : fac_list)
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index d2246b962426aafe5c9ae913a45e619007d275f9..8baee2c1e75af6ab5a8841e85b082d8f7cdd20c9 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -98,7 +98,7 @@ int main(int argc, char** argv)
     //=====================================================
     // Origin Key Frame is fixed
     TimeStamp t = 0;
-    FrameBasePtr origin_frame = problem->emplaceFrame(ESTIMATED, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
+    FrameBasePtr origin_frame = problem->emplaceFrame(KEY, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
     problem->getProcessorMotion()->setOrigin(origin_frame);
     origin_frame->fix();
 
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 547518fc183244b3637014e6f1c9a90f14099bfe..f07a2c1ce3da6eb7168571d3ec1377ffa8b0c388 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -97,11 +97,11 @@ int main(int argc, char** argv)
     /* 1 */
     ProblemPtr problem = Problem::create("PO 3D");
     // One anchor frame to define the lmk, and a copy to make a factor
-    FrameBasePtr kf_1 = problem->emplaceFrame(ESTIMATED,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
-    FrameBasePtr kf_2 = problem->emplaceFrame(ESTIMATED,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_1 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_2 = problem->emplaceFrame(KEY,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
     // and two other frames to observe the lmk
-    FrameBasePtr kf_3 = problem->emplaceFrame(ESTIMATED,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
-    FrameBasePtr kf_4 = problem->emplaceFrame(ESTIMATED,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_3 = problem->emplaceFrame(KEY,(Vector7s()<<0,-1,0,0,0,0,1).finished(), TimeStamp(0));
+    FrameBasePtr kf_4 = problem->emplaceFrame(KEY,(Vector7s()<<0,+1,0,0,0,0,1).finished(), TimeStamp(0));
 
     kf_1->fix();
     kf_2->fix();
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index 5cac693a0213f0d310115d4f8f86f2dab8c2a22a..036ed1250c8dea7c1aadf10e0c437eda243e5ecf 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -24,7 +24,7 @@ void printFrames(ProblemPtr _problem_ptr)
 {
     std::cout << "TRAJECTORY:" << std::endl;
     for (auto frm : _problem_ptr->getTrajectory()->getFrameList())
-        std::cout << "\t " << (frm->isEstimated() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl;
+        std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl;
 }
 
 int main()
@@ -65,12 +65,12 @@ int main()
 
     printFrames(problem_ptr);
 
-    FrameBasePtr frm7 = problem_ptr->emplaceFrame(ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
+    FrameBasePtr frm7 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.7));
     std::cout << std::endl << "created Key Frame " << frm7->id() << " TS: " << 0.7 << std::endl;
 
     printFrames(problem_ptr);
 
-    FrameBasePtr frm8 = problem_ptr->emplaceFrame(ESTIMATED, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
+    FrameBasePtr frm8 = problem_ptr->emplaceFrame(KEY, Eigen::VectorXs::Zero(3), TimeStamp(0.35));
     std::cout << std::endl << "created Key Frame " << frm8->id() << " TS: " << 0.35 << std::endl;
 
     printFrames(problem_ptr);
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index 97af22fa1df25cf2d05c772c6b8a5ca9d19a8b3f..03d214a3d09c8a624129243f181aed1dd722b5a9 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -216,7 +216,7 @@ int main(int argc, char** argv)
 
 	// ------------------------ START EXPERIMENT ------------------------
 	// First frame FIXED
-	last_frame_ptr = bl_problem_ptr->emplaceFrame(ESTIMATED, Eigen::Vector3s::Zero(),TimeStamp(0));
+	last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, Eigen::Vector3s::Zero(),TimeStamp(0));
 	last_frame_ptr->fix();
 	bl_problem_ptr->print(4, true, false, true);
 
@@ -238,7 +238,7 @@ int main(int argc, char** argv)
 				Eigen::Vector3s from_pose = frame_from_ptr->getState();
 				R.topLeftCorner(2,2) = Eigen::Rotation2Ds(from_pose(2)).matrix();
 				Eigen::Vector3s new_frame_pose = from_pose + R*meas;
-				last_frame_ptr = bl_problem_ptr->emplaceFrame(ESTIMATED, new_frame_pose, TimeStamp(double(edge_to)));
+				last_frame_ptr = bl_problem_ptr->emplaceFrame(KEY, new_frame_pose, TimeStamp(double(edge_to)));
 
 				frame_to_ptr = last_frame_ptr;
 
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index 5d065b8ce83113c085bdd12dd8b927f8fc520584..40171695f14ac3ef2aeee7091ca53e8de50a2a90 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -130,8 +130,8 @@ int main(int argc, char** argv)
                     bNum.clear();
 
                     // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
                     wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
                     wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
                     // store
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index 9338b47287d238c6604077fed662d2ba2f290212..b4d9ac23316dc5450b269d99ce45cbb0f7c0e2f0 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -148,8 +148,8 @@ int main(int argc, char** argv)
                     bNum.clear();
 
                     // add frame to problem
-                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
-                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
+                    FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
                     wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
                     wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
                     // store
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index bbe37ef96fdc4d2e33347b34d02b5248dc2cb032..ccdb80f05c15225e7500dddfe6b65f59ebf06a24 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -39,8 +39,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
                 
 FrameBase::~FrameBase()
 {
-    if ( isEstimated() )
-        removeStateBlocks();
 }
 
 void FrameBase::remove()
@@ -71,7 +69,8 @@ void FrameBase::remove()
         if ( isEstimated() )
             removeStateBlocks();
 
-        if (getTrajectory()->getLastImportantFrame()->id() == this_F->id() || getTrajectory()->getLastEstimatedFrame()->id() == this_F->id())
+
+        if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastEstimatedFrame()->id() == this_F->id())
             getTrajectory()->updateLastFrames();
 
 //        std::cout << "Removed       F" << id() << std::endl;
@@ -111,28 +110,27 @@ void FrameBase::removeStateBlocks()
     }
 }
 
-void FrameBase::setImportant()
+void FrameBase::setKey()
 {
-    if (type_ == NON_ESTIMATED)
-    {
+    // register if previously not estimated
+    if (!isEstimated())
         registerNewStateBlocks();
-        getTrajectory()->sortFrame(shared_from_this());
 
-//        WOLF_DEBUG("Set Important", this->id());
-    }
-    type_ = IMPORTANT;
+    // WOLF_DEBUG("Set Key", this->id());
+    type_ = KEY;
+    getTrajectory()->sortFrame(shared_from_this());
+    getTrajectory()->updateLastFrames();
 }
 
-void FrameBase::setEstimated()
+void FrameBase::setAuxiliary()
 {
-    if (type_ == NON_ESTIMATED)
-    {
+    if (!isEstimated())
         registerNewStateBlocks();
-        getTrajectory()->sortFrame(shared_from_this());
 
-//        WOLF_DEBUG("Set Estimated", this->id());
-    }
-    type_ = ESTIMATED;
+    // WOLF_DEBUG("Set Auxiliary", this->id());
+    type_ = AUXILIARY;
+    getTrajectory()->sortFrame(shared_from_this());
+    getTrajectory()->updateLastFrames();
 }
 
 void FrameBase::fix()
@@ -178,7 +176,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
     for (StateBlockPtr sb : state_block_vec_)
         if (sb && (index < _st_size))
         {
-            sb->setState(_state.segment(index, sb->getSize()), isEstimated());
+            sb->setState(_state.segment(index, sb->getSize()), isEstimated()); // do not notify if state block is not estimated by the solver
             index += sb->getSize();
         }
 }
diff --git a/src/problem.cpp b/src/problem.cpp
deleted file mode 100644
index f8eb1676a6384e57fc8772c21aa3ef31c577604d..0000000000000000000000000000000000000000
--- a/src/problem.cpp
+++ /dev/null
@@ -1,1385 +0,0 @@
-// wolf includes
-#include "base/problem.h"
-#include "base/hardware_base.h"
-#include "base/trajectory_base.h"
-#include "base/map_base.h"
-#include "base/sensor/sensor_base.h"
-#include "base/processor/processor_motion.h"
-#include "base/processor/processor_tracker.h"
-#include "base/capture/capture_pose.h"
-#include "base/factor/factor_base.h"
-#include "base/sensor/sensor_factory.h"
-#include "base/processor/processor_factory.h"
-#include "base/state_block.h"
-
-
-// IRI libs includes
-
-// C++ includes
-#include <algorithm>
-
-namespace wolf
-{
-
-// unnamed namespace used for helper functions local to this file.
-namespace
-{
-std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;}
-}
-
-Problem::Problem(const std::string& _frame_structure) :
-        hardware_ptr_(std::make_shared<HardwareBase>()),
-        trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
-        map_ptr_(std::make_shared<MapBase>()),
-        processor_motion_ptr_(),
-        prior_is_set_(false)
-{
-    if (_frame_structure == "PO 2D")
-    {
-        state_size_ = 3;
-        state_cov_size_ = 3;
-        dim_ = 2;
-    }
-
-    else if (_frame_structure == "PO 3D")
-    {
-        state_size_ = 7;
-        state_cov_size_ = 6;
-        dim_ = 3;
-    }
-    else if (_frame_structure == "POV 3D")
-    {
-        state_size_ = 10;
-        state_cov_size_ = 9;
-        dim_ = 3;
-    }
-    else std::runtime_error(
-            "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
-
-}
-
-void Problem::setup()
-{
-    hardware_ptr_  -> setProblem(shared_from_this());
-    trajectory_ptr_-> setProblem(shared_from_this());
-    map_ptr_       -> setProblem(shared_from_this());
-}
-
-ProblemPtr Problem::create(const std::string& _frame_structure)
-{
-    ProblemPtr p(new Problem(_frame_structure)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
-    p->setup();
-    return p->shared_from_this();
-}
-
-Problem::~Problem()
-{
-    //    WOLF_DEBUG("destructed -P");
-}
-
-void Problem::addSensor(SensorBasePtr _sen_ptr)
-{
-    getHardware()->addSensor(_sen_ptr);
-}
-
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
-                                     const std::string& _unique_sensor_name, //
-                                     const Eigen::VectorXs& _extrinsics, //
-                                     IntrinsicsBasePtr _intrinsics)
-{
-    SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
-    addSensor(sen_ptr);
-    return sen_ptr;
-}
-
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
-                                     const std::string& _unique_sensor_name, //
-                                     const Eigen::VectorXs& _extrinsics, //
-                                     const std::string& _intrinsics_filename)
-{
-
-    if (_intrinsics_filename != "")
-    {
-        assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
-        IntrinsicsBasePtr intr_ptr = IntrinsicsFactory::get().create(_sen_type, _intrinsics_filename);
-        return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
-    }
-    else
-        return installSensor(_sen_type, _unique_sensor_name, _extrinsics, IntrinsicsBasePtr());
-
-}
-
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
-                                           const std::string& _unique_processor_name, //
-                                           SensorBasePtr _corresponding_sensor_ptr, //
-                                           ProcessorParamsBasePtr _prc_params)
-{
-    if (_corresponding_sensor_ptr == nullptr)
-    {
-      WOLF_ERROR("Cannot install processor '", _unique_processor_name,
-                 "' since the associated sensor does not exist !");
-      return ProcessorBasePtr();
-    }
-
-    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);
-
-    // setting the origin in all processor motion if origin already setted
-    if (prc_ptr->isMotion() && prior_is_set_)
-        (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
-    // setting the main processor motion
-    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
-        processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
-
-    return prc_ptr;
-}
-
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
-                                           const std::string& _unique_processor_name, //
-                                           const std::string& _corresponding_sensor_name, //
-                                           const std::string& _params_filename)
-{
-    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
-    if (sen_ptr == nullptr)
-        throw std::runtime_error("Sensor not found. Cannot bind Processor.");
-    if (_params_filename == "")
-        return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
-    else
-    {
-        assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
-        ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
-        return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
-    }
-}
-
-SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
-{
-    auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
-                               getHardware()->getSensorList().end(),
-                               [&](SensorBasePtr sb)
-                               {
-                                   return sb->getName() == _sensor_name;
-                               }); // lambda function for the find_if
-
-    if (sen_it == getHardware()->getSensorList().end())
-        return nullptr;
-
-    return (*sen_it);
-}
-
-ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
-{
-    for (auto sen : getHardware()->getSensorList()) // loop all sensors
-    {
-        auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
-                                   sen->getProcessorList().end(),
-                                   [&](ProcessorBasePtr prc)
-                                   {
-                                       return prc->getName() == _processor_name;
-                                   }); // lambda function for the find_if
-
-        if (prc_it != sen->getProcessorList().end())  // found something!
-        {
-            if ((*prc_it)->isMotion()) // found, and it's motion!
-            {
-                std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
-                processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it);
-                return processor_motion_ptr_;
-            }
-            else // found, but it's not motion!
-            {
-                std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
-                return nullptr;
-            }
-        }
-    }
-    // nothing found!
-    std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
-    return nullptr;
-}
-
-void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr)
-{
-    processor_motion_ptr_ = _processor_motion_ptr;
-}
-
-void Problem::clearProcessorMotion()
-{
-    processor_motion_ptr_.reset();
-}
-
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
-                                   FrameType _frame_key_type, //
-                                   const Eigen::VectorXs& _frame_state, //
-                                   const TimeStamp& _time_stamp)
-{
-    FrameBasePtr frm = FrameFactory::get().create(_frame_structure, _frame_key_type, _time_stamp, _frame_state);
-    trajectory_ptr_->addFrame(frm);
-    return frm;
-}
-
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
-                                   FrameType _frame_key_type, //
-                                   const TimeStamp& _time_stamp)
-{
-    return emplaceFrame(_frame_structure, _frame_key_type, getState(_time_stamp), _time_stamp);
-}
-
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
-                                   const Eigen::VectorXs& _frame_state, //
-                                   const TimeStamp& _time_stamp)
-{
-    return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _frame_state, _time_stamp);
-}
-
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
-                                   const TimeStamp& _time_stamp)
-{
-    return emplaceFrame(trajectory_ptr_->getFrameStructure(), _frame_key_type, _time_stamp);
-}
-
-Eigen::VectorXs Problem::getCurrentState()
-{
-    Eigen::VectorXs state(getFrameStructureSize());
-    getCurrentState(state);
-    return state;
-}
-
-void Problem::getCurrentState(Eigen::VectorXs& state)
-{
-    if (processor_motion_ptr_ != nullptr)
-        processor_motion_ptr_->getCurrentState(state);
-    else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
-        trajectory_ptr_->getLastKeyFrame()->getState(state);
-    else
-        state = zeroState();
-}
-
-void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
-{
-    if (processor_motion_ptr_ != nullptr)
-    {
-        processor_motion_ptr_->getCurrentState(state);
-        processor_motion_ptr_->getCurrentTimeStamp(ts);
-    }
-    else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
-    {
-        trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts);
-        trajectory_ptr_->getLastKeyFrame()->getState(state);
-    }
-    else
-        state = zeroState();
-}
-
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
-{
-    // try to get the state from processor_motion if any, otherwise...
-    if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
-    {
-        FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-        if (closest_frame != nullptr)
-            closest_frame->getState(state);
-        else
-            state = zeroState();
-    }
-}
-
-Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
-{
-    Eigen::VectorXs state(getFrameStructureSize());
-    getState(_ts, state);
-    return state;
-}
-
-SizeEigen Problem::getFrameStructureSize() const
-{
-    return state_size_;
-}
-
-void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
-{
-    _x_size   = state_size_;
-    _cov_size = state_cov_size_;
-}
-
-SizeEigen Problem::getDim() const
-{
-    return dim_;
-}
-
-Eigen::VectorXs Problem::zeroState()
-{
-    Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());
-
-    // Set the quaternion identity for 3D states. Set only the real part to 1:
-    if (trajectory_ptr_->getFrameStructure() == "PO 3D" ||
-        trajectory_ptr_->getFrameStructure() == "POV 3D")
-        state(6) = 1.0;
-
-    return state;
-}
-
-bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
-{
-    // This should implement a black list of processors that have forbidden keyframe creation
-    // This decision is made at problem level, not at processor configuration level.
-    // If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
-
-    // Currently allowing all processors to vote:
-    return true;
-}
-
-void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
-{
-    if (_processor_ptr)
-    {
-        WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
-    }
-    else
-    {
-        WOLF_DEBUG("External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
-    }
-
-    for (auto sensor : hardware_ptr_->getSensorList())
-        for (auto processor : sensor->getProcessorList())
-            if (processor && (processor != _processor_ptr) )
-                processor->keyFrameCallback(_keyframe_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)
-{
-    //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
-    if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end())
-    {
-        WOLF_WARN("Adding a state block that has already been added");
-        return _state_ptr;
-    }
-
-    // add the state unit to the list
-    state_block_list_.push_back(_state_ptr);
-
-    // Add add notification
-    // Check if there is already a notification for this state block
-    auto notification_it = state_block_notification_map_.find(_state_ptr);
-    if (notification_it != state_block_notification_map_.end() && notification_it->second == ADD)
-    {
-        WOLF_WARN("There is already an ADD notification of this state block");
-    }
-    else
-        state_block_notification_map_[_state_ptr] = ADD;
-
-    return _state_ptr;
-}
-
-void Problem::removeStateBlock(StateBlockPtr _state_ptr)
-{
-    //std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
-    //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed");
-    if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end())
-    {
-        WOLF_WARN("Removing a state_block that hasn't been added or already removed");
-        return;
-    }
-
-    // add the state unit to the list
-    state_block_list_.remove(_state_ptr);
-
-    // Check if there is already a notification for this state block
-    auto notification_it = state_block_notification_map_.find(_state_ptr);
-    if (notification_it != state_block_notification_map_.end())
-    {
-        if (notification_it->second == REMOVE)
-        {
-            WOLF_WARN("There is already an REMOVE notification of this state block");
-        }
-        // Remove ADD notification
-        else
-        {
-            state_block_notification_map_.erase(notification_it);
-        }
-    }
-    // Add REMOVE notification
-    else
-        state_block_notification_map_[_state_ptr] = REMOVE;
-}
-
-FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
-{
-    //std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
-
-    // Add ADD notification
-    // Check if there is already a notification for this state block
-    auto notification_it = factor_notification_map_.find(_factor_ptr);
-    if (notification_it != factor_notification_map_.end() && notification_it->second == ADD)
-    {
-        WOLF_WARN("There is already an ADD notification of this factor");
-    }
-    // Add ADD notification (override in case of REMOVE)
-    else
-        factor_notification_map_[_factor_ptr] = ADD;
-
-    return _factor_ptr;
-}
-
-void Problem::removeFactor(FactorBasePtr _factor_ptr)
-{
-    //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
-
-    // Check if there is already a notification for this state block
-    auto notification_it = factor_notification_map_.find(_factor_ptr);
-    if (notification_it != factor_notification_map_.end())
-    {
-        if (notification_it->second == REMOVE)
-        {
-            WOLF_WARN("There is already an REMOVE notification of this state block");
-        }
-        // Remove ADD notification
-        else
-            factor_notification_map_.erase(notification_it);
-    }
-    // Add REMOVE notification
-    else
-        factor_notification_map_[_factor_ptr] = REMOVE;
-}
-
-void Problem::clearCovariance()
-{
-    covariances_.clear();
-}
-
-void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov)
-{
-    assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
-    assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
-
-    covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
-}
-
-void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov)
-{
-    assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
-    assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
-
-    covariances_[std::make_pair(_state1, _state1)] = _cov;
-}
-
-bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row,
-                                 const int _col)
-{
-    //std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
-    //std::cout << "_row " << _row << std::endl;
-    //std::cout << "_col " << _col << std::endl;
-    //std::cout << "_state1 size: " << _state1->getSize() << std::endl;
-    //std::cout << "_state2 size: " << _state2->getSize() << std::endl;
-    //std::cout << "part of cov to be filled:" << std::endl <<  _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) << std::endl;
-    //if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
-    //    std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl;
-    //else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
-    //    std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl;
-
-    assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-
-    if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
-        _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
-                covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
-    else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
-       _cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
-                covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose();
-    else
-    {
-      WOLF_DEBUG("Could not find the requested covariance block.");
-      return false;
-    }
-
-    return true;
-}
-
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
-{
-    // fill covariance
-    for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
-        for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
-        {
-            StateBlockPtr sb1 = it1->first;
-            StateBlockPtr sb2 = it2->first;
-            std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
-            std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);
-
-            // search st1 & st2
-            if (covariances_.find(pair_12) != covariances_.end())
-            {
-                assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-                assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-
-                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_12];
-                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_12].transpose();
-            }
-            else if (covariances_.find(pair_21) != covariances_.end())
-            {
-                assert(_sb_2_idx[sb1] + sb1->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb2] + sb2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-                assert(_sb_2_idx[sb2] + sb2->getSize() <= _cov.rows() &&
-                       _sb_2_idx[sb1] + sb1->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
-
-                _cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getSize(), sb2->getSize()) = covariances_[pair_21].transpose();
-                _cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getSize(), sb1->getSize()) = covariances_[pair_21];
-            }
-            else
-                return false;
-        }
-
-    return true;
-}
-
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col)
-{
-    return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
-}
-
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance)
-{
-    bool success(true);
-    int i = 0, j = 0;
-
-    const auto& state_bloc_vec = _frame_ptr->getStateBlockVec();
-
-    // computing size
-    SizeEigen sz = 0;
-    for (const auto& sb : state_bloc_vec)
-        if (sb)
-            sz += sb->getSize();
-
-    // resizing
-    _covariance = Eigen::MatrixXs(sz, sz);
-
-    // filling covariance
-    for (const auto& sb_i : state_bloc_vec)
-    {
-        if (sb_i)
-        {
-            j = 0;
-            for (const auto& sb_j : state_bloc_vec)
-            {
-                if (sb_j)
-                {
-                    success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-                    j += sb_j->getSize();
-                }
-            }
-            i += sb_i->getSize();
-        }
-    }
-    return success;
-}
-
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
-{
-    FrameBasePtr frm = getLastKeyFrame();
-    return getFrameCovariance(frm, cov);
-}
-
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance)
-{
-    bool success(true);
-    int i = 0, j = 0;
-
-    const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
-
-    // computing size
-    SizeEigen sz = 0;
-    for (const auto& sb : state_bloc_vec)
-        if (sb)
-            sz += sb->getSize();
-
-    // resizing
-    _covariance = Eigen::MatrixXs(sz, sz);
-
-    // filling covariance
-
-    for (const auto& sb_i : state_bloc_vec)
-    {
-        if (sb_i)
-        {
-            j = 0;
-            for (const auto& sb_j : state_bloc_vec)
-            {
-                if (sb_j)
-                {
-                    success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-                    j += sb_j->getSize();
-                }
-            }
-            i += sb_i->getSize();
-        }
-    }
-    return success;
-}
-
-MapBasePtr Problem::getMap()
-{
-    return map_ptr_;
-}
-
-TrajectoryBasePtr Problem::getTrajectory()
-{
-    return trajectory_ptr_;
-}
-
-HardwareBasePtr Problem::getHardware()
-{
-    return hardware_ptr_;
-}
-
-FrameBasePtr Problem::getLastFrame()
-{
-    return trajectory_ptr_->getLastFrame();
-}
-
-FrameBasePtr Problem::getLastKeyFrame()
-{
-    return trajectory_ptr_->getLastKeyFrame();
-}
-
-StateBlockPtrList& Problem::getStateBlockPtrList()
-{
-    return state_block_list_;
-}
-
-FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
-{
-    if ( ! prior_is_set_ )
-    {
-        // Create origin frame
-        FrameBasePtr origin_keyframe = emplaceFrame(IMPORTANT, _prior_state, _ts);
-
-        // create origin capture with the given state as data
-        // Capture fix only takes 3D position and Quaternion orientation
-        CapturePosePtr init_capture;
-        if (trajectory_ptr_->getFrameStructure() == "POV 3D")
-            init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
-        else
-            init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov);
-
-        origin_keyframe->addCapture(init_capture);
-
-        // emplace feature and factor
-        init_capture->emplaceFeatureAndFactor();
-
-        // Notify all processors about the prior KF
-        for (auto sensor : hardware_ptr_->getSensorList())
-            for (auto processor : sensor->getProcessorList())
-                if (processor->isMotion())
-                    // Motion processors will set its origin at the KF
-                    (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);
-
-        prior_is_set_ = true;
-
-        // Notify all other processors about the origin KF --> they will join it or not depending on their received data
-        for (auto sensor : hardware_ptr_->getSensorList())
-            for (auto processor : sensor->getProcessorList())
-                if ( !processor->isMotion() )
-                    processor->keyFrameCallback(origin_keyframe, _time_tolerance);
-
-        return origin_keyframe;
-    }
-    else
-        throw std::runtime_error("Origin already set!");
-}
-
-void Problem::loadMap(const std::string& _filename_dot_yaml)
-{
-    getMap()->load(_filename_dot_yaml);
-}
-
-void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
-{
-    getMap()->save(_filename_dot_yaml, _map_name);
-}
-
-void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
-{
-    using std::cout;
-    using std::endl;
-
-    cout << endl;
-    cout << "P: wolf tree status ---------------------" << endl;
-    cout << "Hardware" << ((depth < 1) ? ("   -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "")  << endl;
-    if (depth >= 1)
-    {
-        // Sensors =======================================================================================
-        for (auto S : getHardware()->getSensorList())
-        {
-            cout << "  S" << S->id() << " " << S->getType();
-            if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
-            if (depth < 2)
-                cout << " -- " << S->getProcessorList().size() << "p";
-            cout << endl;
-            if (metric && state_blocks)
-            {
-                for (unsigned int i = 0; i < S->getStateBlockVec().size(); i++)
-                {
-                    if (i==0) cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    if (i==2) cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlock(i);
-                    if (sb)
-                    {
-                        cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
-                    }
-                    if (i==1) cout << " ]" << endl;
-                }
-                if (S->getStateBlockVec().size() > 2) cout << " ]" << endl;
-            }
-            else if (metric)
-            {
-                cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
-                if (S->getP())
-                    cout << S->getP()->getState().transpose();
-                if (S->getO())
-                    cout << " " << S->getO()->getState().transpose();
-                cout  << " )";
-                if (S->getIntrinsic())
-                    cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
-                cout << endl;
-            }
-            else if (state_blocks)
-            {
-                cout << "    sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
-                for (auto sb : S->getStateBlockVec())
-                    if (sb != nullptr)
-                        cout << " " << (sb->isFixed() ? "Fix" : "Est");
-                cout << endl;
-            }
-            if (depth >= 2)
-            {
-                // Processors =======================================================================================
-                for (auto p : S->getProcessorList())
-                {
-                    if (p->isMotion())
-                    {
-                        std::cout << "    pm" << p->id() << " " << p->getType() << endl;
-                        ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
-                        if (pm->getOrigin())
-                            cout << "      o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isEstimated() ? "  KF" : "  F")
-                            << pm->getOrigin()->getFrame()->id() << endl;
-                        if (pm->getLast())
-                            cout << "      l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? "  KF" : "  F")
-                            << pm->getLast()->getFrame()->id() << endl;
-                        if (pm->getIncoming())
-                            cout << "      i: C" << pm->getIncoming()->id() << endl;
-                    }
-                    else
-                    {
-                        cout << "    pt" << p->id() << " " << p->getType() << endl;
-                        ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
-                        if (pt)
-                        {
-//                            ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
-//                            if (ptt)
-//                            {
-//                                if (ptt->getPrevOrigin())
-//                                    cout << "      p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? "  KF" : "  F")
-//                                    << ptt->getPrevOrigin()->getFrame()->id() << endl;
-//                            }
-                            if (pt->getOrigin())
-                                cout << "      o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isEstimated() ? "  KF" : "  F")
-                                << pt->getOrigin()->getFrame()->id() << endl;
-                            if (pt->getLast())
-                                cout << "      l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? "  KF" : "  F")
-                                << pt->getLast()->getFrame()->id() << endl;
-                            if (pt->getIncoming())
-                                cout << "      i: C" << pt->getIncoming()->id() << endl;
-                        }
-                    }
-                } // for p
-            }
-        } // for S
-    }
-    cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "")  << endl;
-    if (depth >= 1)
-    {
-        // Frames =======================================================================================
-        for (auto F : getTrajectory()->getFrameList())
-        {
-            cout << (F->isEstimated() ? (F->isImportant() ? "  KF" : "  EF") : "  F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
-            if (constr_by)
-            {
-                cout << "  <-- ";
-                for (auto cby : F->getConstrainedByList())
-                    cout << "c" << cby->id() << " \t";
-            }
-            cout << endl;
-            if (metric)
-            {
-                cout << (F->isFixed() ? "    Fix" : "    Est") << ", ts=" << std::setprecision(5)
-                        << F->getTimeStamp().get();
-                cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
-                cout << endl;
-            }
-            if (state_blocks)
-            {
-                cout << "    sb:";
-                for (auto sb : F->getStateBlockVec())
-                    if (sb != nullptr)
-                        cout << " " << (sb->isFixed() ? "Fix" : "Est");
-                cout << endl;
-            }
-            if (depth >= 2)
-            {
-                // Captures =======================================================================================
-                for (auto C : F->getCaptureList())
-                {
-                    cout << "    C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
-                    
-                    if(C->getSensor() != nullptr)
-                    {
-                        cout << " -> S" << C->getSensor()->id();
-                        cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
-                        cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
-                    }
-                    else
-                        cout << " -> S-";
-                    if (C->isMotion())
-                    {
-                        auto CM = std::static_pointer_cast<CaptureMotion>(C);
-                        if (CM->getOriginFrame())
-                            cout << " -> OF" << CM->getOriginFrame()->id();
-                    }
-
-                    cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
-                    if (constr_by)
-                    {
-                        cout << "  <-- ";
-                        for (auto cby : C->getConstrainedByList())
-                            cout << "c" << cby->id() << " \t";
-                    }
-                    cout << endl;
-
-                    if (state_blocks)
-                        for(auto sb : C->getStateBlockVec())
-                            if(sb != nullptr)
-                            {
-                                cout << "      sb: ";
-                                cout << (sb->isFixed() ? "Fix" : "Est");
-                                if (metric)
-                                    cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
-                                cout << endl;
-                            }
-
-                    if (C->isMotion() )
-                    {
-                        CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
-                        cout << "      buffer size  :  " << CM->getBuffer().get().size() << endl;
-                        if ( metric && ! CM->getBuffer().get().empty())
-                        {
-                            cout << "      delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
-                            if (CM->hasCalibration())
-                            {
-                                cout << "      calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
-                                cout << "      jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
-                                cout << "      calib current: (" << CM->getCalibration().transpose() << ")" << endl;
-                                cout << "      delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
-                            }
-                        }
-                    }
-
-                    if (depth >= 3)
-                    {
-                        // Features =======================================================================================
-                        for (auto f : C->getFeatureList())
-                        {
-                            cout << "      f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c  " : "");
-                            if (constr_by)
-                            {
-                                cout << "  <--\t";
-                                for (auto cby : f->getConstrainedByList())
-                                    cout << "c" << cby->id() << " \t";
-                            }
-                            cout << endl;
-                            if (metric)
-                                cout << "        m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
-                                        << " )" << endl;
-                            if (depth >= 4)
-                            {
-                                // Factors =======================================================================================
-                                for (auto c : f->getFactorList())
-                                {
-                                    cout << "        c" << c->id() << " " << c->getType() << " -->";
-                                    if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
-                                        cout << " A";
-                                    if (c->getFrameOther() != nullptr)
-                                        cout << " F" << c->getFrameOther()->id();
-                                    if (c->getCaptureOther() != nullptr)
-                                        cout << " C" << c->getCaptureOther()->id();
-                                    if (c->getFeatureOther() != nullptr)
-                                        cout << " f" << c->getFeatureOther()->id();
-                                    if (c->getLandmarkOther() != nullptr)
-                                        cout << " L" << c->getLandmarkOther()->id();
-                                    cout << endl;
-                                } // for c
-                            }
-                        } // for f
-                    }
-                } // for C
-            }
-        } // for F
-    }
-    cout << "Map" << ((depth < 1) ? ("        -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
-    if (depth >= 1)
-    {
-        // Landmarks =======================================================================================
-        for (auto L : getMap()->getLandmarkList())
-        {
-            cout << "  L" << L->id() << " " << L->getType();
-            if (constr_by)
-            {
-                cout << "\t<-- ";
-                for (auto cby : L->getConstrainedByList())
-                    cout << "c" << cby->id() << " \t";
-            }
-            cout << endl;
-            if (metric)
-            {
-                cout << (L->isFixed() ? "    Fix" : "    Est");
-                cout << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
-                cout << endl;
-            }
-            if (state_blocks)
-            {
-                cout << "    sb:";
-                for (auto sb : L->getStateBlockVec())
-                    if (sb != nullptr)
-                        cout << (sb->isFixed() ? " Fix" : " Est");
-                cout << endl;
-            }
-        } // for L
-    }
-    cout << "-----------------------------------------" << endl;
-    cout << endl;
-}
-
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
-{
-    return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
-
-bool Problem::check(int verbose_level)
-{
-    using std::cout;
-    using std::endl;
-
-    bool is_consistent = true; // true if all checks passed; false if any check fails.
-
-    if (verbose_level) cout << endl;
-    if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
-    auto P_raw = this;
-    if (verbose_level > 0)
-    {
-        cout << "P @ " << P_raw << endl;
-    }
-    // ------------------------
-    //     HARDWARE branch
-    // ------------------------
-    auto H = hardware_ptr_;
-    if (verbose_level > 0)
-    {
-        cout << "H @ " << H.get() << endl;
-    }
-    // check pointer to Problem
-    is_consistent = is_consistent && (H->getProblem().get() == P_raw);
-
-    // Sensors =======================================================================================
-    for (auto S : H->getSensorList())
-    {
-        if (verbose_level > 0)
-        {
-            cout << "  S" << S->id() << " @ " << S.get() << endl;
-            cout << "    -> P @ " << S->getProblem().get() << endl;
-            cout << "    -> H @ " << S->getHardware().get() << endl;
-            for (auto sb : S->getStateBlockVec())
-            {
-                cout <<  "    sb @ " << sb.get();
-                if (sb)
-                {
-                    auto lp = sb->getLocalParametrization();
-                    if (lp)
-                        cout <<  " (lp @ " << lp.get() << ")";
-                }
-                cout << endl;
-            }
-        }
-        // check problem and hardware pointers
-        is_consistent = is_consistent && (S->getProblem().get() == P_raw);
-        is_consistent = is_consistent && (S->getHardware() == H);
-
-        // Processors =======================================================================================
-        for (auto p : S->getProcessorList())
-        {
-            if (verbose_level > 0)
-            {
-                cout << "    p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
-                cout << "      -> P  @ " << p->getProblem().get() << endl;
-                cout << "      -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl;
-            }
-            // check problem and sensor pointers
-            is_consistent = is_consistent && (p->getProblem().get() == P_raw);
-            is_consistent = is_consistent && (p->getSensor() == S);
-        }
-    }
-    // ------------------------
-    //    TRAJECTORY branch
-    // ------------------------
-    auto T = trajectory_ptr_;
-    if (verbose_level > 0)
-    {
-        cout << "T @ " << T.get() << endl;
-    }
-    // check pointer to Problem
-    is_consistent = is_consistent && (T->getProblem().get() == P_raw);
-
-    // Frames =======================================================================================
-    for (auto F : T->getFrameList())
-    {
-        if (verbose_level > 0)
-        {
-            cout << (F->isEstimated() ? "  KF" : "  F") << F->id() << " @ " << F.get() << endl;
-            cout << "    -> P @ " << F->getProblem().get() << endl;
-            cout << "    -> T @ " << F->getTrajectory().get() << endl;
-            for (auto sb : F->getStateBlockVec())
-            {
-                cout <<  "    sb @ " << sb.get();
-                if (sb)
-                {
-                    auto lp = sb->getLocalParametrization();
-                    if (lp)
-                        cout <<  " (lp @ " << lp.get() << ")";
-                }
-                cout << endl;
-            }
-        }
-        // check problem and trajectory pointers
-        is_consistent = is_consistent && (F->getProblem().get() == P_raw);
-        is_consistent = is_consistent && (F->getTrajectory() == T);
-        for (auto cby : F->getConstrainedByList())
-        {
-            if (verbose_level > 0)
-            {
-                cout << "    <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
-            }
-            // check constrained_by pointer to this frame
-            is_consistent = is_consistent && (cby->getFrameOther() == F);
-            for (auto sb : cby->getStateBlockPtrVector())
-            {
-                if (verbose_level > 0)
-                {
-                    cout << "      sb @ " << sb.get();
-                    if (sb)
-                    {
-                        auto lp = sb->getLocalParametrization();
-                        if (lp)
-                            cout <<  " (lp @ " << lp.get() << ")";
-                    }
-                    cout << endl;
-                }
-            }
-        }
-
-        // Captures =======================================================================================
-        for (auto C : F->getCaptureList())
-        {
-            if (verbose_level > 0)
-            {
-                cout << "    C" << C->id() << " @ " << C.get() << " -> S";
-                if (C->getSensor()) cout << C->getSensor()->id();
-                else cout << "-";
-                cout << endl;
-                cout << "      -> P  @ " << C->getProblem().get() << endl;
-                cout << "      -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
-                for (auto sb : C->getStateBlockVec())
-                {
-                    cout <<  "      sb @ " << sb.get();
-                    if (sb)
-                    {
-                        auto lp = sb->getLocalParametrization();
-                        if (lp)
-                            cout <<  " (lp @ " << lp.get() << ")";
-                    }
-                    cout << endl;
-                }
-            }
-            // check problem and frame pointers
-            is_consistent = is_consistent && (C->getProblem().get() == P_raw);
-            is_consistent = is_consistent && (C->getFrame() == F);
-
-            // Features =======================================================================================
-            for (auto f : C->getFeatureList())
-            {
-                if (verbose_level > 0)
-                {
-                    cout << "      f" << f->id() << " @ " << f.get() << endl;
-                    cout << "        -> P  @ " << f->getProblem().get() << endl;
-                    cout << "        -> C" << f->getCapture()->id() << " @ " << f->getCapture().get()
-                            << endl;
-                }
-                // check problem and capture pointers
-                is_consistent = is_consistent && (f->getProblem().get() == P_raw);
-                is_consistent = is_consistent && (f->getCapture() == C);
-
-                for (auto cby : f->getConstrainedByList())
-                {
-                    if (verbose_level > 0)
-                    {
-                        cout << "     <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
-                    }
-                    // check constrained_by pointer to this feature
-                    is_consistent = is_consistent && (cby->getFeatureOther() == f);
-                }
-
-                // Factors =======================================================================================
-                for (auto c : f->getFactorList())
-                {
-                    if (verbose_level > 0)
-                        cout << "        c" << c->id() << " @ " << c.get();
-
-                    auto Fo = c->getFrameOther();
-                    auto Co = c->getCaptureOther();
-                    auto fo = c->getFeatureOther();
-                    auto Lo = c->getLandmarkOther();
-
-                    if ( !Fo && !Co && !fo && !Lo )    // case ABSOLUTE:
-                    {
-                        if (verbose_level > 0)
-                            cout << " --> Abs." << endl;
-                    }
-
-                    // find constrained_by pointer in constrained frame
-                    if ( Fo )  // case FRAME:
-                    {
-                        if (verbose_level > 0)
-                            cout << " --> F" << Fo->id() << " <- ";
-                        bool found = false;
-                        for (auto cby : Fo->getConstrainedByList())
-                        {
-                            if (verbose_level > 0)
-                                cout << " c" << cby->id();
-                            found = found || (c == cby);
-                        }
-                        if (verbose_level > 0)
-                            cout << endl;
-                        // check constrained_by pointer in constrained frame
-                        is_consistent = is_consistent && found;
-                    }
-
-                    // find constrained_by pointer in constrained capture
-                    if ( Co )  // case CAPTURE:
-                    {
-                        if (verbose_level > 0)
-                            cout << " --> C" << Co->id() << " <- ";
-                        bool found = false;
-                        for (auto cby : Co->getConstrainedByList())
-                        {
-                            if (verbose_level > 0)
-                                cout << " c" << cby->id();
-                            found = found || (c == cby);
-                        }
-                        if (verbose_level > 0)
-                            cout << endl;
-                        // check constrained_by pointer in constrained frame
-                        is_consistent = is_consistent && found;
-                    }
-
-                    // find constrained_by pointer in constrained feature
-                    if ( fo )   // case FEATURE:
-                    {
-                        if (verbose_level > 0)
-                            cout << " --> f" << fo->id() << " <- ";
-                        bool found = false;
-                        for (auto cby : fo->getConstrainedByList())
-                        {
-                            if (verbose_level > 0)
-                                cout << " c" << cby->id();
-                            found = found || (c == cby);
-                        }
-                        if (verbose_level > 0)
-                            cout << endl;
-                        // check constrained_by pointer in constrained feature
-                        is_consistent = is_consistent && found;
-                    }
-
-                    // find constrained_by pointer in constrained landmark
-                    if ( Lo )      // case LANDMARK:
-                    {
-                        if (verbose_level > 0)
-                            cout << " --> L" << Lo->id() << " <- ";
-                        bool found = false;
-                        for (auto cby : Lo->getConstrainedByList())
-                        {
-                            if (verbose_level > 0)
-                                cout << " c" << cby->id();
-                            found = found || (c == cby);
-                        }
-                        if (verbose_level > 0)
-                            cout << endl;
-                        // check constrained_by pointer in constrained landmark
-                        is_consistent = is_consistent && found;
-                    }
-                    if (verbose_level > 0)
-                    {
-                        cout << "          -> P  @ " << c->getProblem().get() << endl;
-                        cout << "          -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
-                    }
-                    // check problem and feature pointers
-                    is_consistent = is_consistent && (c->getProblem().get() == P_raw);
-                    is_consistent = is_consistent && (c->getFeature() == f);
-
-                    // find state block pointers in all constrained nodes
-                    SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
-                    for (auto sb : c->getStateBlockPtrVector())
-                    {
-                        bool found = false;
-                        if (verbose_level > 0)
-                        {
-                            cout <<  "          sb @ " << sb.get();
-                            if (sb)
-                            {
-                                auto lp = sb->getLocalParametrization();
-                                if (lp)
-                                    cout <<  " (lp @ " << lp.get() << ")";
-                            }
-                        }
-                        // find in own Frame
-                        found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end());
-                        // find in own Capture
-                        found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
-                        // find in own Sensor
-                        if (S)
-                            found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end());
-                        // find in constrained Frame
-                        if (Fo)
-                            found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end());
-                        // find in constrained Capture
-                        if (Co)
-                            found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
-                        // find in constrained Feature
-                        if (fo)
-                        {
-                            // find in constrained feature's Frame
-                            FrameBasePtr foF = fo->getFrame();
-                            found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
-                            // find in constrained feature's Capture
-                            CaptureBasePtr foC = fo->getCapture();
-                            found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
-                            // find in constrained feature's Sensor
-                            SensorBasePtr foS = fo->getCapture()->getSensor();
-                            found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
-                        }
-                        // find in constrained landmark
-                        if (Lo)
-                            found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
-                        if (verbose_level > 0)
-                        {
-                            if (found)
-                                cout << " found";
-                            else
-                                cout << " NOT FOUND !";
-                            cout << endl;
-                        }
-                        // check that all state block pointers were found
-                        is_consistent = is_consistent && found;
-                    }
-                }
-            }
-        }
-    }
-    // ------------------------
-    //       MAP branch
-    // ------------------------
-    auto M = map_ptr_;
-    if (verbose_level > 0)
-        cout << "M @ " << M.get() << endl;
-    // check pointer to Problem
-    is_consistent = is_consistent && (M->getProblem().get() == P_raw);
-
-    // Landmarks =======================================================================================
-    for (auto L : M->getLandmarkList())
-    {
-        if (verbose_level > 0)
-        {
-            cout << "  L" << L->id() << " @ " << L.get() << endl;
-            cout << "  -> P @ " << L->getProblem().get() << endl;
-            cout << "  -> M @ " << L->getMap().get() << endl;
-            for (auto sb : L->getStateBlockVec())
-            {
-                cout <<  "  sb @ " << sb.get();
-                if (sb)
-                {
-                    auto lp = sb->getLocalParametrization();
-                    if (lp)
-                        cout <<  " (lp @ " << lp.get() << ")";
-                }
-                cout << endl;
-            }
-        }
-        // check problem and map pointers
-        is_consistent = is_consistent && (L->getProblem().get() == P_raw);
-        is_consistent = is_consistent && (L->getMap() == M);
-        for (auto cby : L->getConstrainedByList())
-        {
-            if (verbose_level > 0)
-                cout << "      <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
-            // check constrained_by pointer to this landmark
-            is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
-            for (auto sb : cby->getStateBlockPtrVector())
-            {
-                if (verbose_level > 0)
-                {
-                    cout << "      sb @ " << sb.get();
-                    if (sb)
-                    {
-                        auto lp = sb->getLocalParametrization();
-                        if (lp)
-                            cout <<  " (lp @ " << lp.get() << ")";
-                    }
-                    cout << endl;
-                }
-            }
-        }
-    }
-
-    if (verbose_level) cout << "--------------------------- Wolf tree " << (is_consistent ? " OK" : "Not OK !!") << endl;
-    if (verbose_level) cout << endl;
-
-    return is_consistent;
-}
-
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks)
-{
-    if (depth.compare("T") == 0)
-        print(0, constr_by, metric, state_blocks);
-    else if (depth.compare("F") == 0)
-        print(1, constr_by, metric, state_blocks);
-    else if (depth.compare("C") == 0)
-        print(2, constr_by, metric, state_blocks);
-    else if (depth.compare("f") == 0)
-        print(3, constr_by, metric, state_blocks);
-    else if (depth.compare("c") == 0)
-        print(4, constr_by, metric, state_blocks);
-    else
-        print(0, constr_by, metric, state_blocks);
-}
-
-} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 3f5214a6f120001d6e37627fabb2f97ad9b9c103..bb913e042a5b14a4e7e1c3bfe9c1f8fbdf858e13 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -127,7 +127,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
 
     // setting the origin in all processor motion if origin already setted
     if (prc_ptr->isMotion() && prior_is_set_)
-        (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastImportantFrame());
+        (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
 
     // setting the main processor motion
     if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
@@ -251,8 +251,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
 {
     if (processor_motion_ptr_ != nullptr)
         processor_motion_ptr_->getCurrentState(state);
-    else if (trajectory_ptr_->getLastImportantFrame() != nullptr)
-        trajectory_ptr_->getLastImportantFrame()->getState(state);
+    else if (trajectory_ptr_->getLastEstimatedFrame() != nullptr)
+        trajectory_ptr_->getLastEstimatedFrame()->getState(state);
     else
         state = zeroState();
 }
@@ -264,10 +264,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
         processor_motion_ptr_->getCurrentState(state);
         processor_motion_ptr_->getCurrentTimeStamp(ts);
     }
-    else if (trajectory_ptr_->getLastImportantFrame() != nullptr)
+    else if (trajectory_ptr_->getLastEstimatedFrame() != nullptr)
     {
-        trajectory_ptr_->getLastImportantFrame()->getTimeStamp(ts);
-        trajectory_ptr_->getLastImportantFrame()->getState(state);
+        trajectory_ptr_->getLastEstimatedFrame()->getTimeStamp(ts);
+        trajectory_ptr_->getLastEstimatedFrame()->getState(state);
     }
     else
         state = zeroState();
@@ -348,6 +348,30 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
                 processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
 }
 
+bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr)
+{
+    // This should implement a black list of processors that have forbidden auxiliary frame creation
+    // This decision is made at problem level, not at processor configuration level.
+    // If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
+
+    // Currently allowing all processors to vote:
+    return true;
+}
+
+void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
+{
+    if (_processor_ptr)
+    {
+        WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+    }
+    else
+    {
+        WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+    }
+
+    processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+}
+
 LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
 {
     getMap()->addLandmark(_lmk_ptr);
@@ -583,7 +607,13 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
 
 bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov)
 {
-    FrameBasePtr frm = getLastImportantFrame();
+    FrameBasePtr frm = getLastKeyFrame();
+    return getFrameCovariance(frm, cov);
+}
+
+bool Problem::getLastEstimatedFrameCovariance(Eigen::MatrixXs& cov)
+{
+    FrameBasePtr frm = getLastEstimatedFrame();
     return getFrameCovariance(frm, cov);
 }
 
@@ -644,9 +674,9 @@ FrameBasePtr Problem::getLastFrame() const
     return trajectory_ptr_->getLastFrame();
 }
 
-FrameBasePtr Problem::getLastImportantFrame() const
+FrameBasePtr Problem::getLastKeyFrame() const
 {
-    return trajectory_ptr_->getLastImportantFrame();
+    return trajectory_ptr_->getLastKeyFrame();
 }
 
 FrameBasePtr Problem::getLastEstimatedFrame() const
@@ -654,9 +684,9 @@ FrameBasePtr Problem::getLastEstimatedFrame() const
     return trajectory_ptr_->getLastEstimatedFrame();
 }
 
-FrameBasePtr Problem::closestImportantFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
 {
-    return trajectory_ptr_->closestImportantFrameToTimeStamp(_ts);
+    return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
 }
 
 FrameBasePtr Problem::closestEstimatedFrameToTimeStamp(const TimeStamp& _ts) const
@@ -669,7 +699,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
     if ( ! prior_is_set_ )
     {
         // Create origin frame
-        FrameBasePtr origin_keyframe = emplaceFrame(IMPORTANT, _prior_state, _ts);
+        FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
 
         // create origin capture with the given state as data
         // Capture fix only takes 3D position and Quaternion orientation
@@ -778,10 +808,10 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                         std::cout << "    pm" << p->id() << " " << p->getType() << endl;
                         ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
                         if (pm->getOrigin())
-                            cout << "      o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isEstimated() ? (pm->getOrigin()->getFrame()->isImportant() ? "  KF" : "  EF" ) : "  F")
+                            cout << "      o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isEstimated() ? (pm->getOrigin()->getFrame()->isKey() ? "  KF" : "  AuxF" ) : "  F")
                             << pm->getOrigin()->getFrame()->id() << endl;
                         if (pm->getLast())
-                            cout << "      l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? (pm->getLast()->getFrame()->isImportant() ? "  KF" : " EF") : "  F")
+                            cout << "      l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isEstimated() ? (pm->getLast()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
                             << pm->getLast()->getFrame()->id() << endl;
                         if (pm->getIncoming())
                             cout << "      i: C" << pm->getIncoming()->id() << endl;
@@ -796,14 +826,14 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
 //                            if (ptt)
 //                            {
 //                                if (ptt->getPrevOrigin())
-//                                    cout << "      p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isImportant() ? "  KF" : " EF") : "  F")
+//                                    cout << "      p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isEstimated() ? (ptt->getPrevOrigin()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
 //                                    << ptt->getPrevOrigin()->getFrame()->id() << endl;
 //                            }
                             if (pt->getOrigin())
-                                cout << "      o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isEstimated() ? (pt->getOrigin()->getFrame()->isImportant() ? "  KF" : " EF") : "  F")
+                                cout << "      o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isEstimated() ? (pt->getOrigin()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
                                 << pt->getOrigin()->getFrame()->id() << endl;
                             if (pt->getLast())
-                                cout << "      l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? (pt->getLast()->getFrame()->isImportant() ? "  KF" : " EF") : "  F")
+                                cout << "      l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isEstimated() ? (pt->getLast()->getFrame()->isKey() ? "  KF" : " AuxF") : "  F")
                                 << pt->getLast()->getFrame()->id() << endl;
                             if (pt->getIncoming())
                                 cout << "      i: C" << pt->getIncoming()->id() << endl;
@@ -819,7 +849,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
         // Frames =======================================================================================
         for (auto F : getTrajectory()->getFrameList())
         {
-            cout << (F->isEstimated() ? (F->isImportant() ? "  KF" : " EF") : "  F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
+            cout << (F->isEstimated() ? (F->isKey() ? "  KF" : " AuxF") : "  F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
             if (constr_by)
             {
                 cout << "  <-- ";
@@ -1055,7 +1085,7 @@ bool Problem::check(int verbose_level)
     {
         if (verbose_level > 0)
         {
-            cout << (F->isEstimated() ? (F->isImportant() ? "  KF" : " EF") : "  F") << F->id() << " @ " << F.get() << endl;
+            cout << (F->isEstimated() ? (F->isKey() ? "  KF" : " EF") : "  F") << F->id() << " @ " << F.get() << endl;
             cout << "    -> P @ " << F->getProblem().get() << endl;
             cout << "    -> T @ " << F->getTrajectory().get() << endl;
             for (auto sb : F->getStateBlockVec())
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 9de484dfa474baf1cc8bc9bc56b4b20eed7aeb89..260f416988b32c8a9ffc8ccbcbc014a4d0be021a 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -28,7 +28,7 @@ bool ProcessorBase::permittedKeyFrame()
 
 FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr)
 {
-    std::cout << "Making " << (_type == ESTIMATED? "key-" : "") << "frame" << std::endl;
+    std::cout << "Making " << (_type == KEY ? "key-" : (_type == AUXILIARY ? "aux-" : "")) << "frame" << std::endl;
 
     FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(_type, _capture_ptr->getTimeStamp());
     new_frame_ptr->addCapture(_capture_ptr);
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index ee1d2e03e79b6c71439a112e6afe6f8d9115417f..7c4e63a75031de399473c9fa1c6e764fa28331a9 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -72,7 +72,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
     // check for LC just if frame is key frame
     // Assert that the evaluated KF has a capture of the
     // same sensor as this processor
-    if (key_it->isEstimated() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
+    if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
     {
       // Check if the two frames currently evaluated are already
       // constrained one-another.
@@ -82,7 +82,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
       for (const FactorBasePtr& crt : fac_list)
       {
         // Are the two frames constrained one-another ?
-        if (crt->getFrameOther() == problem_ptr->getLastImportantFrame())
+        if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
         {
           // By this very processor ?
           if (crt->getProcessor() == shared_from_this())
@@ -127,7 +127,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
         Eigen::Vector5s frame_covariance, current_covariance;
         if (!computeEllipse2D(key_it,
                               frame_covariance)) continue;
-        if (!computeEllipse2D(getProblem()->getLastImportantFrame(),
+        if (!computeEllipse2D(getProblem()->getLastKeyFrame(),
                               current_covariance)) continue;
         found_possible_candidate = ellipse2DIntersect(frame_covariance,
                                                       current_covariance);
@@ -160,7 +160,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
         Eigen::Vector10s frame_covariance, current_covariance;
         if (!computeEllipsoid3D(key_it,
                                 frame_covariance)) continue;
-        if (!computeEllipsoid3D(getProblem()->getLastImportantFrame(),
+        if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(),
                                 frame_covariance)) continue;
         found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
                                                         current_covariance);
@@ -170,7 +170,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
       case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
       {
         found_possible_candidate = insideMahalanisDistance(key_it,
-                                             problem_ptr->getLastImportantFrame());
+                                             problem_ptr->getLastKeyFrame());
         break;
       }
 
@@ -496,7 +496,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
 //##############################################################################
 bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
 {
-  FrameBasePtr keyframe = getProblem()->getLastImportantFrame();
+  FrameBasePtr keyframe = getProblem()->getLastKeyFrame();
   if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
     return false;
   else
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 622c766d2352833020a757ae1b66dfda42a50ec1..755b6c95091c88ad7c5f3362caa3950714046d0f 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -193,11 +193,11 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
     last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
     last_ptr_->getFrame()->setState(getCurrentState());
 
-    if (voteForKeyFrame() && permittedKeyFrame())
+    if (permittedKeyFrame() && voteForKeyFrame())
     {
         // Set the frame of last_ptr as key
         auto key_frame_ptr = last_ptr_->getFrame();
-        key_frame_ptr->setEstimated();
+        key_frame_ptr->setKey();
 
         // create motion feature and add it to the key_capture
         auto key_feature_ptr = emplaceFeature(last_ptr_);
@@ -288,7 +288,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
 
 FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
 {
-    FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(ESTIMATED, _x_origin, _ts_origin);
+    FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
     setOrigin(key_frame_ptr);
 
     return key_frame_ptr;
@@ -299,7 +299,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
     assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
     assert(_origin_frame->getTrajectory() != nullptr
             && "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
-    assert(_origin_frame->isEstimated() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+    assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
 
     // -------- ORIGIN ---------
     // emplace (empty) origin Capture
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 87be1401e46bb0493b33c1dd99cfdb2a64653f5c..5d97526e108fccced45d966bd49c3e5d795c9b84 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -75,7 +75,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         }
         case FIRST_TIME_WITHOUT_PACK :
         {
-            FrameBasePtr kfrm = getProblem()->emplaceFrame(ESTIMATED, incoming_ptr_->getTimeStamp());
+            FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
             kfrm->addCapture(incoming_ptr_);
 
             // Process info
@@ -171,7 +171,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
 
                 // set KF on last
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
-                last_ptr_->getFrame()->setEstimated();
+                last_ptr_->getFrame()->setKey();
 
                 // make F; append incoming to new F
                 FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
@@ -180,9 +180,6 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 // process
                 processNew(params_tracker_->max_new_features);
 
-                //                // Set key
-                //                last_ptr_->getFrame()->setKey();
-                //
                 // Set state to the keyframe
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
 
@@ -258,7 +255,7 @@ void ProcessorTracker::computeProcessingStep()
 
             if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
             {
-                if (last_ptr_->getFrame()->isEstimated())
+                if (last_ptr_->getFrame()->isKey())
                 {
                     WOLF_WARN("||*||");
                     WOLF_INFO(" ... It seems you missed something!");
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 26cd97067e33d1a3546e1422e3527d31f8ac9e54..209dc12e04ed770ec797f2d10b27173b17fe0458 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -226,7 +226,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void)
     FrameBaseRevIter frame_rev_it = frame_list.rbegin();
     while (frame_rev_it != frame_list.rend())
     {
-        if ((*frame_rev_it)->isEstimated())
+        if ((*frame_rev_it)->isKey())
         {
             capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
             if (capture)
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 735e4187079038eed700c8a499b1b6a4ae4b85fb..ca899deef67722885fd6dab673ec834410237ebb 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -6,7 +6,7 @@ namespace wolf {
 TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
     NodeBase("TRAJECTORY", "Base"),
     frame_structure_(_frame_structure),
-    last_important_frame_ptr_(nullptr),
+    last_key_frame_ptr_(nullptr),
     last_estimated_frame_ptr_(nullptr)
 {
 //    WOLF_DEBUG("constructed T");
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index 202f566a6eca49236165a2a2c8a10bb111a4b20d..7c65c1a341a691aaf3135f7c206717d3056634b0 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -480,7 +480,7 @@ class Process_Factor_IMU : public testing::Test
         virtual void buildProblem()
         {
             // ===================================== SET KF in Wolf tree
-            FrameBasePtr KF = problem->emplaceFrame(ESTIMATED, x1_exact, t);
+            FrameBasePtr KF = problem->emplaceFrame(KEY, x1_exact, t);
 
             // ===================================== IMU CALLBACK
             problem->keyFrameCallback(KF, nullptr, dt/2);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 906d810cfb843b91606c7c32afb79d66e59a4fe3..ad8852abc92a528381ed5f8b859f9909309617f0 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -318,7 +318,7 @@ TEST(CeresManager, AddFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -340,7 +340,7 @@ TEST(CeresManager, DoubleAddFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -365,7 +365,7 @@ TEST(CeresManager, RemoveFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -393,7 +393,7 @@ TEST(CeresManager, AddRemoveFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -420,7 +420,7 @@ TEST(CeresManager, DoubleRemoveFactor)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -547,7 +547,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) 2 factors quaternion
-    FrameBasePtr                    F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr                    F = P->emplaceFrame(KEY, 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())));
@@ -589,7 +589,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
     CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
 
     // Create (and add) 2 factors quaternion
-    FrameBasePtr                    F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr                    F = P->emplaceFrame(KEY, 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())));
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index b445e4b1a77eb3f64d43c135ecebdec2855bf16d..fd9650de70d970b9336d62e8471f3c5a090c0221 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV 3D");
 CeresManager ceres_mgr(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(ESTIMATED, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 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));
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index 3b64eafe603c25edbc91070e0718874c3044b8ea..b9c03ab04dcafa19f9fd850a8b72508e25ba02b3 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -55,9 +55,9 @@ class FactorAutodiffDistance3D_Test : public testing::Test
             problem = Problem::create("PO 3D");
             ceres_manager = std::make_shared<CeresManager>(problem);
 
-            F1 = problem->emplaceFrame        (ESTIMATED, pose1, 1.0);
+            F1 = problem->emplaceFrame        (KEY, pose1, 1.0);
 
-            F2 = problem->emplaceFrame        (ESTIMATED, pose2, 2.0);
+            F2 = problem->emplaceFrame        (KEY, pose2, 2.0);
             C2 = std::make_shared<CaptureBase>("Base", 1.0);
             F2->addCapture(C2);
             f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 5eb632976ba8ca2793226992b366b1e499971d5b..78223c6020f9241c9a4faa7d24db5346c06e9c6d 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -133,19 +133,19 @@ class FactorAutodiffTrifocalTest : public testing::Test{
             Vector2s pix(0,0);
             Matrix2s pix_cov; pix_cov.setIdentity();
 
-            F1 = problem->emplaceFrame(ESTIMATED, pose1, 1.0);
+            F1 = problem->emplaceFrame(KEY, pose1, 1.0);
             I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
             F1-> addCapture(I1);
             f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
             I1-> addFeature(f1);
 
-            F2 = problem->emplaceFrame(ESTIMATED, pose2, 2.0);
+            F2 = problem->emplaceFrame(KEY, pose2, 2.0);
             I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1));
             F2-> addCapture(I2);
             f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
             I2-> addFeature(f2);
 
-            F3 = problem->emplaceFrame(ESTIMATED, pose3, 3.0);
+            F3 = problem->emplaceFrame(KEY, pose3, 3.0);
             I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1));
             F3-> addCapture(I3);
             f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index b7c8e5899d8036e0cf8338793482c3fec700059c..2b8a6d20160d9464ffa7b48ff9044b66b03d50d9 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -37,8 +37,8 @@ ProblemPtr problem_ptr = Problem::create("PO 3D");
 CeresManager ceres_mgr(problem_ptr);
 
 // Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(ESTIMATED, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(ESTIMATED, delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 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));
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index aa1b2db5d9d1c2a64c0a28ba53bffb4fe0eb130a..d07b564a3caeed5dd34bb9e107776bc769f3d85d 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO 2D");
 CeresManager ceres_mgr(problem);
 
 // Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(ESTIMATED, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, 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));
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index e7f91dc2d995debc25bb97215cc295039b20971e..620e389c6b5a483f84250584cbe6d318e62ec55b 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO 3D");
 CeresManager ceres_mgr(problem);
 
 // Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(ESTIMATED, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
 
 // Capture, feature and factor
 CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 3cc3a029bba96de511e7b22de6e1f0a30e6b7ae8..c9d894e9ff3a8fafdee13087ef7364bf7173277f 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -77,7 +77,7 @@ 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>(ESTIMATED, 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 = std::make_shared<FrameBase>(KEY, 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
@@ -119,9 +119,9 @@ TEST_F(FeatureIMU_test, check_frame)
     origin_frame->getTimeStamp(ts);
 
     ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
-    ASSERT_TRUE(origin_frame->isEstimated());
-    ASSERT_TRUE(last_frame->isEstimated());
-    ASSERT_TRUE(left_frame->isEstimated());
+    ASSERT_TRUE(origin_frame->isKey());
+    ASSERT_TRUE(last_frame->isKey());
+    ASSERT_TRUE(left_frame->isKey());
 
     wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
     origin_pptr = origin_frame->getP();
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 0e0ff058e4cd43c481f68880ccdb430460504f6e..fc87237511b807fb40e7fa7fd2f4f6a0bf52519f 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -31,6 +31,7 @@ TEST(FrameBase, GettersAndSetters)
     F->getTimeStamp(t);
     ASSERT_EQ(t, 1);
     ASSERT_FALSE(F->isFixed());
+    ASSERT_EQ(F->isKey(), false);
     ASSERT_EQ(F->isEstimated(), false);
 }
 
@@ -121,7 +122,8 @@ TEST(FrameBase, LinksToTree)
     ASSERT_TRUE(F1->isFixed());
 
     // set key
-    F1->setEstimated();
+    F1->setKey();
+    ASSERT_TRUE(F1->isKey());
     ASSERT_TRUE(F1->isEstimated());
 
     // Unlink
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 7043b1be1b46e838db1ea6905f808a0ad43732ce..51dbc519a84fee9b38d393250ac90af0d42e9594 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -75,7 +75,7 @@ void show(const ProblemPtr& problem)
 
     for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
     {
-        if (F->isEstimated())
+        if (F->isKey())
         {
             cout << "----- Key Frame " << F->id() << " -----" << endl;
             if (!F->getCaptureList().empty())
@@ -131,7 +131,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
 
     // KF1 and motion from KF0
     t += dt;
-    FrameBasePtr        F1 = Pr->emplaceFrame(ESTIMATED, Vector3s::Zero(), t);
+    FrameBasePtr        F1 = Pr->emplaceFrame(KEY, 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));
@@ -139,7 +139,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
 
     // KF2 and motion from KF1
     t += dt;
-    FrameBasePtr        F2 = Pr->emplaceFrame(ESTIMATED, Vector3s::Zero(), t);
+    FrameBasePtr        F2 = Pr->emplaceFrame(KEY, 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));
@@ -404,7 +404,7 @@ TEST(Odom2D, KF_callback)
     std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
 
     Vector3s x_split = processor_odom2d->getState(t_split);
-    FrameBasePtr keyframe_2 = problem->emplaceFrame(ESTIMATED, x_split, t_split);
+    FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
 
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
@@ -436,7 +436,7 @@ TEST(Odom2D, KF_callback)
     problem->print(4,1,1,1);
 
     x_split = processor_odom2d->getState(t_split);
-    FrameBasePtr keyframe_1 = problem->emplaceFrame(ESTIMATED, x_split, t_split);
+    FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, x_split, t_split);
 
     ASSERT_TRUE(problem->check(0));
     processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 17e02e54087f0dc7ec6d5f017245e254bcbabd89..4fe1cbcd4ff0bfca536939d9cf97b77e6bd113d0 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -112,7 +112,7 @@ TEST(Problem, SetOrigin_PO_2D)
     ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
 
     // check that the state is correct
-    ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+    ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
 
     // check that we have one frame, one capture, one feature, one factor
     TrajectoryBasePtr T = P->getTrajectory();
@@ -130,8 +130,8 @@ TEST(Problem, SetOrigin_PO_2D)
     ASSERT_FALSE(c->getLandmarkOther());
 
     // check that the Feature measurement and covariance are the ones provided
-    ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
-    ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+    ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
+    ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
 
     //    P->print(4,1,1,1);
 }
@@ -177,9 +177,9 @@ TEST(Problem, emplaceFrame_factory)
 {
     ProblemPtr P = Problem::create("PO 2D");
 
-    FrameBasePtr f0 = P->emplaceFrame("PO 2D",    ESTIMATED, VectorXs(3),  TimeStamp(0.0));
-    FrameBasePtr f1 = P->emplaceFrame("PO 3D",    ESTIMATED, VectorXs(7),  TimeStamp(1.0));
-    FrameBasePtr f2 = P->emplaceFrame("POV 3D",   ESTIMATED, VectorXs(10), TimeStamp(2.0));
+    FrameBasePtr f0 = P->emplaceFrame("PO 2D",    KEY, VectorXs(3),  TimeStamp(0.0));
+    FrameBasePtr f1 = P->emplaceFrame("PO 3D",    KEY, VectorXs(7),  TimeStamp(1.0));
+    FrameBasePtr f2 = P->emplaceFrame("POV 3D",   KEY, VectorXs(10), TimeStamp(2.0));
 
     //    std::cout << "f0: type PO 2D?    "  << f0->getType() << std::endl;
     //    std::cout << "f1: type PO 3D?    "  << f1->getType() << std::endl;
@@ -223,7 +223,7 @@ TEST(Problem, StateBlocks)
     ProcessorBasePtr pm = P->installProcessor("ODOM 3D",            "odom integrator",      "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
 
     // 2 state blocks, estimated
-    P->emplaceFrame("PO 3D", ESTIMATED, xs, 0);
+    P->emplaceFrame("PO 3D", KEY, xs, 0);
     ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
 
     //    P->print(4,1,1,1);
@@ -256,7 +256,7 @@ TEST(Problem, Covariances)
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
-    FrameBasePtr F = P->emplaceFrame("PO 3D", ESTIMATED, xs, 0);
+    FrameBasePtr F = P->emplaceFrame("PO 3D", KEY, xs, 0);
 
     // set covariance (they are not computed without a solver)
     P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 0a3a0ffacb2f58249dfde5d48b14693fc5de19af..34128f0db287f152a36074fe16b2e61db9a37153 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -59,7 +59,7 @@ TEST(ProcessorBase, KeyFrameCallback)
 
     std::cout << "sensor & processor created and added to wolf problem" << std::endl;
 
-    // Sequence to test Important Frame creations (callback calls)
+    // Sequence to test Key Frame creations (callback calls)
 
     // initialize
     TimeStamp   t(0.0);
@@ -89,7 +89,7 @@ TEST(ProcessorBase, KeyFrameCallback)
 //        problem->print(4,1,1,0);
 
         // Only odom creating KFs
-        ASSERT_TRUE( problem->getLastImportantFrame()->getType().compare("PO 2D")==0 );
+        ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );
     }
 }
 
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 9bb2238ed38ef20e0a9fd51734a3968fdfe81945..75a0cdefbb70da4d75edb2d1e4a7e5d79d13a29c 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -72,19 +72,19 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
   auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>());
 
   // create Keyframes
-  F1 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED,
+  F1 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr1,
                                          stateblock_optr1);
-  F2 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED,
+  F2 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr2,
                                          stateblock_optr2);
-  F3 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED,
+  F3 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr3,
                                          stateblock_optr3);
-  F4 = std::make_shared<wolf::FrameBase>(wolf::ESTIMATED,
+  F4 = std::make_shared<wolf::FrameBase>(wolf::KEY,
                                          1,
                                          stateblock_pptr4,
                                          stateblock_optr4);
@@ -147,7 +147,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
   incomming_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
                                                         1.2,
                                                         sensor_ptr);
-  // Make 3rd frame last Important frame
+  // Make 3rd frame last Key frame
   F3->setTimeStamp(wolf::TimeStamp(2.0));
   problem->getTrajectory()->sortFrame(F3);
 
@@ -181,7 +181,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
   problem->addCovarianceBlock( F4->getP(), F4->getP(),
                                position_covariance_matrix);
 
-  // Make 3rd frame last Important frame
+  // Make 3rd frame last Key frame
   F3->setTimeStamp(wolf::TimeStamp(2.0));
   problem->getTrajectory()->sortFrame(F3);
 
@@ -194,7 +194,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
   ASSERT_EQ(testloops[0]->id(), (unsigned int) 1);
   ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
 
-  // Make 4th frame last Important frame
+  // Make 4th frame last Key frame
   F4->setTimeStamp(wolf::TimeStamp(3.0));
   problem->getTrajectory()->sortFrame(F4);
 
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 962572c3de17c2de464829d48e65f8d12088027d..6792b5d84e8889ea4064cbd4f17e972715b76e17 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -499,7 +499,7 @@ TEST(SolverManager, AddFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -521,7 +521,7 @@ TEST(SolverManager, RemoveFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -549,7 +549,7 @@ TEST(SolverManager, AddRemoveFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
@@ -576,7 +576,7 @@ TEST(SolverManager, DoubleRemoveFactor)
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
     // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceFrame(ESTIMATED, P->zeroState(), TimeStamp(0));
+    FrameBasePtr        F = P->emplaceFrame(KEY, 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)));
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 7b2849760750077590692300c3c13823d1ac46ea..f5dd60c2cad9552167220fc7faf34f27fb53020b 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -60,6 +60,47 @@ struct DummyNotificationProcessor
 /// Set to true if you want debug info
 bool debug = false;
 
+TEST(TrajectoryBase, ClosestKeyFrame)
+{
+
+    ProblemPtr P = Problem::create("PO 2D");
+    TrajectoryBasePtr T = P->getTrajectory();
+
+    // Trajectory status:
+    //  KF1   KF2  AuxF3  F4       frames
+    //   1     2     3     4       time stamps
+    // --+-----+-----+-----+--->   time
+
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,     1, nullptr, nullptr);
+    FrameBasePtr F2 = std::make_shared<FrameBase>(KEY,     2, nullptr, nullptr);
+    FrameBasePtr F3 = std::make_shared<FrameBase>(AUXILIARY,     3, nullptr, nullptr);
+    FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 4, nullptr, nullptr);
+    T->addFrame(F1);
+    T->addFrame(F2);
+    T->addFrame(F3);
+    T->addFrame(F4);
+
+    FrameBasePtr KF; // closest key-frame queried
+
+    KF = T->closestKeyFrameToTimeStamp(-0.8);                // before all keyframes    --> return F1
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(1.1);                 // between keyframes       --> return F1
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(1.9);                 // between keyframes       --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(2.6);                 // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(3.2);                 // after the auxiliary frame, between closer to frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+
+    KF = T->closestKeyFrameToTimeStamp(4.2);                 // after the last frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
+}
+
 TEST(TrajectoryBase, ClosestEstimatedFrame)
 {
 
@@ -67,33 +108,33 @@ TEST(TrajectoryBase, ClosestEstimatedFrame)
     TrajectoryBasePtr T = P->getTrajectory();
 
     // Trajectory status:
-    //  kf1   kf2    f3      frames
+    //  KF1   KF2    F3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED,     1, nullptr, nullptr);
-    FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED,     2, nullptr, nullptr);
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, nullptr, nullptr);
-    T->addFrame(f1);
-    T->addFrame(f2);
-    T->addFrame(f3);
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,           1, nullptr, nullptr);
+    FrameBasePtr F2 = std::make_shared<FrameBase>(AUXILIARY,     2, nullptr, nullptr);
+    FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, nullptr, nullptr);
+    T->addFrame(F1);
+    T->addFrame(F2);
+    T->addFrame(F3);
 
-    FrameBasePtr kf; // closest key-frame queried
+    FrameBasePtr KF; // closest key-frame queried
 
-    kf = T->closestEstimatedFrameToTimeStamp(-0.8);                // before all keyframes    --> return f0
-    ASSERT_EQ(kf->id(), f1->id());                           // same id!
+    KF = T->closestEstimatedFrameToTimeStamp(-0.8);          // before all keyframes    --> return f0
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    kf = T->closestEstimatedFrameToTimeStamp(1.1);                 // between keyframes       --> return f1
-    ASSERT_EQ(kf->id(), f1->id());                           // same id!
+    KF = T->closestEstimatedFrameToTimeStamp(1.1);           // between keyframes       --> return F1
+    ASSERT_EQ(KF->id(), F1->id());                           // same id!
 
-    kf = T->closestEstimatedFrameToTimeStamp(1.9);                 // between keyframes       --> return f2
-    ASSERT_EQ(kf->id(), f2->id());                           // same id!
+    KF = T->closestEstimatedFrameToTimeStamp(1.9);           // between keyframes       --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    kf = T->closestEstimatedFrameToTimeStamp(2.6);                 // between keyframe and frame, but closer to frame --> return f2
-    ASSERT_EQ(kf->id(), f2->id());                           // same id!
+    KF = T->closestEstimatedFrameToTimeStamp(2.6);           // between keyframe and frame, but closer to frame --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
 
-    kf = T->closestEstimatedFrameToTimeStamp(3.2);                 // after the last frame    --> return f2
-    ASSERT_EQ(kf->id(), f2->id());                           // same id!
+    KF = T->closestEstimatedFrameToTimeStamp(3.2);           // after the last frame    --> return F2
+    ASSERT_EQ(KF->id(), F2->id());                           // same id!
 }
 
 TEST(TrajectoryBase, Add_Remove_Frame)
@@ -106,36 +147,36 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     DummyNotificationProcessor N(P);
 
     // Trajectory status:
-    //  kf1   kf2    f3      frames
+    //  KF1   KF2    F3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
-    FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+    FrameBasePtr F2 = std::make_shared<FrameBase>(KEY,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+    FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 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
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 1);
     ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f2); // KF
+    T->addFrame(F2); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 2);
     std::cout << __LINE__ << std::endl;
 
-    T->addFrame(f3); // F (not KF so state blocks are not notified)
+    T->addFrame(F3); // F (not KF so state blocks are not notified)
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 3);
     ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), f3->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
     std::cout << __LINE__ << std::endl;
 
     N.update();
@@ -143,24 +184,24 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 
     // remove frames and keyframes
-    f2->remove(); // KF
+    F2->remove(); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 2);
     ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastFrame()->id(), f3->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f1->id());
+    ASSERT_EQ(T->getLastFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
     std::cout << __LINE__ << std::endl;
 
-    f3->remove(); // F
+    F3->remove(); // F
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 1);
     std::cout << __LINE__ << std::endl;
 
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f1->id());
+    ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
 
-    f1->remove(); // KF
+    F1->remove(); // KF
     if (debug) P->print(2,0,0,0);
     ASSERT_EQ(T->getFrameList().                 size(), (unsigned int) 0);
     std::cout << __LINE__ << std::endl;
@@ -179,70 +220,109 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
     TrajectoryBasePtr T = P->getTrajectory();
 
     // Trajectory status:
-    //  kf1   kf2    f3      frames
+    //  KF1   KF2    F3      frames
     //   1     2     3       time stamps
     // --+-----+-----+--->   time
 
-    FrameBasePtr f1 = std::make_shared<FrameBase>(ESTIMATED,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
-    FrameBasePtr f2 = std::make_shared<FrameBase>(ESTIMATED,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
-    FrameBasePtr f3 = std::make_shared<FrameBase>(NON_ESTIMATED, 3, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    FrameBasePtr F1 = std::make_shared<FrameBase>(KEY,     1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // 2 non-fixed
+    FrameBasePtr F2 = std::make_shared<FrameBase>(KEY,     2, make_shared<StateBlock>(2), make_shared<StateBlock>(1, true)); // 1 fixed, 1 not
+    FrameBasePtr F3 = std::make_shared<FrameBase>(NON_ESTIMATED, 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
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f2->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    T->addFrame(f3); // F3
+    T->addFrame(F3); // F3
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    T->addFrame(f1); // KF1
+    T->addFrame(F1); // KF1
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    f3->setEstimated(); // set KF3
+    F3->setKey(); // set KF3
     if (debug) P->print(2,0,0,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f3->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
-    FrameBasePtr f4 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
-    T->addFrame(f4);
+    FrameBasePtr F4 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    T->addFrame(F4);
     // Trajectory status:
-    //  kf1   kf2   kf3     f4       frames
+    //  KF1   KF2   KF3     F4       frames
     //   1     2     3     1.5       time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f4->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f3->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F4->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
-    f4->setEstimated();
+    F4->setKey();
     // Trajectory status:
-    //  kf1   kf4   kf2    kf3       frames
+    //  KF1   KF4   KF2    KF3       frames
     //   1    1.5    2      3        time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f3->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f3->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F3->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F3->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F3->id());
 
-    f2->setTimeStamp(4);
+    F2->setTimeStamp(4);
     // Trajectory status:
-    //  kf1   kf4   kf3    kf2       frames
+    //  KF1   KF4   KF3    KF2       frames
     //   1    1.5    3      4        time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getLastFrame()   ->id(), f2->id());
-    ASSERT_EQ(T->getLastImportantFrame()->id(), f2->id());
+    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
-    f4->setTimeStamp(0.5);
+    F4->setTimeStamp(0.5);
     // Trajectory status:
-    //  kf4   kf2   kf3    kf2       frames
+    //  KF4   KF2   KF3    KF2       frames
     //  0.5    1     3      4        time stamps
     // --+-----+-----+------+--->    time
     if (debug) P->print(2,0,1,0);
-    ASSERT_EQ(T->getFrameList().front()->id(), f4->id());
+    ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
+
+    FrameBasePtr F5 = std::make_shared<FrameBase>(AUXILIARY, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    T->addFrame(F5);
+    // Trajectory status:
+    //  KF4   KF2  AuxF5  KF3   KF2       frames
+    //  0.5    1    1.5    3     4        time stamps
+    // --+-----+-----+-----+-----+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFrame()         ->id(), F2->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F2->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
+
+    F5->setTimeStamp(5);
+    // Trajectory status:
+    //  KF4   KF2   KF3   KF2   AuxF5     frames
+    //  0.5    1     3     4     5        time stamps
+    // --+-----+-----+-----+-----+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFrame()         ->id(), F5->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
+
+    FrameBasePtr F6 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
+    T->addFrame(F6);
+    // Trajectory status:
+    //  KF4   KF2   KF3   KF2   AuxF5  F6       frames
+    //  0.5    1     3     4     5     6        time stamps
+    // --+-----+-----+-----+-----+-----+--->    time
+    if (debug) P->print(2,0,1,0);
+    ASSERT_EQ(T->getLastFrame()         ->id(), F6->id());
+    ASSERT_EQ(T->getLastEstimatedFrame()->id(), F5->id());
+    ASSERT_EQ(T->getLastKeyFrame()      ->id(), F2->id());
 
 }