better handling of per-point timestamps

config/config_ros.json

@@ -19,12 +19,12 @@
   // base_frame_id          : Base frame ID. If blank, IMU frame ID is used.
   // odom_frame_id          : Odometry frame ID
   // map_frame_id           : Map frame ID
-  // public_imu2lidar       : If false, do not publish "imu -> lidar" TF
+  // publish_imu2lidar      : If false, do not publish "imu -> lidar" TF
   // tf_time_offset         : Time offset for TF data (in seconds)
-  // 
+  //
   // --- Extension modules ---
   // extension_modules      : List of extension modules to load
-  // 
+  //
   // --- Topics ---
   // imu_topic              : IMU topic
   // points_topic           : Points topic
@@ -44,7 +44,7 @@
     "base_frame_id": "",
     "odom_frame_id": "odom",
     "map_frame_id": "map",
-    "public_imu2lidar": true,
+    "publish_imu2lidar": true,
     "tf_time_offset": 1e-6,
     // Extension modules
     "extension_modules": [

config/config_sensors.json

@@ -10,19 +10,25 @@
   // global_shutter_lidar        : If true, fill per-point timestamps with zero and disable points deskewing
   // T_lidar_imu                 : LiDAR-IMU transformation (See below for the format)
   //
+  // --- LiDAR per-point time settings ---
+  // autoconf_perpoint_times     : If true, automatically configure per-point timestamps. If false, use the following settings.
+  // perpoint_relative_time      : If true, recognize per-point timestamps as relative to the frame timestamp. If false, recognize as absolute timestamps.
+  // perpoint_prefer_frame_time  : If true, frame timestamp will never be overridden by anything. Otherwise, absolute per-point timestamps will be used if available.
+  // perpoint_time_scale         : Scale factor for per-point timestamps. If 1.0, timestamps are in seconds. If 1e-9, timestamps are in nanoseconds.
+  //
   // --- Camera config (only used in extension modules) ---
   // global_shutter_camera       : If true, assume global shutter camera
   // image_size                  : Image size [width, height]
   // T_lidar_camera              : LiDAR-camera transformation
   // intrinsics                  : Camera intrinsics
   // distortion_model            : Distortion model
   // distortion_coeffs           : Lens distortion coefficients
-  //  
+  //
   // --- Transformation format ---
   // SE3 poses are written in the TUM format [x, y, z, qx, qy, qz, qw]
   // "T_lidar_imu" transforms points in the "imu" frame into the "lidar" frame
   // e.g., p_lidar = T_lidar_imu * p_imu, T_world_imu = T_world_lidar * T_lidar_imu
-  // 
+  //
   // LiDAR-IMU transformations for typical sensors (Do not trust these values blindly!):
   // Ouster OS0: [0.006, -0.012, 0.008, 0.0, 0.0, 0.0, 1.0]
   // Livox Avia: [0, 0, 0, 0, 0, 0, 1]
@@ -51,6 +57,11 @@
       0.0,
       1.0
     ],
