Move the normalization of the timestamps in preprocessing (#433)

* Normalize timestamps

* Format

* more explicit

* Simplify generic dataloader

* Fix mcap reader

* Simplify

* Normalizization happen in pipeline

* Forgot that timestamps can be empty

* Fix pipeline

* Do not use C++ 20 features

* Remove warning of unused argument

---------

Co-authored-by: Benedikt Mersch <[email protected]>

Author: tizianoGuadagnino

cpp/kiss_icp/core/Preprocessing.cpp

@@ -59,6 +59,12 @@ std::vector<Eigen::Vector3d> Preprocessor::Preprocess(const std::vector<Eigen::V
         if (!deskew_ || timestamps.empty()) {
             return frame;
         } else {
+            const auto &[min, max] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
+            const double min_time = *min;
+            const double max_time = *max;
+            const auto normalize = [&](const double t) {
+                return (t - min_time) / (max_time - min_time);
+            };
             const auto &omega = relative_motion.log();
             std::vector<Eigen::Vector3d> deskewed_frame(frame.size());
             tbb::parallel_for(
@@ -68,7 +74,7 @@ std::vector<Eigen::Vector3d> Preprocessor::Preprocess(const std::vector<Eigen::V
                 [&](const tbb::blocked_range<size_t> &r) {
                     for (size_t idx = r.begin(); idx < r.end(); ++idx) {
                         const auto &point = frame.at(idx);
-                        const auto &stamp = timestamps.at(idx);
+                        const auto &stamp = normalize(timestamps.at(idx));
                         const auto pose = Sophus::SE3d::exp((stamp - 1.0) * omega);
                         deskewed_frame.at(idx) = pose * point;
                     };

python/kiss_icp/datasets/generic.py

@@ -103,12 +103,7 @@ def __init__(self, time_field):
                     if self.time_field is None:
                         self.get_timestamps = lambda _: np.array([])
                     else:
-                        self.get_timestamps = lambda pcd: self.min_max_normalize(
-                            pcd.point[self.time_field].numpy().ravel()
-                        )
-
-                def min_max_normalize(self, data):
-                    return (data - np.min(data)) / (np.max(data) - np.min(data))
+                        self.get_timestamps = lambda pcd: pcd.point[self.time_field].numpy().ravel()
 
                 def __call__(self, file):
                     pcd = o3d.t.io.read_point_cloud(file)

python/kiss_icp/datasets/mcap.py

@@ -60,7 +60,7 @@ def __del__(self):
     def __getitem__(self, idx):
         msg = next(self.msgs).ros_msg
         self.timestamps.append(self.stamp_to_sec(msg.header.stamp))
-        return self.read_point_cloud(msg), np.array([])
+        return self.read_point_cloud(msg)
 
     def __len__(self):
         return self.n_scans

python/kiss_icp/tools/point_cloud2.py

@@ -82,9 +82,6 @@ def read_point_cloud(msg: PointCloud2) -> Tuple[np.ndarray, np.ndarray]:
 
     if t_field:
         timestamps = points_structured[t_field].astype(np.float64)
-        min_timestamp = np.min(timestamps)
-        max_timestamp = np.max(timestamps)
-        timestamps = (timestamps - min_timestamp) / (max_timestamp - min_timestamp)
     else:
         timestamps = np.array([])
     return points.astype(np.float64), timestamps