update save timestamp scripts

Author: jian-dong

examples/plot_imu_and_image_ts.py

@@ -0,0 +1,73 @@
+import pandas as pd
+import matplotlib.pyplot as plt
+
+def plot_data(gyro_csv, accel_csv, rgb_csv, depth_csv, sensor_type):
+    # Load the CSV data into DataFrames
+    gyro_data = pd.read_csv(gyro_csv)
+    accel_data = pd.read_csv(accel_csv)
+    rgb_data = pd.read_csv(rgb_csv)
+    depth_data = pd.read_csv(depth_csv)
+
+    # Set up the plot with six subplots: timestamps, timestamp differences, global timestamps for all sensors
+    fig, ax = plt.subplots(5, 1, figsize=(12, 24), sharex=True)
+
+    # Plot timestamps for gyro and accelerometer
+    ax[0].plot(gyro_data['Index'], gyro_data['Timestamp'], label='Gyro Timestamp', color='blue')
+    ax[0].plot(accel_data['Index'], accel_data['Timestamp'], label='Accelerometer Timestamp', color='orange')
+    ax[0].set_title('IMU Timestamps')
+    ax[0].set_ylabel('Timestamp (us)')
+    ax[0].legend()
+
+    # Plot timestamps for RGB and depth sensors
+    ax[1].plot(rgb_data['Index'], rgb_data['Timestamp'], label='RGB Timestamp', color='red')
+    ax[1].plot(depth_data['Index'], depth_data['Timestamp'], label='Depth Timestamp', color='green')
+    ax[1].set_title('Camera Timestamps')
+    ax[1].set_ylabel('Timestamp (us)')
+    ax[1].legend()
+
+    # Plot timestamp differences for all sensors if the column exists
+    if 'Timestamp Difference' in gyro_data.columns:
+        ax[2].plot(gyro_data['Index'], gyro_data['Timestamp Difference'], label='Gyro Timestamp Difference', color='purple')
+        ax[2].plot(accel_data['Index'], accel_data['Timestamp Difference'], label='Accelerometer Timestamp Difference', color='magenta')
+        ax[2].plot(rgb_data['Index'], rgb_data['Timestamp Difference'], label='RGB Timestamp Difference', color='black')
+        ax[2].plot(depth_data['Index'], depth_data['Timestamp Difference'], label='Depth Timestamp Difference', color='grey')
+        ax[2].set_title('Timestamp Differences')
+        ax[2].set_ylabel('Timestamp Difference (us)')
+        ax[2].legend()
+
+    # Plot global timestamps for all sensors if the column exists
+    if 'Global Timestamp' in gyro_data.columns:
+        ax[3].plot(gyro_data['Index'], gyro_data['Global Timestamp'], label='Gyro Global Timestamp', color='cyan')
+        ax[3].plot(accel_data['Index'], accel_data['Global Timestamp'], label='Accelerometer Global Timestamp', color='yellow')
+        ax[3].plot(rgb_data['Index'], rgb_data['Global Timestamp'], label='RGB Global Timestamp', color='brown')
+        ax[3].plot(depth_data['Index'], depth_data['Global Timestamp'], label='Depth Global Timestamp', color='orange')
+        ax[3].set_title('Global Timestamps')
+        ax[3].set_ylabel('Global Timestamp (us)')
+        ax[3].legend()
+
+    # Plot global timestamp differences for all sensors if the column exists
+    if 'Global Timestamp Difference' in gyro_data.columns:
+        ax[4].plot(gyro_data['Index'], gyro_data['Global Timestamp Difference'], label='Gyro Global Timestamp Difference', color='orange')
+        ax[4].plot(accel_data['Index'], accel_data['Global Timestamp Difference'], label='Accelerometer Global Timestamp Difference', color='brown')
+        ax[4].plot(rgb_data['Index'], rgb_data['Global Timestamp Difference'], label='RGB Global Timestamp Difference', color='purple')
+        ax[4].plot(depth_data['Index'], depth_data['Global Timestamp Difference'], label='Depth Global Timestamp Difference', color='magenta')
+        ax[4].set_title('Global Timestamp Differences')
+        ax[4].set_ylabel('Global Timestamp Difference (us)')
+        ax[4].legend()
+
+    # Adjust layout and show plot
+    plt.tight_layout()
+    plt.show()
+
+def main():
+    # Path to the gyro, accel, RGB, and depth CSV files
+    gyro_csv = 'gyro_data.csv'
+    accel_csv = 'accel_data.csv'
+    rgb_csv = 'color_data.csv'
+    depth_csv = 'depth_data.csv'
+
+    # Plot all sensor data
+    plot_data(gyro_csv, accel_csv, rgb_csv, depth_csv, 'Sensor Data')
+
+if __name__ == "__main__":
+    main()