+    // LiDAR per-point time settings
+    "autoconf_perpoint_times": true,
+    "perpoint_relative_time": true,
+    "perpoint_prefer_frame_time": false,
+    "perpoint_time_scale": 1.0,
     // Camera config (required for only extension modules)
     "global_shutter_camera": true,
     "image_size": [

include/glim/util/time_keeper.hpp

@@ -1,30 +1,32 @@
 #pragma once
 
-#include <deque>
+#include <optional>
 #include <glim/util/raw_points.hpp>
 
 namespace glim {
 
-/**
- * @brief Parameter to control the absolute point time handling behavior
- */
-struct AbsPointTimeParams {
+/// @brief Parameters for per-point timestamp management
+struct PerPointTimeSettings {
 public:
-  AbsPointTimeParams() {
-    replace_frame_timestamp = true;
-    wrt_first_frame_timestamp = true;
-  }
-
-  bool replace_frame_timestamp;    ///< If true, replace the frame timestamp with point timestamp
-  bool wrt_first_frame_timestamp;  ///< If true, use the timestamp with respect to the very first points msg
+  PerPointTimeSettings();
+  ~PerPointTimeSettings();
+
+  bool autoconf;       ///< If true, load parameters from config file
+  bool relative_time;  ///< If true, per-point timestamps are relative to the first point. Otherwise, absolute.
+  /// @brief If true, frame timestamp will never be overwritten by antyhing.
+  ///        If false,
+  ///          when per-point timestamps are absolute, overwrite the frame timestamp with the first point timestamp.
+  ///          when per-point timestamps are relative and negative, add an offset to the frame timestamp to make per-point ones positive.
+  bool prefer_frame_time;
+  double point_time_scale;  ///< Scale factor to convert per-point timestamps to seconds.
 };
 
 /**
  * @brief Utility class to unify timestamp convension
  */
 class TimeKeeper {
 public:
-  TimeKeeper(const AbsPointTimeParams& abs_params = AbsPointTimeParams());
+  TimeKeeper();
   ~TimeKeeper();
 
   /**
@@ -45,15 +47,14 @@ class TimeKeeper {
   double estimate_scan_duration(const double stamp);
 
 private:
-  const AbsPointTimeParams abs_params;
+  PerPointTimeSettings settings;
 
-  bool first_warning;        ///< Flag to show warning messages only once
   double last_points_stamp;  ///< Timestamp of the last LiDAR frame
   double last_imu_stamp;     ///< Timestamp of the last IMU data
 
-  int num_scans;                   ///< Number of frames for scan duration estimation
-  double first_points_stamp;       ///< Timestamp of the first frame for scan duration estimation
-  double estimated_scan_duration;  ///< Estimated scan duration
+  // Scan duration estimation
+  double estimated_scan_duration;             ///< Estimated scan duration
+  std::vector<double> scan_duration_history;  ///< History of scan durations
 
   double point_time_offset;  ///< Offset to correct time shift of point times
 };

src/glim/util/time_keeper.cpp

@@ -1,17 +1,33 @@
 #include <glim/util/time_keeper.hpp>
 
+#include <optional>
 #include <spdlog/spdlog.h>
 #include <boost/format.hpp>
+#include <glim/util/config.hpp>
 
 namespace glim {
 
-TimeKeeper::TimeKeeper(const AbsPointTimeParams& abs_params) : abs_params(abs_params) {
-  first_warning = true;
+PerPointTimeSettings::PerPointTimeSettings() {
+  const Config config(GlobalConfig::get_config_path("config_sensors"));
+  autoconf = config.param<bool>("sensors", "autoconf_perpoint_times", true);
+
+  if (autoconf) {
+    relative_time = true;
+    prefer_frame_time = false;
+    point_time_scale = 1.0;
+  } else {
+    relative_time = config.param<bool>("sensors", "perpoint_relative_time", true);
+    prefer_frame_time = config.param<bool>("sensors", "perpoint_prefer_frame_time", false);
+    point_time_scale = config.param<double>("sensors", "perpoint_time_scale", 1.0);
+  }
+}
+
+PerPointTimeSettings::~PerPointTimeSettings() {}
+
+TimeKeeper::TimeKeeper() {
   last_points_stamp = -1.0;
   last_imu_stamp = -1.0;
 
-  num_scans = 0;
-  first_points_stamp = 0.0;
   estimated_scan_duration = -1.0;
   point_time_offset = 0.0;
 }
@@ -44,6 +60,25 @@ bool TimeKeeper::validate_imu_stamp(const double imu_stamp) {
 void TimeKeeper::process(const glim::RawPoints::Ptr& points) {
   replace_points_stamp(points);
 
+  if (points->points.size() != points->times.size()) {
+    // Here must not be reached
+    spdlog::error("inconsistent # of points and # of timestamps found after time conversion!! |points|={} |times|={}", points->points.size(), points->times.size());
+  }
+  if (points->times.front() < 0.0 || points->times.back() < 0.0) {
+    // Here must not be reached
+    spdlog::error("negative per-point timestamp is found after time conversion!! front={:.6f} back={:.6f}", points->times.front(), points->times.back());
+  }
+  if (points->times.front() > 1.0 || points->times.back() > 1.0) {
+    // Here must not be reached
+    spdlog::error("large per-point timestamp is found after time conversion!! front={:.6f} back={:.6f}", points->times.front(), points->times.back());
+  }
+  if (points->stamp < 0.0) {
+    spdlog::warn("frame timestamp is negative!! frame={:.6f}", points->stamp);
+  }
+  if (points->stamp > 3000000000) {
+    spdlog::warn("frame timestamp is wrong (or GLIM has been used for over 40 years)!! frame={:.6f}", points->stamp);
+  }
+
   const double time_diff = points->stamp - last_points_stamp;
   if (last_points_stamp < 0.0) {
     // First LiDAR frame
@@ -60,8 +95,9 @@ void TimeKeeper::process(const glim::RawPoints::Ptr& points) {
 
 void TimeKeeper::replace_points_stamp(const glim::RawPoints::Ptr& points) {
   // No per-point timestamps
-  // Assign timestamps based on scan duration
+  // Assign timestamps based on the estimated scan duration
   if (points->times.empty()) {
+    static bool first_warning = true;
     if (first_warning) {
       spdlog::warn("per-point timestamps are not given!!");
       spdlog::warn("use pseudo per-point timestamps based on the order of points");
@@ -86,82 +122,104 @@ void TimeKeeper::replace_points_stamp(const glim::RawPoints::Ptr& points) {
     return;
   }
 
-  // Check if the per-point timestamps are positive
-  if (points->times.front() < 0.0 || points->times.back() < 0.0) {
-    spdlog::warn("negative per-point timestamp is found!!");
-    spdlog::warn("front={:.6f} back={:.6f}", points->times.front(), points->times.back());
-  }
+  const auto minmax_times = std::minmax_element(points->times.begin(), points->times.end());
+  const double min_time = *minmax_times.first;
+  const double max_time = *minmax_times.second;
 
-  // Point timestamps are already relative to the first one
-  if (points->times.front() < 1.0) {
-    first_warning = false;
-    return;
-  }
+  if (settings.autoconf) {
+    settings.autoconf = false;
 
-  if (first_warning) {
-    spdlog::warn("large point timestamp ({:.6f} > 1.0) found!!", points->times.back());
-    spdlog::warn("assume that point times are absolute and convert them to relative");
-    spdlog::warn("replace_frame_stamp={} wrt_first_frame_timestamp={}", abs_params.replace_frame_timestamp, abs_params.wrt_first_frame_timestamp);
-  }
+    if (min_time < 0.0) {
+      spdlog::warn("negative per-point timestamp is found!! min={:.6f} max={:.6f}", min_time, max_time);
 
-  if (points->times.front() > 1e16) {
-    if (first_warning) {
-      spdlog::warn("too large point timestamp ({:.6f} > 1e16) found!!", points->times.front());
-      spdlog::warn("maybe using a Livox LiDAR that use FLOAT64 nanosec per-point timestamps");
-      spdlog::warn("convert per-point timestamps from nanosec to sec");
+      if (settings.prefer_frame_time) {
+        spdlog::warn("use frame timestamp as is!!");
+      } else {
+        spdlog::warn("add an offset to the frame timestamp to make per-point ones positive!!");
+      }
     }
 
-    for (auto& time : points->times) {
-      time *= 1e-9;
+    if (max_time < 1.0) {
+      settings.relative_time = true;
+    } else {
+      settings.relative_time = false;
+      spdlog::warn("large point timestamp (min={:.6f} max={:.6f} > 1.0) found!!", min_time, max_time);
+      spdlog::warn("assume that point times are absolute and convert them to relative");
+
+      if (min_time > 1e16) {
+        spdlog::warn("too large point timestamp (min={:.6f} max={:.6f} > 1e16) found!!", min_time, max_time);
+        spdlog::warn("maybe using a Livox LiDAR that use FLOAT64 nanosec per-point timestamps");
+        settings.point_time_scale = 1e-9;
+      }
+
+      if (settings.prefer_frame_time) {
+        spdlog::warn("frame timestamp will be prioritized over the first point timestamp!!");
+      } else {
+        spdlog::warn("frame timestamp will be overwritten by the first point timestamp!!");
+      }
     }
   }
 
-  // Convert absolute times to relative times
-  if (abs_params.replace_frame_timestamp) {
-    if (!abs_params.wrt_first_frame_timestamp || std::abs(points->stamp - points->times.front()) < 1.0) {
-      if (first_warning) {
-        spdlog::warn("use first point timestamp as frame timestamp");
-        spdlog::warn("frame={:.6f} point={:.6f}", points->stamp, points->times.front());
+  // Per-point timestamps are relative to the first one
+  if (settings.relative_time) {
+    // Make per-point timestamps positive
+    if (min_time < 0.0) {
+      if (!settings.prefer_frame_time) {
+        // Shift the frame timestamp to keep the consistency
+        points->stamp += min_time * settings.point_time_scale;
       }
 
-      point_time_offset = 0.0;
-      points->stamp = points->times.front();
-    } else {
-      if (first_warning) {
-        spdlog::warn("point timestamp is too apart from frame timestamp!!");
-        spdlog::warn("use time offset w.r.t. the first frame timestamp");
-        spdlog::warn("frame={:.6f} point={:.6f} diff={:.6f}", points->stamp, points->times.front(), points->stamp - points->times.front());
-
-        point_time_offset = points->stamp - points->times.front();
+      for (auto& time : points->times) {
+        time -= min_time;
       }
-
-      points->stamp = points->times.front() + point_time_offset;
     }
 
-    const double first_stamp = points->times.front();
-    for (auto& t : points->times) {
-      t -= first_stamp;
+    if (std::abs(settings.point_time_scale - 1.0) > 1e-6) {
+      // Convert timestamps to seconds
+      for (auto& time : points->times) {
+        time *= settings.point_time_scale;
+      }
     }
+
+    return;
+  }
+
+  // Per-point timestamps are absolute
+
+  if (!settings.prefer_frame_time) {
+    // Overwrite the frame timestamp with the first point timestamp
+    points->stamp = min_time * settings.point_time_scale;
   }
 
-  first_warning = false;
+  // Make per-point timestamps relative to the frame timestamp
+  for (auto& time : points->times) {
+    time = (time - min_time) * settings.point_time_scale;
+  }
 }
 
 double TimeKeeper::estimate_scan_duration(const double stamp) {
   if (estimated_scan_duration > 0.0) {
     return estimated_scan_duration;
   }
 
-  if ((num_scans++) == 0) {
-    first_points_stamp = stamp;
+  if (last_points_stamp < 0) {
     return -1.0;
   }
 
-  const double scan_duration = (stamp - first_points_stamp) / (num_scans - 1);
+  scan_duration_history.emplace_back(stamp - last_points_stamp);
+  std::nth_element(scan_duration_history.begin(), scan_duration_history.begin() + scan_duration_history.size() / 2, scan_duration_history.end());
+  double scan_duration = scan_duration_history[scan_duration_history.size() / 2];
 
-  if (num_scans == 1000) {
+  if (scan_duration_history.size() == 1000) {
     spdlog::info("estimated scan duration: {}", scan_duration);
     estimated_scan_duration = scan_duration;
+    scan_duration_history.clear();
+    scan_duration_history.shrink_to_fit();
+  }
+
+  if (scan_duration < 0.01 || scan_duration > 1.0) {
+    spdlog::warn("invalid scan duration estimate: {}", scan_duration);
+    scan_duration = -1.0;
   }
 
   return scan_duration;