map-closures 2.0.2__tar.gz → 2.2.0__tar.gz

{map_closures-2.0.2 → map_closures-2.2.0}/CMakeLists.txt

@@ -1,7 +1,6 @@
 # MIT License
 #
-# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
-# Stachniss.
+# Copyright (c) 2026 Saurabh Gupta
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -21,7 +20,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 cmake_minimum_required(VERSION 3.16...3.26)
-project(map_closure_pybind VERSION 2.0.2 LANGUAGES CXX)
+project(map_closure_pybind VERSION 2.2.0 LANGUAGES CXX)
 
 # Set build type
 set(CMAKE_BUILD_TYPE Release)

{map_closures-2.0.2 → map_closures-2.2.0}/LICENSE

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2024 Saurabh Gupta, Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill Stachniss
+Copyright (c) 2026 Saurabh Gupta
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

{map_closures-2.0.2 → map_closures-2.2.0}/PKG-INFO

@@ -1,25 +1,36 @@
 Metadata-Version: 2.2
 Name: map_closures
-Version: 2.0.2
+Version: 2.2.0
 Summary: Effectively Detecting Loop Closures using Point Cloud Density Maps
 Keywords: Loop Closures,Localization,SLAM,LiDAR
 Author: Saurabh Gupta, Tiziano Guadagnino
+License: MIT
+Classifier: Intended Audience :: Developers
+Classifier: Intended Audience :: Education
+Classifier: Intended Audience :: Other Audience
+Classifier: Intended Audience :: Science/Research
+Classifier: Programming Language :: C++
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.10
+Classifier: Programming Language :: Python :: 3.11
+Classifier: Programming Language :: Python :: 3.12
+Classifier: Programming Language :: Python :: 3.13
+Classifier: Programming Language :: Python :: 3.14
 Project-URL: Homepage, https://github.com/PRBonn/MapClosures
-Requires-Python: >=3.8
-Requires-Dist: kiss-icp>=1.2.0
-Requires-Dist: numpy<2.0.0
-Requires-Dist: pyquaternion
-Requires-Dist: pydantic>=2
-Requires-Dist: pydantic-settings
-Requires-Dist: tqdm
-Requires-Dist: typer[all]>=0.6.0
-Requires-Dist: rich
+Requires-Python: >=3.10
+Requires-Dist: kiss-icp<=1.3.0,>=1.2.3
+Requires-Dist: numpy<2.3.0,>=2.1.0
+Requires-Dist: scipy<1.18.0,>=1.14.0
+Requires-Dist: pydantic<2.14,>=2.11
+Requires-Dist: pydantic-settings<2.15,>=2.12
+Requires-Dist: typer[all]<0.26.0,>=0.24.0
 Requires-Dist: typing_extensions
+Requires-Dist: tqdm~=4.67
 Provides-Extra: all
-Requires-Dist: open3d>0.13; extra == "all"
-Requires-Dist: PyYAML; extra == "all"
+Requires-Dist: open3d<0.20,>=0.19; extra == "all"
+Requires-Dist: pyyaml~=6.0; extra == "all"
 Requires-Dist: mcap-ros2-support; extra == "all"
-Requires-Dist: rosbags; extra == "all"
+Requires-Dist: rosbags<0.12,>=0.10; extra == "all"
 Requires-Dist: trimesh; extra == "all"
 Requires-Dist: ouster-sdk; extra == "all"
 Requires-Dist: pyntcloud; extra == "all"
@@ -42,6 +53,8 @@ Description-Content-Type: text/markdown
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
     <a href=https://www.ipb.uni-bonn.de/pdfs/gupta2024icra.pdf>ICRA24 Paper</a>
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
+    <a href=https://www.ipb.uni-bonn.de/pdfs/gupta2026ijrr.pdf>IJRR26 Paper</a>
+    <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
     <a href=https://github.com/PRBonn/MapClosures/issues>Contact Us</a>
   <br />
   <br />
@@ -85,7 +98,7 @@ map_closure_pipeline -v
 
 ## Citation
 
