map-closures 2.0.2__tar.gz → 2.1.0__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {map_closures-2.0.2 → map_closures-2.1.0}/CMakeLists.txt +2 -3
- {map_closures-2.0.2 → map_closures-2.1.0}/LICENSE +1 -1
- {map_closures-2.0.2 → map_closures-2.1.0}/PKG-INFO +24 -13
- {map_closures-2.0.2/map_closures/tools → map_closures-2.1.0/map_closures}/__init__.py +3 -2
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/config/__init__.py +1 -2
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/config/config.py +1 -2
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/apollo.py +3 -5
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/generic.py +57 -14
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/helipr.py +54 -28
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/kitti.py +3 -3
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/mcap.py +21 -4
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/mulran.py +1 -1
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/ncd.py +4 -9
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/ouster.py +15 -55
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/rosbag.py +23 -11
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/map_closures.py +9 -13
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/pipeline.py +76 -122
- {map_closures-2.0.2/map_closures → map_closures-2.1.0/map_closures/tools}/__init__.py +1 -4
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/tools/cmd.py +3 -5
- map_closures-2.1.0/map_closures/tools/evaluation.py +126 -0
- map_closures-2.1.0/map_closures/tools/utils.py +58 -0
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/visualizer/closures_visualizer.py +1 -2
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/visualizer/local_maps_visualizer.py +1 -2
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/visualizer/registration_visualizer.py +1 -2
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/visualizer/visualizer.py +1 -2
- map_closures-2.1.0/map_closures/voxel_map.py +54 -0
- {map_closures-2.0.2 → map_closures-2.1.0}/pyproject.toml +31 -16
- map_closures-2.0.2/map_closures/tools/evaluation.py +0 -199
- {map_closures-2.0.2 → map_closures-2.1.0}/MANIFEST.in +0 -0
- {map_closures-2.0.2 → map_closures-2.1.0}/README.md +0 -0
- {map_closures-2.0.2 → map_closures-2.1.0}/map_closures/datasets/__init__.py +0 -0
|
@@ -1,7 +1,6 @@
|
|
|
1
1
|
# MIT License
|
|
2
2
|
#
|
|
3
|
-
# Copyright (c)
|
|
4
|
-
# Stachniss.
|
|
3
|
+
# Copyright (c) 2026 Saurabh Gupta
|
|
5
4
|
#
|
|
6
5
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
7
6
|
# of this software and associated documentation files (the "Software"), to deal
|
|
@@ -21,7 +20,7 @@
|
|
|
21
20
|
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
22
21
|
# SOFTWARE.
|
|
23
22
|
cmake_minimum_required(VERSION 3.16...3.26)
|
|
24
|
-
project(map_closure_pybind VERSION 2.0
|
|
23
|
+
project(map_closure_pybind VERSION 2.1.0 LANGUAGES CXX)
|
|
25
24
|
|
|
26
25
|
# Set build type
|
|
27
26
|
set(CMAKE_BUILD_TYPE Release)
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
MIT License
|
|
2
2
|
|
|
3
|
-
Copyright (c)
|
|
3
|
+
Copyright (c) 2026 Saurabh Gupta
|
|
4
4
|
|
|
5
5
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
6
6
|
of this software and associated documentation files (the "Software"), to deal
|
|
@@ -1,25 +1,36 @@
|
|
|
1
1
|
Metadata-Version: 2.2
|
|
2
2
|
Name: map_closures
|
|
3
|
-
Version: 2.0
|
|
3
|
+
Version: 2.1.0
|
|
4
4
|
Summary: Effectively Detecting Loop Closures using Point Cloud Density Maps
|
|
5
5
|
Keywords: Loop Closures,Localization,SLAM,LiDAR
|
|
6
6
|
Author: Saurabh Gupta, Tiziano Guadagnino
|
|
7
|
+
License: MIT
|
|
8
|
+
Classifier: Intended Audience :: Developers
|
|
9
|
+
Classifier: Intended Audience :: Education
|
|
10
|
+
Classifier: Intended Audience :: Other Audience
|
|
11
|
+
Classifier: Intended Audience :: Science/Research
|
|
12
|
+
Classifier: Programming Language :: C++
|
|
13
|
+
Classifier: Programming Language :: Python :: 3
|
|
14
|
+
Classifier: Programming Language :: Python :: 3.10
|
|
15
|
+
Classifier: Programming Language :: Python :: 3.11
|
|
16
|
+
Classifier: Programming Language :: Python :: 3.12
|
|
17
|
+
Classifier: Programming Language :: Python :: 3.13
|
|
18
|
+
Classifier: Programming Language :: Python :: 3.14
|
|
7
19
|
Project-URL: Homepage, https://github.com/PRBonn/MapClosures
|
|
8
|
-
Requires-Python: >=3.
|
|
9
|
-
Requires-Dist: kiss-icp
|
|
10
|
-
Requires-Dist: numpy<2.0.0
|
|
11
|
-
Requires-Dist:
|
|
12
|
-
Requires-Dist: pydantic
|
|
13
|
-
Requires-Dist: pydantic-settings
|
|
14
|
-
Requires-Dist:
|
|
15
|
-
Requires-Dist: typer[all]>=0.6.0
|
|
16
|
-
Requires-Dist: rich
|
|
20
|
+
Requires-Python: >=3.10
|
|
21
|
+
Requires-Dist: kiss-icp<=1.3.0,>=1.2.3
|
|
22
|
+
Requires-Dist: numpy<2.3.0,>=2.1.0
|
|
23
|
+
Requires-Dist: scipy<1.18.0,>=1.14.0
|
|
24
|
+
Requires-Dist: pydantic<2.14,>=2.11
|
|
25
|
+
Requires-Dist: pydantic-settings<2.15,>=2.12
|
|
26
|
+
Requires-Dist: typer[all]<0.26.0,>=0.24.0
|
|
17
27
|
Requires-Dist: typing_extensions
|
|
28
|
+
Requires-Dist: tqdm~=4.67
|
|
18
29
|
Provides-Extra: all
|
|
19
|
-
Requires-Dist: open3d
|
|
20
|
-
Requires-Dist:
|
|
30
|
+
Requires-Dist: open3d<0.20,>=0.19; extra == "all"
|
|
31
|
+
Requires-Dist: pyyaml~=6.0; extra == "all"
|
|
21
32
|
Requires-Dist: mcap-ros2-support; extra == "all"
|
|
22
|
-
Requires-Dist: rosbags; extra == "all"
|
|
33
|
+
Requires-Dist: rosbags<0.12,>=0.10; extra == "all"
|
|
23
34
|
Requires-Dist: trimesh; extra == "all"
|
|
24
35
|
Requires-Dist: ouster-sdk; extra == "all"
|
|
25
36
|
Requires-Dist: pyntcloud; extra == "all"
|
|
@@ -1,7 +1,6 @@
|
|
|
1
1
|
# MIT License
|
|
2
2
|
#
|
|
3
|
-
# Copyright (c)
|
|
4
|
-
# Cyrill Stachniss.
|
|
3
|
+
# Copyright (c) 2026 Saurabh Gupta
|
|
5
4
|
#
|
|
6
5
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
7
6
|
# of this software and associated documentation files (the "Software"), to deal
|
|
@@ -20,3 +19,5 @@
|
|
|
20
19
|
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
21
20
|
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
22
21
|
# SOFTWARE.
|
|
22
|
+
|
|
23
|
+
__version__ = "2.1.0"
|
|
@@ -1,7 +1,6 @@
|
|
|
1
1
|
# MIT License
|
|
2
2
|
#
|
|
3
|
-
# Copyright (c)
|
|
4
|
-
# Cyrill Stachniss.
|
|
3
|
+
# Copyright (c) 2026 Saurabh Gupta
|
|
5
4
|
#
|
|
6
5
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
7
6
|
# of this software and associated documentation files (the "Software"), to deal
|
|
@@ -1,7 +1,6 @@
|
|
|
1
1
|
# MIT License
|
|
2
2
|
#
|
|
3
|
-
# Copyright (c)
|
|
4
|
-
# Cyrill Stachniss.
|
|
3
|
+
# Copyright (c) 2026 Saurabh Gupta
|
|
5
4
|
#
|
|
6
5
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
7
6
|
# of this software and associated documentation files (the "Software"), to deal
|
|
@@ -49,7 +49,7 @@ class ApolloDataset:
|
|
|
49
49
|
return len(self.scan_files)
|
|
50
50
|
|
|
51
51
|
def __getitem__(self, idx):
|
|
52
|
-
return self.get_scan(self.scan_files[idx])
|
|
52
|
+
return self.get_scan(self.scan_files[idx]), np.array([])
|
|
53
53
|
|
|
54
54
|
def get_scan(self, scan_file: str):
|
|
55
55
|
points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64)
|
|
@@ -57,13 +57,11 @@ class ApolloDataset:
|
|
|
57
57
|
|
|
58
58
|
@staticmethod
|
|
59
59
|
def read_poses(file):
|
|
60
|
-
from
|
|
60
|
+
from scipy.spatial.transform import Rotation as R
|
|
61
61
|
|
|
62
62
|
data = np.loadtxt(file)
|
|
63
63
|
_, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1)
|
|
64
|
-
rotations = np.array(
|
|
65
|
-
[Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw]
|
|
66
|
-
)
|
|
64
|
+
rotations = np.array([R.from_quat(q).as_matrix() for q in qxyzw])
|
|
67
65
|
poses = np.zeros([rotations.shape[0], 4, 4])
|
|
68
66
|
poses[:, :3, -1] = translations
|
|
69
67
|
poses[:, :3, :3] = rotations
|
|
@@ -61,8 +61,8 @@ class GenericDataset:
|
|
|
61
61
|
return self.read_point_cloud(self.scan_files[idx])
|
|
62
62
|
|
|
63
63
|
def read_point_cloud(self, file_path: str):
|
|
64
|
-
points = self._read_point_cloud(file_path)
|
|
65
|
-
return points.astype(np.float64)
|
|
64
|
+
points, timestamps = self._read_point_cloud(file_path)
|
|
65
|
+
return points.astype(np.float64), timestamps.astype(np.float64)
|
|
66
66
|
|
|
67
67
|
def _get_point_cloud_reader(self):
|
|
68
68
|
"""Attempt to guess with try/catch blocks which is the best point cloud reader to use for
|
|
@@ -75,34 +75,77 @@ class GenericDataset:
|
|
|
75
75
|
# This is easy, the old KITTI format
|
|
76
76
|
if self.file_extension == "bin":
|
|
77
77
|
print("[WARNING] Reading .bin files, the only format supported is the KITTI format")
|
|
78
|
-
|
|
78
|
+
|
|
79
|
+
class ReadKITTI:
|
|
80
|
+
def __call__(self, file):
|
|
81
|
+
return np.fromfile(file, dtype=np.float32).reshape((-1, 4))[:, :3], np.array([])
|
|
82
|
+
|
|
83
|
+
return ReadKITTI()
|
|
79
84
|
|
|
80
85
|
first_scan_file = self.scan_files[0]
|
|
81
86
|
|
|
82
|
-
# first try
|
|
87
|
+
# first try open3d
|
|
88
|
+
try:
|
|
89
|
+
import open3d as o3d
|
|
90
|
+
|
|
91
|
+
try_pcd = o3d.t.io.read_point_cloud(first_scan_file)
|
|
92
|
+
if try_pcd.is_empty():
|
|
93
|
+
# open3d binding does not raise an exception if file is unreadable or extension is not supported
|
|
94
|
+
raise Exception("Generic Dataloader| Open3d PointCloud file is empty")
|
|
95
|
+
|
|
96
|
+
stamps_keys = ["t", "timestamp", "timestamps", "time", "stamps"]
|
|
97
|
+
stamp_field = None
|
|
98
|
+
for key in stamps_keys:
|
|
99
|
+
try:
|
|
100
|
+
try_pcd.point[key]
|
|
101
|
+
stamp_field = key
|
|
102
|
+
print("Generic Dataloader| found timestamps")
|
|
103
|
+
break
|
|
104
|
+
except:
|
|
105
|
+
continue
|
|
106
|
+
|
|
107
|
+
class ReadOpen3d:
|
|
108
|
+
def __init__(self, time_field):
|
|
109
|
+
self.time_field = time_field
|
|
110
|
+
if self.time_field is None:
|
|
111
|
+
self.get_timestamps = lambda _: np.array([])
|
|
112
|
+
else:
|
|
113
|
+
self.get_timestamps = lambda pcd: pcd.point[self.time_field].numpy().ravel()
|
|
114
|
+
|
|
115
|
+
def __call__(self, file):
|
|
116
|
+
pcd = o3d.t.io.read_point_cloud(file)
|
|
117
|
+
points = pcd.point.positions.numpy()
|
|
118
|
+
return points, self.get_timestamps(pcd)
|
|
119
|
+
|
|
120
|
+
return ReadOpen3d(stamp_field)
|
|
121
|
+
except:
|
|
122
|
+
pass
|
|
123
|
+
|
|
83
124
|
try:
|
|
84
125
|
import trimesh
|
|
85
126
|
|
|
86
127
|
trimesh.load(first_scan_file)
|
|
87
|
-
|
|
128
|
+
|
|
129
|
+
class ReadTriMesh:
|
|
130
|
+
def __call__(self, file):
|
|
131
|
+
return np.asarray(trimesh.load(file).vertices), np.array([])
|
|
132
|
+
|
|
133
|
+
return ReadTriMesh()
|
|
88
134
|
except:
|
|
89
135
|
pass
|
|
90
136
|
|
|
91
|
-
# then try pyntcloud
|
|
92
137
|
try:
|
|
93
138
|
from pyntcloud import PyntCloud
|
|
94
139
|
|
|
95
140
|
PyntCloud.from_file(first_scan_file)
|
|
96
|
-
return lambda file: PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy()
|
|
97
|
-
except:
|
|
98
|
-
pass
|
|
99
141
|
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
142
|
+
class ReadPynt:
|
|
143
|
+
def __call__(self, file):
|
|
144
|
+
return PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy(), np.array(
|
|
145
|
+
[]
|
|
146
|
+
)
|
|
103
147
|
|
|
104
|
-
|
|
105
|
-
return lambda file: np.asarray(o3d.io.read_point_cloud(file).points, dtype=np.float64)
|
|
148
|
+
return ReadPynt()
|
|
106
149
|
except:
|
|
107
150
|
print("[ERROR], File format not supported")
|
|
108
151
|
sys.exit(1)
|
|
@@ -34,7 +34,11 @@ class HeLiPRDataset:
|
|
|
34
34
|
self.sequence_id = sequence
|
|
35
35
|
self.sequence_dir = os.path.realpath(data_dir)
|
|
36
36
|
self.scans_dir = os.path.join(self.sequence_dir, "LiDAR", self.sequence_id)
|
|
37
|
-
self.
|
|
37
|
+
self.bin_format = False
|
|
38
|
+
self.scan_files = sorted(glob.glob(self.scans_dir + "/*.ply"))
|
|
39
|
+
if len(self.scan_files) == 0:
|
|
40
|
+
self.bin_format = True
|
|
41
|
+
self.scan_files = sorted(glob.glob(self.scans_dir + "/*.bin"))
|
|
38
42
|
|
|
39
43
|
self.gt_file = os.path.join(
|
|
40
44
|
self.sequence_dir, "LiDAR_GT", f"global_{self.sequence_id}_gt.txt"
|
|
@@ -44,29 +48,52 @@ class HeLiPRDataset:
|
|
|
44
48
|
if len(self.scan_files) == 0:
|
|
45
49
|
raise ValueError(f"Tried to read point cloud files in {data_dir} but none found")
|
|
46
50
|
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
self.
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
self.
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
51
|
+
if self.bin_format:
|
|
52
|
+
# Obtain the pointcloud reader for the given data folder
|
|
53
|
+
if self.sequence_id == "Avia":
|
|
54
|
+
self.format_string = "fffBBBL"
|
|
55
|
+
self.intensity_channel = None
|
|
56
|
+
self.time_channel = 6
|
|
57
|
+
elif self.sequence_id == "Aeva":
|
|
58
|
+
self.format_string = "ffffflBf"
|
|
59
|
+
self.format_string_no_intensity = "ffffflB"
|
|
60
|
+
self.intensity_channel = 7
|
|
61
|
+
self.time_channel = 5
|
|
62
|
+
elif self.sequence_id == "Ouster":
|
|
63
|
+
self.format_string = "ffffIHHH"
|
|
64
|
+
self.intensity_channel = 3
|
|
65
|
+
self.time_channel = 4
|
|
66
|
+
elif self.sequence_id == "Velodyne":
|
|
67
|
+
self.format_string = "ffffHf"
|
|
68
|
+
self.intensity_channel = 3
|
|
69
|
+
self.time_channel = 5
|
|
70
|
+
else:
|
|
71
|
+
print("[ERROR] Unsupported LiDAR Type")
|
|
72
|
+
sys.exit(1)
|
|
64
73
|
|
|
65
74
|
def __len__(self):
|
|
66
75
|
return len(self.scan_files)
|
|
67
76
|
|
|
68
77
|
def __getitem__(self, idx):
|
|
69
|
-
|
|
78
|
+
if self.bin_format:
|
|
79
|
+
data = self.get_data(idx)
|
|
80
|
+
points = self.read_point_cloud(data)
|
|
81
|
+
timestamps = self.read_timestamps(data)
|
|
82
|
+
return points, timestamps
|
|
83
|
+
else:
|
|
84
|
+
return self.get_ply_data(idx)
|
|
85
|
+
|
|
86
|
+
def get_ply_data(self, idx: int):
|
|
87
|
+
import open3d as o3d
|
|
88
|
+
|
|
89
|
+
file_path = self.scan_files[idx]
|
|
90
|
+
pcd = o3d.t.io.read_point_cloud(file_path)
|
|
91
|
+
points = pcd.point.positions.numpy()
|
|
92
|
+
try:
|
|
93
|
+
timestamps = pcd.point.timestamps.numpy()
|
|
94
|
+
except KeyError:
|
|
95
|
+
timestamps = np.array([])
|
|
96
|
+
return points, timestamps
|
|
70
97
|
|
|
71
98
|
def get_data(self, idx: int):
|
|
72
99
|
file_path = self.scan_files[idx]
|
|
@@ -89,21 +116,20 @@ class HeLiPRDataset:
|
|
|
89
116
|
data = np.stack(list_lines)
|
|
90
117
|
return data
|
|
91
118
|
|
|
92
|
-
def
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
119
|
+
def read_timestamps(self, data: np.ndarray) -> np.ndarray:
|
|
120
|
+
time = data[:, self.time_channel]
|
|
121
|
+
return (time - time.min()) / (time.max() - time.min())
|
|
122
|
+
|
|
123
|
+
def read_point_cloud(self, data: np.ndarray) -> np.ndarray:
|
|
124
|
+
return data[:, :3]
|
|
96
125
|
|
|
97
126
|
def load_poses(self, poses_file):
|
|
98
|
-
from
|
|
127
|
+
from scipy.spatial.transform import Rotation as R
|
|
99
128
|
|
|
100
129
|
poses = np.loadtxt(poses_file, delimiter=" ")
|
|
101
|
-
n = poses.shape[0]
|
|
102
130
|
|
|
103
131
|
xyz = poses[:, 1:4]
|
|
104
|
-
rotations = np.array(
|
|
105
|
-
[Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in poses[:, 4:]]
|
|
106
|
-
)
|
|
132
|
+
rotations = np.array([R.from_quat(q).as_matrix() for q in poses[:, 4:]])
|
|
107
133
|
poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(self.__len__(), axis=0)
|
|
108
134
|
poses[:, :3, :3] = rotations
|
|
109
135
|
poses[:, :3, 3] = xyz
|
|
@@ -28,7 +28,7 @@ import numpy as np
|
|
|
28
28
|
|
|
29
29
|
class KITTIOdometryDataset:
|
|
30
30
|
def __init__(self, data_dir, sequence: str, *_, **__):
|
|
31
|
-
self.sequence_id = sequence
|
|
31
|
+
self.sequence_id = str(sequence).zfill(2)
|
|
32
32
|
self.sequence_dir = os.path.join(data_dir, "sequences", self.sequence_id)
|
|
33
33
|
self.velodyne_dir = os.path.join(self.sequence_dir, "velodyne/")
|
|
34
34
|
|
|
@@ -36,7 +36,7 @@ class KITTIOdometryDataset:
|
|
|
36
36
|
self.calibration = self.read_calib_file(os.path.join(self.sequence_dir, "calib.txt"))
|
|
37
37
|
|
|
38
38
|
# Load GT Poses (if available)
|
|
39
|
-
if sequence < 11:
|
|
39
|
+
if int(sequence) < 11:
|
|
40
40
|
self.poses_fn = os.path.join(data_dir, f"poses/{self.sequence_id}.txt")
|
|
41
41
|
self.gt_poses = self.load_poses(self.poses_fn)
|
|
42
42
|
|
|
@@ -54,7 +54,7 @@ class KITTIOdometryDataset:
|
|
|
54
54
|
return len(self.scan_files)
|
|
55
55
|
|
|
56
56
|
def scans(self, idx):
|
|
57
|
-
return self.read_point_cloud(self.scan_files[idx])
|
|
57
|
+
return self.read_point_cloud(self.scan_files[idx]), np.array([])
|
|
58
58
|
|
|
59
59
|
def apply_calibration(self, poses: np.ndarray) -> np.ndarray:
|
|
60
60
|
"""Converts from Velodyne to Camera Frame"""
|
|
@@ -40,14 +40,18 @@ class McapDataloader:
|
|
|
40
40
|
# we expect `data_dir` param to be a path to the .mcap file, so rename for clarity
|
|
41
41
|
assert os.path.isfile(data_dir), "mcap dataloader expects an existing MCAP file"
|
|
42
42
|
self.sequence_id = os.path.basename(data_dir).split(".")[0]
|
|
43
|
-
mcap_file = str(data_dir)
|
|
43
|
+
self.mcap_file = str(data_dir)
|
|
44
44
|
|
|
45
|
-
self.
|
|
45
|
+
self.make_reader = make_reader
|
|
46
|
+
self.read_ros2_messages = read_ros2_messages
|
|
47
|
+
self.read_point_cloud = read_point_cloud
|
|
48
|
+
|
|
49
|
+
self.bag = self.make_reader(open(self.mcap_file, "rb"))
|
|
46
50
|
self.summary = self.bag.get_summary()
|
|
47
51
|
self.topic = self.check_topic(topic)
|
|
48
52
|
self.n_scans = self._get_n_scans()
|
|
49
|
-
self.msgs = read_ros2_messages(mcap_file, topics=topic)
|
|
50
|
-
self.
|
|
53
|
+
self.msgs = self.read_ros2_messages(self.mcap_file, topics=[self.topic])
|
|
54
|
+
self.timestamps = []
|
|
51
55
|
self.use_global_visualizer = True
|
|
52
56
|
|
|
53
57
|
def __del__(self):
|
|
@@ -56,6 +60,7 @@ class McapDataloader:
|
|
|
56
60
|
|
|
57
61
|
def __getitem__(self, idx):
|
|
58
62
|
msg = next(self.msgs).ros_msg
|
|
63
|
+
self.timestamps.append(self.stamp_to_sec(msg.header.stamp))
|
|
59
64
|
return self.read_point_cloud(msg)
|
|
60
65
|
|
|
61
66
|
def __len__(self):
|
|
@@ -68,6 +73,18 @@ class McapDataloader:
|
|
|
68
73
|
if self.summary.channels[id].topic == self.topic
|
|
69
74
|
)
|
|
70
75
|
|
|
76
|
+
def reset(self):
|
|
77
|
+
self.timestamps = []
|
|
78
|
+
self.bag = self.make_reader(open(self.mcap_file, "rb"))
|
|
79
|
+
self.msgs = self.read_ros2_messages(self.mcap_file, topics=[self.topic])
|
|
80
|
+
|
|
81
|
+
@staticmethod
|
|
82
|
+
def stamp_to_sec(stamp):
|
|
83
|
+
return stamp.sec + float(stamp.nanosec) / 1e9
|
|
84
|
+
|
|
85
|
+
def get_frames_timestamps(self) -> list:
|
|
86
|
+
return self.timestamps
|
|
87
|
+
|
|
71
88
|
def check_topic(self, topic: str) -> str:
|
|
72
89
|
# Extract schema id from the .mcap file that encodes the PointCloud2 msg
|
|
73
90
|
schema_id = [
|
|
@@ -48,7 +48,7 @@ class MulranDataset:
|
|
|
48
48
|
timestamps = self.get_timestamps()
|
|
49
49
|
if points.shape[0] != timestamps.shape[0]:
|
|
50
50
|
# MuRan has some broken point clouds, just fallback to no timestamps
|
|
51
|
-
return points.astype(np.float64), np.
|
|
51
|
+
return points.astype(np.float64), np.array([])
|
|
52
52
|
return points.astype(np.float64), timestamps
|
|
53
53
|
|
|
54
54
|
@staticmethod
|
|
@@ -27,7 +27,7 @@ import sys
|
|
|
27
27
|
from pathlib import Path
|
|
28
28
|
|
|
29
29
|
import numpy as np
|
|
30
|
-
from
|
|
30
|
+
from scipy.spatial.transform import Rotation as R
|
|
31
31
|
|
|
32
32
|
|
|
33
33
|
class NewerCollegeDataset:
|
|
@@ -63,7 +63,7 @@ class NewerCollegeDataset:
|
|
|
63
63
|
timestamps = self.get_timestamps()
|
|
64
64
|
if points.shape[0] != timestamps.shape[0]:
|
|
65
65
|
# MuRan has some broken point clouds, just fallback to no timestamps
|
|
66
|
-
return points.astype(np.float64), np.
|
|
66
|
+
return points.astype(np.float64), np.array([])
|
|
67
67
|
return points.astype(np.float64), timestamps
|
|
68
68
|
|
|
69
69
|
@staticmethod
|
|
@@ -89,12 +89,7 @@ class NewerCollegeDataset:
|
|
|
89
89
|
"""Taken from pyLiDAR-SLAM/blob/master/slam/dataset/nhcd_dataset.py"""
|
|
90
90
|
ground_truth_df = np.genfromtxt(str(file_path), delimiter=",", dtype=np.float64)
|
|
91
91
|
xyz = ground_truth_df[:, 2:5]
|
|
92
|
-
rotations = np.array(
|
|
93
|
-
[
|
|
94
|
-
Quaternion(x=x, y=y, z=z, w=w).rotation_matrix
|
|
95
|
-
for x, y, z, w in ground_truth_df[:, 5:]
|
|
96
|
-
]
|
|
97
|
-
)
|
|
92
|
+
rotations = np.array([R.from_quat(q).as_matrix() for q in ground_truth_df[:, 5:]])
|
|
98
93
|
|
|
99
94
|
num_poses = rotations.shape[0]
|
|
100
95
|
poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0)
|
|
@@ -102,7 +97,7 @@ class NewerCollegeDataset:
|
|
|
102
97
|
poses[:, :3, 3] = xyz
|
|
103
98
|
|
|
104
99
|
T_CL = np.eye(4, dtype=np.float32)
|
|
105
|
-
T_CL[:3, :3] =
|
|
100
|
+
T_CL[:3, :3] = R.from_quat([0, 0, 0.924, 0.383]).as_matrix()
|
|
106
101
|
T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32)
|
|
107
102
|
poses = np.einsum("nij,jk->nik", poses, T_CL)
|
|
108
103
|
poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses)
|
|
@@ -22,30 +22,12 @@
|
|
|
22
22
|
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
23
23
|
# SOFTWARE.
|
|
24
24
|
|
|
25
|
-
import glob
|
|
26
25
|
import os
|
|
27
26
|
from typing import Optional
|
|
28
27
|
|
|
29
28
|
import numpy as np
|
|
30
29
|
|
|
31
30
|
|
|
32
|
-
def find_metadata_json(pcap_file: str) -> str:
|
|
33
|
-
"""Attempts to resolve the metadata json file for a provided pcap file."""
|
|
34
|
-
dir_path, filename = os.path.split(pcap_file)
|
|
35
|
-
if not filename:
|
|
36
|
-
return ""
|
|
37
|
-
if not dir_path:
|
|
38
|
-
dir_path = os.getcwd()
|
|
39
|
-
json_candidates = sorted(glob.glob(f"{dir_path}/*.json"))
|
|
40
|
-
if not json_candidates:
|
|
41
|
-
return ""
|
|
42
|
-
prefix_sizes = list(
|
|
43
|
-
map(lambda p: len(os.path.commonprefix((filename, os.path.basename(p)))), json_candidates)
|
|
44
|
-
)
|
|
45
|
-
max_elem = max(range(len(prefix_sizes)), key=lambda i: prefix_sizes[i])
|
|
46
|
-
return json_candidates[max_elem]
|
|
47
|
-
|
|
48
|
-
|
|
49
31
|
class OusterDataloader:
|
|
50
32
|
"""Ouster pcap dataloader"""
|
|
51
33
|
|
|
@@ -83,64 +65,42 @@ class OusterDataloader:
|
|
|
83
65
|
"""
|
|
84
66
|
|
|
85
67
|
try:
|
|
86
|
-
|
|
87
|
-
from ouster import client
|
|
68
|
+
from ouster.sdk import client, open_source
|
|
88
69
|
except ImportError:
|
|
89
|
-
print(
|
|
90
|
-
f'[ERROR] ouster-sdk is not installed on your system, run "pip install ouster-sdk"'
|
|
91
|
-
)
|
|
70
|
+
print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"')
|
|
92
71
|
exit(1)
|
|
93
72
|
|
|
94
|
-
# since we import ouster-sdk's client module locally, we keep it locally as well
|
|
95
|
-
self._client = client
|
|
96
|
-
|
|
97
73
|
assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file"
|
|
98
74
|
|
|
99
75
|
# we expect `data_dir` param to be a path to the .pcap file, so rename for clarity
|
|
100
76
|
pcap_file = data_dir
|
|
101
77
|
|
|
102
|
-
|
|
103
|
-
if
|
|
104
|
-
print("[ERROR] Ouster pcap dataloader can't find metadata json file.")
|
|
105
|
-
exit(1)
|
|
106
|
-
print("[INFO] Ouster pcap dataloader: using metadata json: ", metadata_json)
|
|
78
|
+
print("Indexing Ouster pcap to count the scans number ...")
|
|
79
|
+
source = open_source(str(pcap_file), meta=[meta] if meta else [], index=True)
|
|
107
80
|
|
|
108
|
-
|
|
81
|
+
# since we import ouster-sdk's client module locally, we keep reference
|
|
82
|
+
# to it locally as well
|
|
83
|
+
self._client = client
|
|
109
84
|
|
|
110
|
-
|
|
111
|
-
self._info_json = json.read()
|
|
112
|
-
self._info = client.SensorInfo(self._info_json)
|
|
85
|
+
self.data_dir = os.path.dirname(data_dir)
|
|
113
86
|
|
|
114
87
|
# lookup table for 2D range image projection to a 3D point cloud
|
|
115
|
-
self._xyz_lut = client.XYZLut(
|
|
88
|
+
self._xyz_lut = client.XYZLut(source.metadata)
|
|
116
89
|
|
|
117
90
|
self._pcap_file = str(data_dir)
|
|
118
91
|
|
|
119
|
-
|
|
120
|
-
print("
|
|
121
|
-
self._source = pcap.Pcap(self._pcap_file, self._info)
|
|
122
|
-
self._scans_num = sum((1 for _ in client.Scans(self._source)))
|
|
123
|
-
print(f"[INFO] Ouster pcap total scans number: {self._scans_num}")
|
|
92
|
+
self._scans_num = len(source)
|
|
93
|
+
print(f"Ouster pcap total scans number: {self._scans_num}")
|
|
124
94
|
|
|
125
95
|
# frame timestamps array
|
|
126
96
|
self._timestamps = np.linspace(0, self._scans_num, self._scans_num, endpoint=False)
|
|
127
97
|
|
|
128
|
-
|
|
129
|
-
self._source = pcap.Pcap(self._pcap_file, self._info)
|
|
130
|
-
self._scans_iter = iter(client.Scans(self._source))
|
|
131
|
-
self._next_idx = 0
|
|
98
|
+
self._source = source
|
|
132
99
|
|
|
133
100
|
def __getitem__(self, idx):
|
|
134
|
-
|
|
135
|
-
|
|
136
|
-
|
|
137
|
-
"Ouster pcap dataloader supports only sequential reads. "
|
|
138
|
-
f"Expected idx: {self._next_idx}, but got {idx}"
|
|
139
|
-
)
|
|
140
|
-
scan = next(self._scans_iter)
|
|
141
|
-
self._next_idx += 1
|
|
142
|
-
|
|
143
|
-
self._timestamps[self._next_idx - 1] = 1e-9 * scan.timestamp[0]
|
|
101
|
+
scan = self._source[idx]
|
|
102
|
+
|
|
103
|
+
self._timestamps[idx] = 1e-9 * scan.timestamp[0]
|
|
144
104
|
|
|
145
105
|
timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1))
|
|
146
106
|
|