diff --git a/include/subscriber_camera.h b/include/subscriber_camera.h
index 67896dcb3849c5b8590078dae5798529faea23cf..b2797de768ded98e3ed5a33936006fedd09ba0a8 100644
--- a/include/subscriber_camera.h
+++ b/include/subscriber_camera.h
@@ -59,11 +59,7 @@ namespace wolf
 {
 class SubscriberCamera : public Subscriber
 {
-    bool first_image_received_;
-    bool use_debug_img_;
-    ros::Time t0_cam_,  ts_cam_;
     ros::NodeHandle my_nh_;
-    bool first_time;
     public:
         // Constructor
         SubscriberCamera(const std::string& _unique_name,
@@ -75,7 +71,9 @@ class SubscriberCamera : public Subscriber
 
         void callback(const sensor_msgs::ImageConstPtr& msg);
         void publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg, cv::Mat& frame);
-        image_transport::Publisher publisher_;
+        image_transport::Publisher publisher_debug_img_;
+
+        bool first_time;
 };
 
 WOLF_REGISTER_SUBSCRIBER(SubscriberCamera);
diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp
index ea59397b0a342b03af8ef29d03d3b376e3071d3d..7cf5f7f80898c6d9b2393a598dd0c2fe2aafe5ee 100644
--- a/src/subscriber_camera.cpp
+++ b/src/subscriber_camera.cpp
@@ -29,15 +29,13 @@
 
 namespace wolf
 {
+
 // Constructor
 SubscriberCamera::SubscriberCamera(const std::string& _unique_name,
                                    const ParamsServer& _server,
                                    const SensorBasePtr _sensor_ptr)
     : Subscriber(_unique_name, _server, _sensor_ptr)
 {
-    first_image_received_ = false;
-    use_debug_img_ = true;
-    t0_cam_ = ts_cam_ = ros::Time(0);
     first_time = true;
 }
 
@@ -52,57 +50,62 @@ void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg)
 {
     updateLastHeader(msg->header);
 
-    auto start = std::chrono::high_resolution_clock::now();
+    // auto start = std::chrono::high_resolution_clock::now();
 
-    ts_cam_ = msg->header.stamp;
-    if (!first_image_received_){
-        t0_cam_ = ts_cam_;
-        // Relative timestamps -> first KF at timestamp 0
-        // if put before setPrior, high chance that the first process(imu) occurs before the prior is inititialized, which results in a runtime error
-        first_image_received_ = true;
-        // setPriors((ts_cam_ - t0_cam_).toSec());
-        // start the solver callback timer
-    }
     cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
-    // WOLF_TRACE("CAM (ts_cam_ - t0_cam_).toSec(): ", (ts_cam_ - t0_cam_).toSec())
     auto camera_ptr = std::dynamic_pointer_cast<SensorCamera>(sensor_ptr_);
-    CaptureImagePtr cap = std::make_shared<CaptureImage>((ts_cam_ - t0_cam_).toSec(), camera_ptr, frame);
+    CaptureImagePtr cap = std::make_shared<CaptureImage>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), 
+                                                         camera_ptr, 
+                                                         frame);
     sensor_ptr_->process(cap);
 
     ros::Time t_end = ros::Time::now();
 
-    for(auto prc : sensor_ptr_->getProcessorList())
-    {
-        WOLF_DEBUG("PRC TYPE ", prc->getType());
-        if(prc->getType() == "ProcessorTrackerLandmarkApriltag")
-        {
-            auto april_prc = std::dynamic_pointer_cast<ProcessorTracker>(prc);
-            auto f_list = april_prc->getLast()->getFeatureList();
-            int nb_without_rot = f_list.size();
-            for (auto f_april: f_list){
-                if (std::static_pointer_cast<FeatureApriltag>(f_april)->getUserotation()){
-                    nb_without_rot--;
-                }
-            }
-        }
-    }
-    auto stopbef = std::chrono::high_resolution_clock::now();
-    auto durationbef = std::chrono::duration_cast<std::chrono::microseconds>(stopbef - start);
+    // TBD if usefull or not
+    // for(auto prc : sensor_ptr_->getProcessorList())
+    // {
+    //     WOLF_DEBUG("PRC TYPE ", prc->getType());
+    //     if(prc->getType() == "ProcessorTrackerLandmarkApriltag")
+    //     {
+    //         auto april_prc = std::dynamic_pointer_cast<ProcessorTracker>(prc);
+    //         auto f_list = april_prc->getLast()->getFeatureList();
+    //         int nb_without_rot = f_list.size();
+    //         for (auto f_april: f_list){
+    //             if (std::static_pointer_cast<FeatureApriltag>(f_april)->getUserotation()){
+    //                 nb_without_rot--;
+    //             }
+    //         }
+    //     }
+    // }
+
+    // auto stopbef = std::chrono::high_resolution_clock::now();
+    // auto durationbef = std::chrono::duration_cast<std::chrono::microseconds>(stopbef - start);
     // std::cout << "APRILTAGBEFORE " << durationbef.count() << std::endl;
+
     publishDebugImg(msg, frame);
-    auto stopaft = std::chrono::high_resolution_clock::now();
-    auto durationaft = std::chrono::duration_cast<std::chrono::microseconds>(stopaft - start);
+
+    // auto stopaft = std::chrono::high_resolution_clock::now();
+    // auto durationaft = std::chrono::duration_cast<std::chrono::microseconds>(stopaft - start);
 
 }
+
+
 void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg, cv::Mat& frame){
     ///////////////////////////////
     // Draw tags detected in the last capture
+    ///////////////////////////////
+
+    ////////////////
+    // THIS IS UGLY: their should be a better place to advertise the publisher
+    ////////////////
     if(first_time)
     {
         first_time = false;
         image_transport::ImageTransport it_(my_nh_);
-        publisher_ = it_.advertise("/image_debug", 10);
+        publisher_debug_img_ = it_.advertise("/image_debug", 10);
     }
+    ////////////////
+
     ProcessorTrackerLandmarkApriltagPtr proc_april_;
     for(auto prc : sensor_ptr_->getProcessorList())
     {
@@ -198,6 +201,7 @@ void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg
     out_msg.encoding = sensor_msgs::image_encodings::BGR8;
     out_msg.image    = frame;
 
-    publisher_.publish(out_msg.toImageMsg());
-}
+    publisher_debug_img_.publish(out_msg.toImageMsg());
 }
+
+}  // namespace