-If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/pdfs/gupta2024icra.pdf).
+If you use this library for any academic work, please cite our original papers.
 
 ```bibtex
 @inproceedings{gupta2024icra,
@@ -95,6 +108,15 @@ If you use this library for any academic work, please cite our original [paper](
     year       = {2024},
     codeurl    = {https://github.com/PRBonn/MapClosures},
 }
+@article{gupta2026ijrr,
+    author     = {Saurabh Gupta and Tiziano Guadagnino and Benedikt Mersch and Niklas Trekel and Meher V. R. Malladi and Cyrill Stachniss},
+    title      = {{Efficiently Closing Loops in LiDAR-Based SLAM Using Point Cloud Density Maps}},
+    booktitle  = {The International Journal of Robotics Research (IJRR)},
+    volume     = {0},
+    number     = {0},
+    year       = {2026},
+    codeurl    = {https://github.com/PRBonn/MapClosures},
+}
 ```
 
 ## Acknowledgement

{map_closures-2.0.2 → map_closures-2.2.0}/README.md

@@ -12,6 +12,8 @@
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
     <a href=https://www.ipb.uni-bonn.de/pdfs/gupta2024icra.pdf>ICRA24 Paper</a>
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
+    <a href=https://www.ipb.uni-bonn.de/pdfs/gupta2026ijrr.pdf>IJRR26 Paper</a>
+    <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
     <a href=https://github.com/PRBonn/MapClosures/issues>Contact Us</a>
   <br />
   <br />
@@ -55,7 +57,7 @@ map_closure_pipeline -v
 
 ## Citation
 
-If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/pdfs/gupta2024icra.pdf).
+If you use this library for any academic work, please cite our original papers.
 
 ```bibtex
 @inproceedings{gupta2024icra,
@@ -65,6 +67,15 @@ If you use this library for any academic work, please cite our original [paper](
     year       = {2024},
     codeurl    = {https://github.com/PRBonn/MapClosures},
 }
+@article{gupta2026ijrr,
+    author     = {Saurabh Gupta and Tiziano Guadagnino and Benedikt Mersch and Niklas Trekel and Meher V. R. Malladi and Cyrill Stachniss},
+    title      = {{Efficiently Closing Loops in LiDAR-Based SLAM Using Point Cloud Density Maps}},
+    booktitle  = {The International Journal of Robotics Research (IJRR)},
+    volume     = {0},
+    number     = {0},
+    year       = {2026},
+    codeurl    = {https://github.com/PRBonn/MapClosures},
+}
 ```
 
 ## Acknowledgement

{map_closures-2.0.2/map_closures/tools → map_closures-2.2.0/map_closures}/__init__.py

@@ -1,7 +1,6 @@
 # MIT License
 #
-# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch,
-# Cyrill Stachniss.
+# Copyright (c) 2026 Saurabh Gupta
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -20,3 +19,5 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
+
+__version__ = "2.2.0"

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/config/__init__.py

@@ -1,7 +1,6 @@
 # MIT License
 #
-# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch,
-# Cyrill Stachniss.
+# Copyright (c) 2026 Saurabh Gupta
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/config/config.py

@@ -1,7 +1,6 @@
 # MIT License
 #
-# Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch,
-# Cyrill Stachniss.
+# Copyright (c) 2026 Saurabh Gupta
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/apollo.py

@@ -49,7 +49,7 @@ class ApolloDataset:
         return len(self.scan_files)
 
     def __getitem__(self, idx):
-        return self.get_scan(self.scan_files[idx])
+        return self.get_scan(self.scan_files[idx]), np.array([])
 
     def get_scan(self, scan_file: str):
         points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64)
@@ -57,13 +57,11 @@ class ApolloDataset:
 
     @staticmethod
     def read_poses(file):
-        from pyquaternion import Quaternion
+        from scipy.spatial.transform import Rotation as R
 
         data = np.loadtxt(file)
         _, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1)
-        rotations = np.array(
-            [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw]
-        )
+        rotations = np.array([R.from_quat(q).as_matrix() for q in qxyzw])
         poses = np.zeros([rotations.shape[0], 4, 4])
         poses[:, :3, -1] = translations
         poses[:, :3, :3] = rotations

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/generic.py

@@ -61,8 +61,8 @@ class GenericDataset:
         return self.read_point_cloud(self.scan_files[idx])
 
     def read_point_cloud(self, file_path: str):