examples/save_imu.py renamed to examples/save_imu_and_image_ts.py

@@ -35,6 +35,43 @@
 accel_writer = csv.writer(accel_csv_file)
 accel_writer.writerow(['Index', 'Timestamp', 'Global Timestamp','Timestamp Difference', 'Global Timestamp Difference'])
 
+color_csv_file = open('color_data.csv', 'w', newline='')
+color_writer = csv.writer(color_csv_file)
+color_writer.writerow(['Index', 'Timestamp', 'Global Timestamp', 'Timestamp Difference', 'Global Timestamp Difference'])
+
+depth_csv_file = open('depth_data.csv', 'w', newline='')
+depth_writer = csv.writer(depth_csv_file)
+depth_writer.writerow(['Index', 'Timestamp', 'Global Timestamp', 'Timestamp Difference', 'Global Timestamp Difference'])
+last_color_ts = 0
+last_depth_ts = 0
+last_color_global_ts = 0
+last_depth_global_ts = 0
+
+def on_new_frame_set_callback(frame: FrameSet):
+    global last_color_ts, last_depth_ts, last_color_global_ts, last_depth_global_ts
+    if frame is None:
+        return
+    color_frame = frame.get_color_frame()
+    depth_frame = frame.get_depth_frame()
+    if color_frame is not None:
+        timestamp = color_frame.get_timestamp_us()
+        global_timestamp = color_frame.get_global_timestamp_us()
+        index = color_frame.get_index()
+        timestamp_diff = timestamp - last_color_ts if last_color_ts != 0 else 0
+        global_timestamp_diff = global_timestamp - last_color_global_ts if last_color_global_ts != 0 else 0
+        color_writer.writerow([index, timestamp, global_timestamp, timestamp_diff, global_timestamp_diff])
+        last_color_ts = timestamp
+        last_color_global_ts = global_timestamp
+    if depth_frame is not None:
+        timestamp = depth_frame.get_timestamp_us()
+        global_timestamp = depth_frame.get_global_timestamp_us()
+        index = depth_frame.get_index()
+        timestamp_diff = timestamp - last_depth_ts if last_depth_ts != 0 else 0
+        global_timestamp_diff = global_timestamp - last_depth_global_ts if last_depth_global_ts != 0 else 0
+        depth_writer.writerow([index, timestamp, global_timestamp, timestamp_diff, global_timestamp_diff])
+        last_depth_ts = timestamp
+        last_depth_global_ts = global_timestamp
+
 def on_gyro_frame_callback(frame: Frame):
     if frame is None:
         return
@@ -131,24 +168,12 @@ def main():
         print(e)
         return
 
-    pipeline.start(config)
+    pipeline.start(config, lambda frames: on_new_frame_set_callback(frames))
     pipeline.enable_frame_sync()
 
-    last_time = time.time()
     while True:
         try:
-            frames = pipeline.wait_for_frames(100)
-            if not frames:
-                continue
-            color_frame = frames.get_color_frame()
-            depth_frame = frames.get_depth_frame()
-            if not color_frame or not depth_frame:
-                continue
-            if time.time() - last_time > 5.0:
-                print("get color and depth frame")
-                print("color frame timestamp: ", color_frame.get_timestamp())
-                print("depth frame timestamp: ", depth_frame.get_timestamp())
-                last_time = time.time()
+           time.sleep(1.0)
 
         except KeyboardInterrupt:
             # Don't forget to stop the sensor