Fix timestamp (#877)

* Precise timestamps are now synced with the unix system time, also added more timestamps to the timestamp_us section of the footer.

Author: MaxBA1602

Backend/includes/data_struct/time_map.hh

@@ -4,28 +4,76 @@
 #include <cstdint>
 #include <vector>
 
+/**
+ * @class FrameTimeMap
+ *
+ * @brief Lock-free ring buffer that stores per-frame timing information.
+ *
+ * This class is used to associate frame IDs (monotonically increasing counters)
+ * with their corresponding timestamps. It maintains three parallel rings:
+ *  - Synced timestamp (in Unix µs): host-synchronized frame time
+ *  - Camera timestamp (in µs since device boot, 0 if unavailable)
+ *  - Offset (in µs): applied offset between host and camera clock domains
+ *
+ * The constructor takes a capacity that must be a power of two; this allows
+ * efficient wrapping using a bit mask instead of modulo arithmetic.
+ *
+ * Typical usage:
+ *  - The producer thread (camera acquisition) calls write_batch() once per
+ *    batch of frames. This writes N consecutive entries starting at base_id.
+ *  - Consumer threads (recorder, processing pipeline) call lookup_*() with
+ *    a frame_id to retrieve the stored timing information.
+ *
+ * Concurrency model:
+ *  - Single producer, multiple readers.
+ *  - Writes and reads use relaxed atomics since there is only one writer,
+ *    and eventual consistency is sufficient for time-stamping.
+ */
 class FrameTimeMap
 {
   public:
     explicit FrameTimeMap(size_t capacity_pow2 = 1 << 20)
         : mask_(capacity_pow2 - 1)
-        , ring_(capacity_pow2)
+        , ring_synced_(capacity_pow2)
+        , ring_cam_(capacity_pow2)
+        , ring_off_(capacity_pow2)
     {
     }
 
-    // Write exact per-frame timestamps for a batch:
-    inline void write_batch(uint64_t base_id, uint64_t ts0_us, uint64_t period_us, unsigned count)
+    inline void write_batch(uint64_t base_id,
+                            uint64_t ts0_synced_us,
+                            uint64_t period_us,
+                            unsigned count,
+                            uint64_t ts0_cam_us,
+                            uint64_t offset_us)
     {
         for (unsigned i = 0; i < count; ++i)
         {
-            ring_[(base_id + i) & mask_].store(ts0_us + uint64_t(i) * period_us, std::memory_order_relaxed);
+            const uint64_t id = (base_id + i) & mask_;
+            const uint64_t synced = ts0_synced_us + uint64_t(i) * period_us;
+            const uint64_t cam = ts0_cam_us ? (ts0_cam_us + uint64_t(i) * period_us) : 0;
+            ring_synced_[id].store(synced, std::memory_order_relaxed);
+            ring_cam_[id].store(cam, std::memory_order_relaxed);
+            ring_off_[id].store(offset_us, std::memory_order_relaxed);
         }
     }
 
-    // Lookup (relaxed is fine; recorder is a consumer)
-    inline uint64_t lookup(uint64_t frame_id) const { return ring_[(frame_id)&mask_].load(std::memory_order_relaxed); }
+    inline uint64_t lookup_synced(uint64_t frame_id) const
+    {
+        return ring_synced_[frame_id & mask_].load(std::memory_order_relaxed);
+    }
+    inline uint64_t lookup_camera(uint64_t frame_id) const
+    {
+        return ring_cam_[frame_id & mask_].load(std::memory_order_relaxed);
+    }
+    inline uint64_t lookup_offset(uint64_t frame_id) const
+    {
+        return ring_off_[frame_id & mask_].load(std::memory_order_relaxed);
+    }
 
   private:
     size_t mask_;
-    std::vector<std::atomic<uint64_t>> ring_;
-};
+    std::vector<std::atomic<uint64_t>> ring_synced_;
+    std::vector<std::atomic<uint64_t>> ring_cam_;
+    std::vector<std::atomic<uint64_t>> ring_off_;
+};

Backend/includes/io/output_file/output_holo_file.hh