-        points = self._read_point_cloud(file_path)
-        return points.astype(np.float64)
+        points, timestamps = self._read_point_cloud(file_path)
+        return points.astype(np.float64), timestamps.astype(np.float64)
 
     def _get_point_cloud_reader(self):
         """Attempt to guess with try/catch blocks which is the best point cloud reader to use for
@@ -75,34 +75,77 @@ class GenericDataset:
         # This is easy, the old KITTI format
         if self.file_extension == "bin":
             print("[WARNING] Reading .bin files, the only format supported is the KITTI format")
-            return lambda file: np.fromfile(file, dtype=np.float32).reshape((-1, 4))[:, :3]
+
+            class ReadKITTI:
+                def __call__(self, file):
+                    return np.fromfile(file, dtype=np.float32).reshape((-1, 4))[:, :3], np.array([])
+
+            return ReadKITTI()
 
         first_scan_file = self.scan_files[0]
 
-        # first try trimesh
+        # first try open3d
+        try:
+            import open3d as o3d
+
+            try_pcd = o3d.t.io.read_point_cloud(first_scan_file)
+            if try_pcd.is_empty():
+                # open3d binding does not raise an exception if file is unreadable or extension is not supported
+                raise Exception("Generic Dataloader| Open3d PointCloud file is empty")
+
+            stamps_keys = ["t", "timestamp", "timestamps", "time", "stamps"]
+            stamp_field = None
+            for key in stamps_keys:
+                try:
+                    try_pcd.point[key]
+                    stamp_field = key
+                    print("Generic Dataloader| found timestamps")
+                    break
+                except:
+                    continue
+
+            class ReadOpen3d:
+                def __init__(self, time_field):
+                    self.time_field = time_field
+                    if self.time_field is None:
+                        self.get_timestamps = lambda _: np.array([])
+                    else:
+                        self.get_timestamps = lambda pcd: pcd.point[self.time_field].numpy().ravel()
+
+                def __call__(self, file):
+                    pcd = o3d.t.io.read_point_cloud(file)
+                    points = pcd.point.positions.numpy()
+                    return points, self.get_timestamps(pcd)
+
+            return ReadOpen3d(stamp_field)
+        except:
+            pass
+
         try:
             import trimesh
 
             trimesh.load(first_scan_file)
-            return lambda file: np.asarray(trimesh.load(file).vertices)
+
+            class ReadTriMesh:
+                def __call__(self, file):
+                    return np.asarray(trimesh.load(file).vertices), np.array([])
+
+            return ReadTriMesh()
         except:
             pass
 
-        # then try pyntcloud
         try:
             from pyntcloud import PyntCloud
 
             PyntCloud.from_file(first_scan_file)
-            return lambda file: PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy()
-        except:
-            pass
 
-        # lastly with open3d
-        try:
-            import open3d as o3d
+            class ReadPynt:
+                def __call__(self, file):
+                    return PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy(), np.array(
+                        []
+                    )
 
-            o3d.io.read_point_cloud(first_scan_file)
-            return lambda file: np.asarray(o3d.io.read_point_cloud(file).points, dtype=np.float64)
+            return ReadPynt()
         except:
             print("[ERROR], File format not supported")
             sys.exit(1)

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/helipr.py

@@ -34,7 +34,11 @@ class HeLiPRDataset:
         self.sequence_id = sequence
         self.sequence_dir = os.path.realpath(data_dir)
         self.scans_dir = os.path.join(self.sequence_dir, "LiDAR", self.sequence_id)
-        self.scan_files = sorted(glob.glob(self.scans_dir + "/*.bin"))
+        self.bin_format = False
+        self.scan_files = sorted(glob.glob(self.scans_dir + "/*.ply"))
+        if len(self.scan_files) == 0:
+            self.bin_format = True
+            self.scan_files = sorted(glob.glob(self.scans_dir + "/*.bin"))
 
         self.gt_file = os.path.join(
             self.sequence_dir, "LiDAR_GT", f"global_{self.sequence_id}_gt.txt"
@@ -44,29 +48,52 @@ class HeLiPRDataset:
         if len(self.scan_files) == 0:
             raise ValueError(f"Tried to read point cloud files in {data_dir} but none found")
 
-        # Obtain the pointcloud reader for the given data folder
-        if self.sequence_id == "Avia":
-            self.format_string = "fffBBBL"
-            self.intensity_channel = None
-        elif self.sequence_id == "Aeva":
-            self.format_string = "ffffflBf"
-            self.format_string_no_intensity = "ffffflB"
-            self.intensity_channel = 7
-        elif self.sequence_id == "Ouster":
-            self.format_string = "ffffIHHH"
-            self.intensity_channel = 3
-        elif self.sequence_id == "Velodyne":
-            self.format_string = "ffffHf"
-            self.intensity_channel = 3
-        else:
-            print("[ERROR] Unsupported LiDAR Type")
-            sys.exit(1)
+        if self.bin_format:
+            # Obtain the pointcloud reader for the given data folder
+            if self.sequence_id == "Avia":
+                self.format_string = "fffBBBL"
+                self.intensity_channel = None
+                self.time_channel = 6
+            elif self.sequence_id == "Aeva":
+                self.format_string = "ffffflBf"
+                self.format_string_no_intensity = "ffffflB"
+                self.intensity_channel = 7
+                self.time_channel = 5
+            elif self.sequence_id == "Ouster":
+                self.format_string = "ffffIHHH"
+                self.intensity_channel = 3
+                self.time_channel = 4
+            elif self.sequence_id == "Velodyne":
+                self.format_string = "ffffHf"
+                self.intensity_channel = 3
+                self.time_channel = 5
+            else:
+                print("[ERROR] Unsupported LiDAR Type")
+                sys.exit(1)
 
     def __len__(self):
         return len(self.scan_files)
 
     def __getitem__(self, idx):
-        return self.read_point_cloud(idx)
+        if self.bin_format:
+            data = self.get_data(idx)
+            points = self.read_point_cloud(data)
+            timestamps = self.read_timestamps(data)
+            return points, timestamps
+        else:
+            return self.get_ply_data(idx)
+
+    def get_ply_data(self, idx: int):
+        import open3d as o3d
+
+        file_path = self.scan_files[idx]
+        pcd = o3d.t.io.read_point_cloud(file_path)
+        points = pcd.point.positions.numpy()
+        try:
+            timestamps = pcd.point.timestamps.numpy()
+        except KeyError:
+            timestamps = np.array([])
+        return points, timestamps
 
     def get_data(self, idx: int):
         file_path = self.scan_files[idx]
@@ -89,21 +116,20 @@ class HeLiPRDataset:
         data = np.stack(list_lines)
         return data
 
-    def read_point_cloud(self, idx: int):
-        data = self.get_data(idx)
-        points = data[:, :3]
-        return points.astype(np.float64)
+    def read_timestamps(self, data: np.ndarray) -> np.ndarray:
+        time = data[:, self.time_channel]
+        return (time - time.min()) / (time.max() - time.min())
+
+    def read_point_cloud(self, data: np.ndarray) -> np.ndarray:
+        return data[:, :3]
 
     def load_poses(self, poses_file):
-        from pyquaternion import Quaternion
+        from scipy.spatial.transform import Rotation as R
 
         poses = np.loadtxt(poses_file, delimiter=" ")
-        n = poses.shape[0]
 
         xyz = poses[:, 1:4]
-        rotations = np.array(
-            [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in poses[:, 4:]]
-        )
+        rotations = np.array([R.from_quat(q).as_matrix() for q in poses[:, 4:]])
         poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(self.__len__(), axis=0)
         poses[:, :3, :3] = rotations
         poses[:, :3, 3] = xyz

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/kitti.py

@@ -28,7 +28,7 @@ import numpy as np
 
 class KITTIOdometryDataset:
     def __init__(self, data_dir, sequence: str, *_, **__):
-        self.sequence_id = sequence
+        self.sequence_id = str(sequence).zfill(2)
         self.sequence_dir = os.path.join(data_dir, "sequences", self.sequence_id)
         self.velodyne_dir = os.path.join(self.sequence_dir, "velodyne/")
 
@@ -36,7 +36,7 @@ class KITTIOdometryDataset:
         self.calibration = self.read_calib_file(os.path.join(self.sequence_dir, "calib.txt"))
 
         # Load GT Poses (if available)
-        if sequence < 11:
+        if int(sequence) < 11:
             self.poses_fn = os.path.join(data_dir, f"poses/{self.sequence_id}.txt")
             self.gt_poses = self.load_poses(self.poses_fn)
 
@@ -54,7 +54,7 @@ class KITTIOdometryDataset:
         return len(self.scan_files)
 
     def scans(self, idx):
-        return self.read_point_cloud(self.scan_files[idx])
+        return self.read_point_cloud(self.scan_files[idx]), np.array([])
 
     def apply_calibration(self, poses: np.ndarray) -> np.ndarray:
         """Converts from Velodyne to Camera Frame"""

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/mcap.py

@@ -40,14 +40,18 @@ class McapDataloader:
         # we expect `data_dir` param to be a path to the .mcap file, so rename for clarity
         assert os.path.isfile(data_dir), "mcap dataloader expects an existing MCAP file"
         self.sequence_id = os.path.basename(data_dir).split(".")[0]
-        mcap_file = str(data_dir)
+        self.mcap_file = str(data_dir)
 
-        self.bag = make_reader(open(mcap_file, "rb"))
+        self.make_reader = make_reader
+        self.read_ros2_messages = read_ros2_messages
+        self.read_point_cloud = read_point_cloud
+
+        self.bag = self.make_reader(open(self.mcap_file, "rb"))
         self.summary = self.bag.get_summary()
         self.topic = self.check_topic(topic)
         self.n_scans = self._get_n_scans()
-        self.msgs = read_ros2_messages(mcap_file, topics=topic)
-        self.read_point_cloud = read_point_cloud
+        self.msgs = self.read_ros2_messages(self.mcap_file, topics=[self.topic])
+        self.timestamps = []
         self.use_global_visualizer = True
 
     def __del__(self):
@@ -56,6 +60,7 @@ class McapDataloader:
 
     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)
 
     def __len__(self):
@@ -68,6 +73,18 @@ class McapDataloader:
             if self.summary.channels[id].topic == self.topic
         )
 
+    def reset(self):
+        self.timestamps = []
+        self.bag = self.make_reader(open(self.mcap_file, "rb"))
+        self.msgs = self.read_ros2_messages(self.mcap_file, topics=[self.topic])
+
+    @staticmethod
+    def stamp_to_sec(stamp):
+        return stamp.sec + float(stamp.nanosec) / 1e9
+
+    def get_frames_timestamps(self) -> list:
+        return self.timestamps
+
     def check_topic(self, topic: str) -> str:
         # Extract schema id from the .mcap file that encodes the PointCloud2 msg
         schema_id = [

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/mulran.py

@@ -48,7 +48,7 @@ class MulranDataset:
         timestamps = self.get_timestamps()
         if points.shape[0] != timestamps.shape[0]:
             # MuRan has some broken point clouds, just fallback to no timestamps
-            return points.astype(np.float64), np.ones(points.shape[0])
+            return points.astype(np.float64), np.array([])
         return points.astype(np.float64), timestamps
 
     @staticmethod

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/ncd.py

@@ -27,7 +27,7 @@ import sys
 from pathlib import Path
 
 import numpy as np
-from pyquaternion import Quaternion
+from scipy.spatial.transform import Rotation as R
 
 
 class NewerCollegeDataset:
@@ -63,7 +63,7 @@ class NewerCollegeDataset:
         timestamps = self.get_timestamps()
         if points.shape[0] != timestamps.shape[0]:
             # MuRan has some broken point clouds, just fallback to no timestamps
-            return points.astype(np.float64), np.ones(points.shape[0])
+            return points.astype(np.float64), np.array([])
         return points.astype(np.float64), timestamps
 
     @staticmethod
@@ -89,12 +89,7 @@ class NewerCollegeDataset:
         """Taken from pyLiDAR-SLAM/blob/master/slam/dataset/nhcd_dataset.py"""
         ground_truth_df = np.genfromtxt(str(file_path), delimiter=",", dtype=np.float64)
         xyz = ground_truth_df[:, 2:5]
-        rotations = np.array(
-            [
-                Quaternion(x=x, y=y, z=z, w=w).rotation_matrix
-                for x, y, z, w in ground_truth_df[:, 5:]
-            ]
-        )
+        rotations = np.array([R.from_quat(q).as_matrix() for q in ground_truth_df[:, 5:]])
 
         num_poses = rotations.shape[0]
         poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0)
@@ -102,7 +97,7 @@ class NewerCollegeDataset:
         poses[:, :3, 3] = xyz
 
         T_CL = np.eye(4, dtype=np.float32)
-        T_CL[:3, :3] = Quaternion(x=0, y=0, z=0.924, w=0.383).rotation_matrix
+        T_CL[:3, :3] = R.from_quat([0, 0, 0.924, 0.383]).as_matrix()
         T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32)
         poses = np.einsum("nij,jk->nik", poses, T_CL)
         poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses)

{map_closures-2.0.2 → map_closures-2.2.0}/map_closures/datasets/ouster.py

@@ -22,30 +22,12 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 
-import glob
 import os
 from typing import Optional
 
 import numpy as np
 
 
-def find_metadata_json(pcap_file: str) -> str:
-    """Attempts to resolve the metadata json file for a provided pcap file."""
-    dir_path, filename = os.path.split(pcap_file)
-    if not filename:
-        return ""
-    if not dir_path:
-        dir_path = os.getcwd()
-    json_candidates = sorted(glob.glob(f"{dir_path}/*.json"))
-    if not json_candidates:
-        return ""
-    prefix_sizes = list(
-        map(lambda p: len(os.path.commonprefix((filename, os.path.basename(p)))), json_candidates)
-    )
-    max_elem = max(range(len(prefix_sizes)), key=lambda i: prefix_sizes[i])
-    return json_candidates[max_elem]
-
-
 class OusterDataloader:
     """Ouster pcap dataloader"""
 
@@ -83,64 +65,42 @@ class OusterDataloader:
         """
 
         try:
