diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 7bbae22c3f2f0598b60c19f1bad5d45e177ab0c0..8674d4db5792df5d90454fe65531342254a77187 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -478,8 +478,8 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
 
 inline Eigen::VectorXd FactorImu::expectation() const
 {
-    FrameBasePtr frm_current = getFeature()->getFrame();
-    FrameBasePtr frm_previous = getFrameOther();
+    auto frm_current = getFeature()->getFrame();
+    auto frm_previous = getFrameOther();
 
     //get information on current_frame in the FactorImu
     const Eigen::Vector3d frame_current_pos    = (frm_current->getP()->getState());
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 12676c83d8536bc8f2ccbcc78eb288dfe74804cd..12ebd1bb39f87ad420005b99b60ff7f11662095c 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -81,7 +81,7 @@ class ProcessorImu : public ProcessorMotion{
         Eigen::VectorXd deltaZero() const override;
         Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
                                              const Eigen::VectorXd& delta_step) const override;
-        VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
+        VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
         void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
         bool voteForKeyFrame() const override;
         CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 433ccf6b92724169420813bdea8692cf5bc6777e..7ccf86a70690b9936a29be8e32b83ec26a56526c 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -81,7 +81,7 @@ class ProcessorImu2d : public ProcessorMotion{
         Eigen::VectorXd deltaZero() const override;
         Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
                                              const Eigen::VectorXd& delta_step) const override;
-        VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+        VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
         void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
         bool voteForKeyFrame() const override;
         CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 69dbf111bc7a5bbcd4ad11796633338c5e877083..ec1db9e4391f0015ad2f6e084110063ba17f5203 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -102,7 +102,7 @@ FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
     return feature;
 }
 
-VectorXd ProcessorImu::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorImu::getCalibration (const CaptureBaseConstPtr _capture) const
 {
     if (_capture)
         return _capture->getStateBlock('I')->getState();
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 036d488e88371d3f4a9628452f1ecd7e7f0e172a..dc35d28c0a43844868505597948fed1315b83af3 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -111,7 +111,7 @@ namespace wolf {
     return feature;
   }
 
-  VectorXd ProcessorImu2d::getCalibration (const CaptureBasePtr _capture) const
+  VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
   {
       if (_capture)
           return _capture->getStateBlock('I')->getState();
@@ -214,7 +214,7 @@ namespace wolf {
      *
      * In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used
      */
-    if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() ) 
+    if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() ) 
         _x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
     else
       _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index ddf1624c4a9c7c9d44c6ce43c35d8f975c8d2494..63d20e2e9b3a9d2b813802aca3f0ab0af187cefe 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -329,11 +329,9 @@ class ProcessorImu2dStaticInitTest : public testing::Test
 
           // add zero displacement and rotation capture & feature & factor with all previous frames
           assert(sensor_ptr_->getProblem());
-          for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
-              frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
-              frm_pair++)
+          for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
           {
-            if (frm_pair->second == last_frame_)
+            if (frm_pair.second == last_frame_)
               break;
             auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
 
@@ -344,7 +342,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
 
             FactorBase::emplace<FactorRelativePose2d>(feature_zero,
                 feature_zero,
-                frm_pair->second,
+                frm_pair.second,
                 nullptr,
                 false,
                 TOP_MOTION);
@@ -443,7 +441,6 @@ class ProcessorImu2dStaticInitTest : public testing::Test
 };
 
 
-
 TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
 {
    Vector3d body_magnitudes = Vector3d::Zero();
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index e0c769b4a30a4d1dc122be4ebe88622634186e1a..e4eb78b29764bb30340448a3235199fb640e51c8 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -339,11 +339,9 @@ class ProcessorImuStaticInitTest : public testing::Test
 
           // add zero displacement and rotation capture & feature & factor with all previous frames
           assert(sensor_ptr_->getProblem());
-          for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
-              frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
-              frm_pair++)
+          for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
           {
-            if (frm_pair->second == last_frame_)
+            if (frm_pair.second == last_frame_)
               break;
             auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
 
@@ -354,11 +352,10 @@ class ProcessorImuStaticInitTest : public testing::Test
 
             FactorBase::emplace<FactorRelativePose3d>(feature_zero,
                 feature_zero,
-                frm_pair->second,
+                frm_pair.second,
                 nullptr,
                 false,
                 TOP_MOTION);
-
           }
 
           // impose static
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index 6255fa38517be1218fcd6e39647c18e2151974c6..9af67f31fd02d661d75c5b15e54947f9cf3f5437 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -196,7 +196,7 @@ TEST(ProcessorImu, voteForKeyFrame)
         - 1 KeyFrame at origin
         - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
     */
-    ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(),(unsigned int) 2);
+    ASSERT_EQ(problem->getTrajectory()->size(),(unsigned int) 2);
 
     /* if max_time_span == 2,  Wolf tree should be
 
diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp
index 87c3619d3fbe37425bfc62bb6c0e2b17cfc40d9c..eabb79523f4952dd885c8432363071aef3dab7a9 100644
--- a/test/gtest_processor_imu2d.cpp
+++ b/test/gtest_processor_imu2d.cpp
@@ -137,7 +137,7 @@ TEST_F(ProcessorImu2dTest, Prior)
     auto KF0 = problem->setPriorFix(x0c, t0);
     processor_motion->setOrigin(KF0);
     //WOLF_DEBUG("x0 =", x0c);
-    //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t)->getState("POV"));
+    //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t0)->getState("POV"));
 }
 
 TEST_F(ProcessorImu2dTest, MRUA)
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 93ba8561b5b4fea55e814cc9d3b68501ae2439ac..0d77493e9c809bb3cc9094584714142777baf6bc 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -221,7 +221,7 @@ TEST_F(ProcessorImuTest, getState)
 #endif
 
 
-        if (problem_->getTrajectory()->getFrameMap().size() > nb_kf){
+        if (problem_->getTrajectory()->size() > nb_kf){
             FrameBasePtr KF = problem_->getTrajectory()->getLastFrame();
             KF->setState(integrateX(AX, KF->getTimeStamp().get())); 
             KF->fix();
@@ -229,7 +229,7 @@ TEST_F(ProcessorImuTest, getState)
             solver_->solve();
             problem_->print(4, true, true, true);
 
-            nb_kf = problem_->getTrajectory()->getFrameMap().size();
+            nb_kf = problem_->getTrajectory()->size();
 
 #if WRITE_CSV_FILE
             // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
@@ -262,22 +262,17 @@ TEST_F(ProcessorImuTest, getState)
      * though we only test at KFs so t = { 0, 1, 2, 3 }
      */
 
-    for (auto F : *problem_->getTrajectory() )
+    for (auto F_pair : problem_->getTrajectory()->getFrameMap() )
     {
-        auto ts = F->getTimeStamp();
+        auto ts = F_pair.second->getTimeStamp();
 
         double bias = (t == 0 ? 0.42 : 0.0);
-        ASSERT_NEAR(F->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
-        ASSERT_NEAR(F->getP()->getState()(0),0.0,1e-6);
-        ASSERT_NEAR(F->getV()->getState()(0),0.0,1e-6);
+        ASSERT_NEAR(F_pair.second->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
+        ASSERT_NEAR(F_pair.second->getP()->getState()(0),0.0,1e-6);
+        ASSERT_NEAR(F_pair.second->getV()->getState()(0),0.0,1e-6);
     }
-
 }
 
-
-
-
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);