Merge pull request #116 from koide3/vel

deal with negative stamps
koide3 · Sep 22, 2024 · e110ca2 · e110ca2
2 parents f679d0b + ad577d4
commit e110ca2
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/src/vlcal/common/time_keeper.cpp b/src/vlcal/common/time_keeper.cpp
@@ -92,6 +92,14 @@ void TimeKeeper::replace_points_stamp(const vlcal::RawPoints::Ptr& points) {
   if (points->times.front() < 0.0 || points->times.back() < 0.0) {
     std::cerr << console::yellow << boost::format("warning: negative per-point timestamp (%.6f or %.6f) found!!") % points->times.front() % points->times.back() << console::reset
               << std::endl;
+
+    const double min_time = *std::min_element(points->times.begin(), points->times.end());
+    std::cerr << console::yellow << boost::format("       : min_stamp=%.6f") % min_time << console::reset
+              << std::endl;
+    for (auto& time : points->times) {
+      time -= min_time;
+    }
+    points->stamp -= min_time;
   }
 
   // Point timestamps are already relative to the first one