-            import ouster.pcap as pcap
-            from ouster import client
+            from ouster.sdk import client, open_source
         except ImportError:
-            print(
-                f'[ERROR] ouster-sdk is not installed on your system, run "pip install ouster-sdk"'
-            )
+            print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"')
             exit(1)
 
-        # since we import ouster-sdk's client module locally, we keep it locally as well
-        self._client = client
-
         assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file"
 
         # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity
         pcap_file = data_dir
 
-        metadata_json = meta or find_metadata_json(pcap_file)
-        if not metadata_json:
-            print("[ERROR] Ouster pcap dataloader can't find metadata json file.")
-            exit(1)
-        print("[INFO] Ouster pcap dataloader: using metadata json: ", metadata_json)
+        print("Indexing Ouster pcap to count the scans number ...")
+        source = open_source(str(pcap_file), meta=[meta] if meta else [], index=True)
 
-        self.data_dir = os.path.dirname(data_dir)
+        # since we import ouster-sdk's client module locally, we keep reference
+        # to it locally as well
+        self._client = client
 
-        with open(metadata_json) as json:
-            self._info_json = json.read()
-            self._info = client.SensorInfo(self._info_json)
+        self.data_dir = os.path.dirname(data_dir)
 
         # lookup table for 2D range image projection to a 3D point cloud
