FIX: timestamps in insert velodyne obs

MRPT · Jun 23, 2024 · 2146603 · 2146603
1 parent 43f8768
commit 2146603
Showing 1 changed file with 13 additions and 4 deletions.
diff --git a/libs/maps/src/maps/CPointsMapXYZIRT.cpp b/libs/maps/src/maps/CPointsMapXYZIRT.cpp
@@ -436,16 +436,25 @@ bool CPointsMapXYZIRT::internal_insertObservation(
 
     std::vector<double> ts;
     ts.reserve(n);
-    double minT = 0, maxT = 0;
+    std::optional<double> minT, maxT;
     for (size_t i = 0; i < n; i++)
     {
       const double t = mrpt::Clock::toDouble(pc.timestamp[i]);
-      mrpt::keep_min(minT, t);
-      mrpt::keep_max(maxT, t);
+      if (!minT)
+      {
+        minT = t;
+        maxT = t;
+      }
+      else
+      {
+        mrpt::keep_min(*minT, t);
+        mrpt::keep_max(*maxT, t);
+      }
       ts.push_back(t);
     }
+
     // scale timestamps so the center is At=0:
-    const double Tmid = (minT + maxT) * 0.5;
+    const double Tmid = minT ? (*minT + *maxT) * 0.5 : .0;
     for (size_t i = 0; i < n; i++) ts[i] -= Tmid;
 
     for (size_t i = 0; i < n; i++)