diff --git a/include/publisher_imu_bias.h b/include/publisher_imu_bias.h
index 5cf9658827d4608db1302246acd09c2e879d3c63..d085dd20591bb79a198581080be1ec80c163da20 100644
--- a/include/publisher_imu_bias.h
+++ b/include/publisher_imu_bias.h
@@ -37,12 +37,12 @@ class PublisherImuBias: public Publisher
 {
         std::string map_frame_id_;
         ros::Publisher pub_accel_, pub_gyro_;
-        SensorImuPtr sensor_imu_;
+        SensorImuConstPtr sensor_imu_;
 
     public:
         PublisherImuBias(const std::string& _unique_name,
-                      const ParamsServer& _server,
-                      const ProblemPtr _problem);
+                         const ParamsServer& _server,
+                         ProblemConstPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherImuBias);
 
         virtual ~PublisherImuBias(){};
diff --git a/src/publisher_imu_bias.cpp b/src/publisher_imu_bias.cpp
index 2feec6ca1dbfbcd66ab78385eba3412b97f4af43..cdfb11afcd648503e11f0a54a005655093524136 100644
--- a/src/publisher_imu_bias.cpp
+++ b/src/publisher_imu_bias.cpp
@@ -29,10 +29,10 @@ namespace wolf
 
 PublisherImuBias::PublisherImuBias(const std::string& _unique_name,
                                    const ParamsServer& _server,
-                                   const ProblemPtr _problem) :
+                                   ProblemConstPtr _problem) :
         Publisher(_unique_name, _server, _problem)
 {
-    sensor_imu_ = std::static_pointer_cast<SensorImu>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
+    sensor_imu_ = std::static_pointer_cast<const SensorImu>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
 }
 
 void PublisherImuBias::initialize(ros::NodeHandle& nh, const std::string& topic)
diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp
index 4daa5895eb0adbb3baf21e94ffac707d85bb1fdc..4861f7216521970acbf7f8283088b143b68a6076 100644
--- a/src/subscriber_imu_enableable.cpp
+++ b/src/subscriber_imu_enableable.cpp
@@ -155,11 +155,11 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                         Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
                         if(sensor_ptr_->getProblem()->getDim() == 3) 
                           zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
-                        for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
-                             frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
-                             frm_pair++)
+                        
+                        auto frame_map = sensor_ptr_->getProblem()->getTrajectory()->getFrameMap();
+                        for (auto frm_pair : frame_map)
                         {
-                            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);
                             auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
@@ -169,14 +169,14 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                             if(sensor_ptr_->getProblem()->getDim() == 3) 
                               FactorBase::emplace<FactorRelativePose3d>(feature_zero,
                                                                         feature_zero,
-                                                                        frm_pair->second,
+                                                                        frm_pair.second,
                                                                         nullptr,
                                                                         false,
                                                                         TOP_MOTION);
                             else
                               FactorBase::emplace<FactorRelativePose2d>(feature_zero,
                                                                         feature_zero,
-                                                                        frm_pair->second,
+                                                                        frm_pair.second,
                                                                         nullptr,
                                                                         false,
                                                                         TOP_MOTION);