-        self._xyz_lut = client.XYZLut(self._info)
+        self._xyz_lut = client.XYZLut(source.metadata)
 
         self._pcap_file = str(data_dir)
 
-        # read pcap file for the first pass to count scans
-        print("[INFO] Pre-reading Ouster pcap to count the scans number ...")
-        self._source = pcap.Pcap(self._pcap_file, self._info)
-        self._scans_num = sum((1 for _ in client.Scans(self._source)))
-        print(f"[INFO] Ouster pcap total scans number:  {self._scans_num}")
+        self._scans_num = len(source)
+        print(f"Ouster pcap total scans number:  {self._scans_num}")
 
         # frame timestamps array
         self._timestamps = np.linspace(0, self._scans_num, self._scans_num, endpoint=False)
 
-        # start Scans iterator for consumption in __getitem__
-        self._source = pcap.Pcap(self._pcap_file, self._info)
-        self._scans_iter = iter(client.Scans(self._source))
-        self._next_idx = 0
+        self._source = source
 
     def __getitem__(self, idx):
-        # we assume that users always reads sequentially and do not
-        # pass idx as for a random access collection
-        assert self._next_idx == idx, (
-            "Ouster pcap dataloader supports only sequential reads. "
-            f"Expected idx: {self._next_idx}, but got {idx}"
-        )
-        scan = next(self._scans_iter)
-        self._next_idx += 1
-
-        self._timestamps[self._next_idx - 1] = 1e-9 * scan.timestamp[0]
+        scan = self._source[idx]
+
+        self._timestamps[idx] = 1e-9 * scan.timestamp[0]
 
         timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1))