@@ -63,11 +63,18 @@ class OutputHoloFile : public OutputFrameFile, public HoloFile
      *
      * \param first_us Timestamp in microseconds of the first frame
      * \param last_us Timestamp in microseconds of the last frame
+     * \param first_camera_us Camera timestamp in microseconds of the first frame
+     * \param last_camera_us Camera timestamp in microseconds of the last frame
+     * \param offset_us Offset in microseconds applied to the camera timestamps to obtain the system timestamps
      */
-    void set_session_timestamps_us(uint64_t first_us, uint64_t last_us)
+    void set_session_timestamps_us(
+        uint64_t first_us, uint64_t last_us, uint64_t first_camera_us, uint64_t last_camera_us, uint64_t offset_us)
     {
         session_first_ts_us_ = first_us;
         session_last_ts_us_ = last_us;
+        session_first_camera_ts_us_ = first_camera_us;
+        session_last_camera_ts_us_ = last_camera_us;
+        session_offset_us_ = offset_us;
         has_session_ts_ = true;
     }
 
@@ -92,5 +99,8 @@ class OutputHoloFile : public OutputFrameFile, public HoloFile
     bool has_session_ts_ = false;
     uint64_t session_first_ts_us_ = 0;
     uint64_t session_last_ts_us_ = 0;
+    uint64_t session_first_camera_ts_us_ = 0;
+    uint64_t session_last_camera_ts_us_ = 0;
+    uint64_t session_offset_us_ = 0;
 };
 } // namespace holovibes::io_files

Backend/sources/io/output_file/output_holo_file.cc

@@ -126,6 +126,7 @@ void OutputHoloFile::export_compute_settings(int input_fps, size_t contiguous)
                                          {"ExposureTime", exposure_time}};
         }
 
+        // Get current date and time for the record timestamp
         auto now = std::chrono::system_clock::now();
         std::time_t now_c = std::chrono::system_clock::to_time_t(now);
         std::tm local_tm = *std::localtime(&now_c);
@@ -134,22 +135,32 @@ void OutputHoloFile::export_compute_settings(int input_fps, size_t contiguous)
         ss << std::put_time(&local_tm, "%Y-%m-%d %H:%M:%S");
         std::string record_timestamp = ss.str();
 
+        // Precise timestamps (us)
         uint64_t first_ts_us = has_session_ts_ ? session_first_ts_us_ : 0;
         uint64_t last_ts_us = has_session_ts_ ? session_last_ts_us_ : 0;
         uint64_t duration_us = (has_session_ts_ && last_ts_us >= first_ts_us) ? (last_ts_us - first_ts_us) : 0;
+        uint64_t first_camera_ts_us = has_session_ts_ ? session_first_camera_ts_us_ : 0;
+        uint64_t last_camera_ts_us = has_session_ts_ ? session_last_camera_ts_us_ : 0;
+        uint64_t offset_us = has_session_ts_ ? session_offset_us_ : 0;
 
         // Build the info JSON without top-level camera_fps
-        auto j_fi = nlohmann::json{
-            {"pixel_pitch", {{"x", api.input.get_pixel_size()}, {"y", api.input.get_pixel_size()}}},
-            {"input_fps", api.input.can_get_camera_fps() ? camera_fps : input_fps},
-            {"camera_fps", camera_fps}, // camera frames per second
-            {"eye_type", api.record.get_recorded_eye()},
-            {"contiguous", contiguous},
-            {"holovibes_version", __HOLOVIBES_VERSION__},
-            {"camera", camera_info},
-            {"file_create_timestamp", file_creation_timestamp_},
-            {"file_record_timestamp", record_timestamp},
-            {"timestamps_us", {{"first", first_ts_us}, {"last", last_ts_us}, {"duration", duration_us}}}};
+        auto j_fi =
+            nlohmann::json{{"pixel_pitch", {{"x", api.input.get_pixel_size()}, {"y", api.input.get_pixel_size()}}},
+                           {"input_fps", api.input.can_get_camera_fps() ? camera_fps : input_fps},
+                           {"camera_fps", camera_fps}, // camera frames per second
+                           {"eye_type", api.record.get_recorded_eye()},
+                           {"contiguous", contiguous},
+                           {"holovibes_version", __HOLOVIBES_VERSION__},
+                           {"camera", camera_info},
+                           {"file_create_timestamp", file_creation_timestamp_},
+                           {"file_record_timestamp", record_timestamp},
+                           {"timestamps_us",
+                            {{"first", first_ts_us},
+                             {"last", last_ts_us},
+                             {"duration", duration_us},
+                             {"camera_first", first_camera_ts_us},
+                             {"camera_last", last_camera_ts_us},
+                             {"offset", offset_us}}}};
 
         meta_data_ = nlohmann::json{{"compute_settings", api.settings.compute_settings_to_json()}, {"info", j_fi}};
     }

