-
Notifications
You must be signed in to change notification settings - Fork 44
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[UPDATE] drop kiss-loader dependency, update readme
- Loading branch information
Yue Pan
committed
Jun 5, 2024
1 parent
3c273d2
commit dfa5b17
Showing
29 changed files
with
2,279 additions
and
137 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,83 @@ | ||
# MIT License | ||
# | ||
# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill | ||
# Stachniss. | ||
# | ||
# Permission is hereby granted, free of charge, to any person obtaining a copy | ||
# of this software and associated documentation files (the "Software"), to deal | ||
# in the Software without restriction, including without limitation the rights | ||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
# copies of the Software, and to permit persons to whom the Software is | ||
# furnished to do so, subject to the following conditions: | ||
# | ||
# The above copyright notice and this permission notice shall be included in all | ||
# copies or substantial portions of the Software. | ||
# | ||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
# 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. | ||
from pathlib import Path | ||
from typing import Dict, List | ||
|
||
|
||
def supported_file_extensions(): | ||
return [ | ||
"bin", | ||
"pcd", | ||
"ply", | ||
"xyz", | ||
"obj", | ||
"ctm", | ||
"off", | ||
"stl", | ||
] | ||
|
||
|
||
def sequence_dataloaders(): | ||
# TODO: automatically infer this | ||
return ["kitti", "kitti_raw", "nuscenes", "helipr", "replica"] | ||
|
||
|
||
def available_dataloaders() -> List: | ||
import os.path | ||
import pkgutil | ||
|
||
pkgpath = os.path.dirname(__file__) | ||
return [name for _, name, _ in pkgutil.iter_modules([pkgpath])] | ||
|
||
|
||
def jumpable_dataloaders(): | ||
_jumpable_dataloaders = available_dataloaders() | ||
_jumpable_dataloaders.remove("mcap") | ||
_jumpable_dataloaders.remove("ouster") | ||
_jumpable_dataloaders.remove("rosbag") | ||
return _jumpable_dataloaders | ||
|
||
|
||
def dataloader_types() -> Dict: | ||
import ast | ||
import importlib | ||
|
||
dataloaders = available_dataloaders() | ||
_types = {} | ||
for dataloader in dataloaders: | ||
script = importlib.util.find_spec(f".{dataloader}", __name__).origin | ||
with open(script) as f: | ||
tree = ast.parse(f.read(), script) | ||
classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)] | ||
_types[dataloader] = classes[0].name # assuming there is only 1 class | ||
return _types | ||
|
||
|
||
def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs): | ||
import importlib | ||
|
||
dataloader_type = dataloader_types()[dataloader] | ||
module = importlib.import_module(f".{dataloader}", __name__) | ||
assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}" | ||
dataset = getattr(module, dataloader_type) | ||
return dataset(data_dir=data_dir, *args, **kwargs) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,71 @@ | ||
# MIT License | ||
# | ||
# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill | ||
# Stachniss. | ||
# | ||
# Permission is hereby granted, free of charge, to any person obtaining a copy | ||
# of this software and associated documentation files (the "Software"), to deal | ||
# in the Software without restriction, including without limitation the rights | ||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
# copies of the Software, and to permit persons to whom the Software is | ||
# furnished to do so, subject to the following conditions: | ||
# | ||
# The above copyright notice and this permission notice shall be included in all | ||
# copies or substantial portions of the Software. | ||
# | ||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
# 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. | ||
import glob | ||
import importlib | ||
import os | ||
import sys | ||
from pathlib import Path | ||
|
||
import natsort | ||
import numpy as np | ||
from pyquaternion import Quaternion | ||
|
||
|
||
class ApolloDataset: | ||
def __init__(self, data_dir: Path, *_, **__): | ||
try: | ||
self.o3d = importlib.import_module("open3d") | ||
except ModuleNotFoundError: | ||
print( | ||
'pcd files requires open3d and is not installed on your system run "pip install open3d"' | ||
) | ||
sys.exit(1) | ||
|
||
self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/pcds/*.pcd")) | ||
self.gt_poses = self.read_poses(f"{data_dir}/poses/gt_poses.txt") | ||
self.sequence_id = os.path.basename(data_dir) | ||
|
||
def __len__(self): | ||
return len(self.scan_files) | ||
|
||
def __getitem__(self, idx): | ||
return self.get_scan(self.scan_files[idx]) | ||
|
||
def get_scan(self, scan_file: str): | ||
points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64) | ||
return points.astype(np.float64) | ||
|
||
@staticmethod | ||
def read_poses(file): | ||
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] | ||
) | ||
poses = np.zeros([rotations.shape[0], 4, 4]) | ||
poses[:, :3, -1] = translations | ||
poses[:, :3, :3] = rotations | ||
poses[:, -1, -1] = 1 | ||
# Convert from global coordinate poses to local poses | ||
first_pose = poses[0, :, :] | ||
return np.linalg.inv(first_pose) @ poses |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,89 @@ | ||
# MIT License | ||
# | ||
# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill | ||
# Stachniss. | ||
# | ||
# Permission is hereby granted, free of charge, to any person obtaining a copy | ||
# of this software and associated documentation files (the "Software"), to deal | ||
# in the Software without restriction, including without limitation the rights | ||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
# copies of the Software, and to permit persons to whom the Software is | ||
# furnished to do so, subject to the following conditions: | ||
# | ||
# The above copyright notice and this permission notice shall be included in all | ||
# copies or substantial portions of the Software. | ||
# | ||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
# 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. | ||
import glob | ||
import os | ||
from pathlib import Path | ||
|
||
import natsort | ||
import numpy as np | ||
|
||
|
||
class BoreasDataset: | ||
def __init__(self, data_dir: Path, *_, **__): | ||
self.root_dir = os.path.realpath(data_dir) | ||
self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/lidar/*.bin")) | ||
self.gt_poses = self.load_poses(f"{data_dir}/applanix/lidar_poses.csv") | ||
self.sequence_id = os.path.basename(data_dir) | ||
assert len(self.scan_files) == self.gt_poses.shape[0] | ||
|
||
def __len__(self): | ||
return len(self.scan_files) | ||
|
||
def __getitem__(self, idx): | ||
return self.read_point_cloud(self.scan_files[idx]) | ||
|
||
def read_point_cloud(self, scan_file: str): | ||
points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 6))[:, :3] | ||
return points.astype(np.float64), self.get_timestamps(points) | ||
|
||
def load_poses(self, poses_file): | ||
data = np.loadtxt(poses_file, delimiter=",", skiprows=1) | ||
n, m = data.shape | ||
t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[0, :] | ||
first_pose = self.get_transformation_matrix(x, y, z, ya, p, r) | ||
poses = np.empty((n, 4, 4), dtype=np.float32) | ||
poses[0, :, :] = np.identity(4, dtype=np.float32) | ||
for i in range(n): | ||
t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[i, :] | ||
current_pose = self.get_transformation_matrix(x, y, z, ya, p, r) | ||
poses[i, :, :] = np.linalg.inv(first_pose) @ current_pose | ||
return poses | ||
|
||
@staticmethod | ||
def get_timestamps(points): | ||
x = points[:, 0] | ||
y = points[:, 1] | ||
yaw = -np.arctan2(y, x) | ||
timestamps = 0.5 * (yaw / np.pi + 1.0) | ||
return timestamps | ||
|
||
@staticmethod | ||
def get_transformation_matrix(x, y, z, yaw, pitch, roll): | ||
T_enu_sensor = np.identity(4, dtype=np.float64) | ||
R_yaw = np.array( | ||
[[np.cos(yaw), np.sin(yaw), 0], [-np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]], | ||
dtype=np.float64, | ||
) | ||
R_pitch = np.array( | ||
[[np.cos(pitch), 0, -np.sin(pitch)], [0, 1, 0], [np.sin(pitch), 0, np.cos(pitch)]], | ||
dtype=np.float64, | ||
) | ||
R_roll = np.array( | ||
[[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]], | ||
dtype=np.float64, | ||
) | ||
C_enu_sensor = R_roll @ R_pitch @ R_yaw | ||
T_enu_sensor[:3, :3] = C_enu_sensor | ||
r_sensor_enu_in_enu = np.array([x, y, z]).reshape(3, 1) | ||
T_enu_sensor[:3, 3:] = r_sensor_enu_in_enu | ||
return T_enu_sensor |
Oops, something went wrong.