actual output

include/time_autosync/cdkf.h

@@ -23,11 +23,11 @@ class CDKF {
     bool verbose = false;
     double mah_threshold = 10.0;
 
-    // inital values
+    // initial values
     double inital_delta_t = 0.05;
     double inital_offset = 0.0;
 
-    // inital noise values
+    // initial noise values
     double inital_timestamp_sd = 0.1;
     double inital_delta_t_sd = 0.1;
     double inital_offset_sd = 0.1;

launch/euroc_dataset.launch

@@ -2,18 +2,36 @@
 
 <param name="/use_sim_time" value="true"/>
 
-<node name="player" pkg="rosbag" type="play" output="screen" args=" -s 35 -r 1.0 --clock /home/z/Datasets/V1_01_easy.bag"/>
-
-<node name="image_undistort_node" pkg="image_undistort" type="image_undistort_node">
-  <param name="input_camera_namespace" value="cam0"/>      
-  <param name="input_camera_info_from_ros_params" value = "true"/>    
-  <rosparam file="$(find viuk)/cfg/euroc.yaml"/>
-  <remap from="input/image" to="/cam0/image_raw"/>
-</node>
+<node name="player" pkg="rosbag" type="play" output="screen" args="--clock /home/z/Datasets/V1_01_easy.bag"/>
 
 <node name="time_autosync" pkg="time_autosync" type="time_autosync_node" >
 
+    <!-- Note: all these parameters are set to these values by default, you only
+     need to set them in your launch files if you want to change them to
+     something else-->
+
     <param name="stamp_on_arrival" value="false"/>
+    <param name="max_imu_data_age_s" value="2.0"/>
+    <param name="delay_by_n_frames" value="5"/>
+    <param name="focal_length" value="460"/>
+    <param name="calc_offset" value="true"/>
+
+    <param name="verbose" value="true"/>
+    <param name="mah_threshold" value="10.0"/>
+
+    <param name="inital_delta_t" value="0.05"/>
+    <param name="inital_offset" value="0.0"/>
+
+    <param name="inital_timestamp_sd" value="0.1"/>
+    <param name="inital_delta_t_sd" value="0.1"/>
+    <param name="inital_offset_sd" value="0.1"/>
+
+    <param name="timestamp_sd" value="0.02"/>
+    <param name="angular_velocity_sd" value="0.03"/>
+
+    <param name="delta_t_sd" value="0.0001"/>
+    <param name="offset_sd" value="0.0001"/>
+
     <remap from="/time_autosync/input/image" to="/cam0/image_raw"/>
     <remap from="/time_autosync/input/imu" to="/imu0"/>
 </node>

src/cdkf.cpp

@@ -60,6 +60,15 @@ void CDKF::getSyncedTimestamp(const ros::Time& received_timestamp,
   }
   *synced_timestamp += ros::Duration(num_frames * delta_t);
   *synced_timestamp += ros::Duration(accessS(state_, OFFSET)[0]);
+
+  if (verbose_) {
+    ROS_INFO_STREAM("Input Timestamp: " << received_timestamp.sec << "."
+                                        << std::setfill('0') << std::setw(9)
+                                        << received_timestamp.nsec);
+    ROS_INFO_STREAM("Output Timestamp: " << synced_timestamp->sec << "."
+                                         << std::setfill('0') << std::setw(9)
+                                         << synced_timestamp->nsec);
+  }
 }
 
 void CDKF::predictionUpdate(const ros::Time& received_timestamp) {

src/time_autosync.cpp

@@ -12,7 +12,7 @@ TimeAutosync::TimeAutosync(const ros::NodeHandle& nh,
       it_(nh_private_),
       stamp_on_arrival_(false),
       max_imu_data_age_s_(2.0),
-      delay_by_n_frames_(3),
+      delay_by_n_frames_(5),
       focal_length_(460.0),
       calc_offset_(true) {
 
@@ -193,11 +193,18 @@ void TimeAutosync::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
     stamp = msg->header.stamp;
   }
 
+  static std::list<cv_bridge::CvImage> images;
   cv_bridge::CvImagePtr image = cv_bridge::toCvCopy(msg, "mono8");
+
+  // fire the image back out with minimal lag
+  if (images.size() >= (delay_by_n_frames_-1)) {
+    cdkf_->getSyncedTimestamp(stamp, &(image->header.stamp));
+    image_pub_.publish(image->toImageMsg());
+  }
+
   image->header.stamp = stamp;
 
   // delay by a few messages to ensure IMU messages span needed range
-  static std::list<cv_bridge::CvImage> images;
   images.push_back(*image);
 
   if (images.size() < delay_by_n_frames_) {