Backend/sources/thread/camera_frame_read_worker.cc

@@ -68,8 +68,14 @@ void CameraFrameReadWorker::enqueue_loop(const camera::CapturedFramesDescriptor&
     // assign a contiguous ID range for this batch
     const uint64_t base_id = next_frame_id_.fetch_add(total, std::memory_order_relaxed);
 
-    // fill the time map for RAW 1->1
-    g_time_map.write_batch(base_id, captured_fd.first_frame_timestamp_us, captured_fd.frame_period_us, total);
+    // store timestamps in global time map
+    g_time_map.write_batch(base_id,
+                           captured_fd.first_frame_timestamp_us, // ts0_synced (Unix µs)
+                           captured_fd.frame_period_us,
+                           total,
+                           captured_fd.camera_timestamp_us, // ts0_cam (boot µs) or 0 if not available with this camera
+                           captured_fd.frame_offset_us      // offset applied (µs)
+    );
 
     // enqueue region1 with IDs
     if (captured_fd.count1 > 0)

Backend/sources/thread/frame_record_worker.cc

@@ -221,7 +221,7 @@ void FrameRecordWorker::run()
             if (API.record.get_record_mode() == RecordMode::RAW)
             {
                 this_id = g_record_id_queue.pop_one_blocking();
-                uint64_t this_ts = g_time_map.lookup(this_id);
+                uint64_t this_ts = g_time_map.lookup_synced(this_id);
                 if (!first_id)
                 {
                     first_id = this_id;
@@ -285,20 +285,29 @@ void FrameRecordWorker::run()
 
         if (API.record.get_record_mode() == RecordMode::RAW && first_id.has_value())
         {
-            const uint64_t first_ts_us = g_time_map.lookup(*first_id);
-            const uint64_t last_ts_us = g_time_map.lookup(last_id);
+            const uint64_t first_ts_us = g_time_map.lookup_synced(*first_id);
+            const uint64_t last_ts_us = g_time_map.lookup_synced(last_id);
 
             LOG_INFO("Record timestamps (us): first={} last={}", first_ts_us, last_ts_us);
 
             const uint64_t duration_us = (last_ts_us >= first_ts_us) ? (last_ts_us - first_ts_us) : 0;
 
+            const uint64_t first_camera_ts_us = g_time_map.lookup_camera(*first_id);
+            const uint64_t last_camera_ts_us = g_time_map.lookup_camera(last_id);
+
+            const uint64_t offset_us = g_time_map.lookup_offset(last_id);
+
             LOG_INFO("Record duration: {} us ({} ms, {:.3f} s)",
                      duration_us,
                      duration_us / 1000,
                      static_cast<double>(duration_us) / 1'000'000.0);
             if (auto* holo = dynamic_cast<io_files::OutputHoloFile*>(output_frame_file))
             {
-                holo->set_session_timestamps_us(first_ts_us, last_ts_us);
+                holo->set_session_timestamps_us(first_ts_us,
+                                                last_ts_us,
+                                                first_camera_ts_us,
+                                                last_camera_ts_us,
+                                                offset_us);
             }
         }
 

Camera/CamerasPhantom/camera_phantom_interface.cc

@@ -16,14 +16,23 @@
 
 #include <windows.h>
 
-static inline uint64_t qpc_now_us()
+static inline uint64_t unix_now_us()
 {
-    LARGE_INTEGER qpc, freq;
-    QueryPerformanceCounter(&qpc);
-    QueryPerformanceFrequency(&freq);
-    // Cast to long double to avoid overflow in the division
-    long double us = (long double)qpc.QuadPart * 1e6L / (long double)freq.QuadPart;
-    return (uint64_t)us;
+    FILETIME ft;
+#if defined(NTDDI_WIN8) && (NTDDI_VERSION >= NTDDI_WIN8)
+    GetSystemTimePreciseAsFileTime(&ft);
+#else
+    GetSystemTimeAsFileTime(&ft);
+#endif
+
+    ULARGE_INTEGER uli;
+    uli.LowPart = ft.dwLowDateTime;
+    uli.HighPart = ft.dwHighDateTime;
+
+    // FILETIME is 100-ns ticks since 1601-01-01; convert to Unix epoch µs
+    static constexpr uint64_t EPOCH_DIFF_US = 11644473600000000ULL; // 1601->1970
+    uint64_t us = (uli.QuadPart / 10ULL);                           // 100ns -> µs
+    return us - EPOCH_DIFF_US;
 }
 
 struct SoftClockAlign
@@ -33,19 +42,17 @@ struct SoftClockAlign
 
     void observe(uint64_t t_cam_us, uint64_t t_host_us)
     {
-        // t_host_us >= acquisition_time + latency >= t_cam_us + latency'
-        // Therefore (t_host - t_cam) is an upper bound of the real offset.
+        // Upper bound candidate
         uint64_t cand = (t_host_us > t_cam_us) ? (t_host_us - t_cam_us) : 0;
 
-        // Keep the minimum offset seen so far
+        // Try to update the minimum offset
         uint64_t cur = offset_min_us.load(std::memory_order_relaxed);
         while (cand < cur && !offset_min_us.compare_exchange_weak(cur, cand))
         { /* spin */
         }
 
-        // [DEBUG] Log the current offset estimate
-        // std::cout << "[SoftClockAlign] Candidate offset=" << cand
-        //          << " us, current_min=" << offset_min_us.load(std::memory_order_relaxed) << " us" << std::endl;
+        uint64_t min_off = offset_min_us.load(std::memory_order_relaxed);
+        uint64_t extra_latency = (cand >= min_off) ? (cand - min_off) : 0;
     }
 
     uint64_t to_host_time(uint64_t t_cam_us) const
@@ -289,13 +296,13 @@ void CameraPhantomInt::shutdown_camera() { return; }
 CapturedFramesDescriptor CameraPhantomInt::get_frames()
 {
     // 1) Host timestamp right BEFORE waiting (lower bound)
-    uint64_t host_before_us = qpc_now_us();
+    uint64_t host_before_us = unix_now_us();
 
     // 2) Blocking wait for a buffer
     auto buffer = Euresys::ScopedBuffer(*(grabber_->available_grabbers_[0]));
 
     // 3) Host timestamp right AFTER the buffer (upper bound, better for offset)
-    uint64_t host_after_us = qpc_now_us();
+    uint64_t host_after_us = unix_now_us();
 
     // Multi-grabber support
     unsigned int nb_grabbers = params_.at<unsigned int>("NbGrabbers");
@@ -330,6 +337,9 @@ CapturedFramesDescriptor CameraPhantomInt::get_frames()
     ret.frame_period_us = period_us;
     ret.has_hw_timestamp = (cam_ts_us != 0);
 
+    ret.frame_offset_us = (ret.has_hw_timestamp) ? (synced_ts_us - cam_ts_us) : 0;
+    ret.camera_timestamp_us = cam_ts_us;
+
     return ret;
 }
 

Camera/include/icamera.hh

@@ -64,6 +64,12 @@ struct CapturedFramesDescriptor
     /*! \brief Nominal period between consecutive frames in this batch (us). If unknown, leave 0. */
     uint64_t frame_period_us = 0;
 
+    /*! \brief Frame offset between system timestamps and camera timestamps (us). If unknown, leave 0. */
+    uint64_t frame_offset_us = 0;
+
+    /*! \brief Camera timestamp of the first frame in region1, if available. 0 otherwise. */
+    uint64_t camera_timestamp_us = 0;
+
     /*! \brief True if first_frame_timestamp_us came from the camera/grabber. */
     bool has_hw_timestamp = false;