Merge pull request #38 from koide3/livox

handling livox point timestamps
koide3 · Jul 31, 2023 · 82df995 · 82df995
2 parents 841f4f3 + 0613dcd
commit 82df995
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/src/vlcal/common/time_keeper.cpp b/src/vlcal/common/time_keeper.cpp
@@ -107,6 +107,18 @@ void TimeKeeper::replace_points_stamp(const vlcal::RawPoints::Ptr& points) {
               << console::reset << std::endl;
   }
 
+  if (points->times.front() > 1e16) {
+    if (first_warning) {
+      std::cerr << console::yellow << boost::format("warning: too large point timestamp (%.6f > 1e16) found!!") % points->times.front() << std::endl;
+      std::cerr << console::yellow << "       : maybe using a Livox LiDAR that use FLOAT64 nanosec per-point timestamps" << std::endl;
+      std::cerr << console::yellow << "       : convert per-point timestamps from nanosec to sec" << std::endl;
+    }
+
+    for (auto& time : points->times) {
+      time *= 1e-9;
+    }
+  }
+
   // 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) {