From 1dba3e80c824356e948a9d794dd3a466813e6580 Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Wed, 6 Aug 2025 12:04:52 -0400 Subject: [PATCH 1/8] Add CuMulti dataset + ROS2 bag improvements --- python/evalio/datasets/__init__.py | 2 + python/evalio/datasets/cumulti.py | 231 +++++++++++++++++++++++++++++ python/evalio/datasets/loaders.py | 12 +- 3 files changed, 239 insertions(+), 6 deletions(-) create mode 100644 python/evalio/datasets/cumulti.py diff --git a/python/evalio/datasets/__init__.py b/python/evalio/datasets/__init__.py index 5df729f1..ea89a3af 100644 --- a/python/evalio/datasets/__init__.py +++ b/python/evalio/datasets/__init__.py @@ -1,5 +1,6 @@ from .base import Dataset, DatasetIterator, set_data_dir, get_data_dir from .botanic_garden import BotanicGarden +from .cumulti import CUMulti from .enwide import EnWide from .helipr import HeLiPR from .hilti_2022 import Hilti2022 @@ -16,6 +17,7 @@ "Dataset", "DatasetIterator", "BotanicGarden", + "CUMulti", "EnWide", "HeLiPR", "Hilti2022", diff --git a/python/evalio/datasets/cumulti.py b/python/evalio/datasets/cumulti.py new file mode 100644 index 00000000..f2b908c1 --- /dev/null +++ b/python/evalio/datasets/cumulti.py @@ -0,0 +1,231 @@ +import os +import urllib +import urllib.request +import zipfile +from enum import auto +from pathlib import Path + +import numpy as np +from tqdm.rich import tqdm + +from evalio.datasets.loaders import ( + LidarDensity, + LidarFormatParams, + LidarMajor, + LidarPointStamp, + LidarStamp, + RosbagIter, +) +from evalio.types import SE3, SO3, Trajectory + +from .base import Dataset, DatasetIterator, ImuParams, LidarParams + + +# https://github.com/pytorch/vision/blob/fc746372bedce81ecd53732ee101e536ae3afec1/torchvision/datasets/utils.py#L27 +def _urlretrieve(url: str, filename: Path, chunk_size: int = 1024 * 32) -> None: + """ + Retrieves a file from url using urllib + """ + with urllib.request.urlopen( + urllib.request.Request(url, headers={"User-Agent": "evalio"}) + ) as response: + with ( + open(filename, "wb") as fh, + tqdm( + total=100e9, # GLOBUS does not include size in request response, guess=100GB + unit="B", + unit_scale=True, + dynamic_ncols=True, + ) as pbar, + ): + while chunk := response.read(chunk_size): + fh.write(chunk) + pbar.update(len(chunk)) + + +def _extract_noreplace(zip_file: Path, dest_dir: Path): + """ + Extracts files from a zip archive (zip_file) but skips any file that already exists at the destination + (Allows to resume zip extractions) + """ + print(f"Extracting: {zip_file.name}") + with zipfile.ZipFile(str(zip_file)) as archive: + with tqdm(total=len(archive.namelist()), dynamic_ncols=True) as pbar: + for filename in archive.namelist(): + if not (dest_dir / filename).is_file(): + archive.extract(filename, path=dest_dir) + pbar.update() + + +class CUMulti(Dataset): + """ + Dataset collected by a ground robot (AgileX - Hunter) on the University of Colorado Boulder Campus. + """ + kittredge_loop_robot1 = auto() + kittredge_loop_robot2 = auto() + kittredge_loop_robot3 = auto() + kittredge_loop_robot4 = auto() + main_campus_robot1 = auto() + main_campus_robot2 = auto() + main_campus_robot3 = auto() + main_campus_robot4 = auto() + + # ------------------------- For loading data ------------------------- # + def data_iter(self) -> DatasetIterator: + lidar_format = LidarFormatParams( + stamp=LidarStamp.End, + point_stamp=LidarPointStamp.Start, + major=LidarMajor.Row, + density=LidarDensity.AllPoints, + ) + + robot_name = self.seq_name.split("_")[-1] + return RosbagIter( + self.folder, + f"{robot_name}/ouster/points", + f"{robot_name}/imu/data", + self.lidar_params(), + lidar_format=lidar_format, + is_ros2=True, + ) + + def ground_truth_raw(self) -> Trajectory: + # Sequence Naming information + components = self.seq_name.split("_") + robot_name = components[-1] + loc_name = "_".join(components[:2]) + gt_file = self.folder / f"{loc_name}_{robot_name}_ref.csv" + + # Load the Trajectory which provides solutions in the UTM frame + traj_utm = Trajectory.from_csv( + gt_file, ["sec", "x", "y", "z", "qx", "qy", "qz", "qw"] + ) + + # Subtract out the initial position to get a local frame reference + init_position = traj_utm.poses[0].trans + return Trajectory( + poses=[SE3(p.rot, p.trans - init_position) for p in traj_utm.poses], + stamps=traj_utm.stamps, + ) + + # ------------------------- For loading params ------------------------- # + def imu_T_lidar(self) -> SE3: + # Derived from the published `description/single_robot_urdf.xacro` + mountplate_T_ouster_riser = SE3( + SO3.identity(), np.array([(0.1524 * 0.15), 0.0, (0.02 / 2.0 + 0.015 / 2.0)]) + ) + ouster_riser_T_ouster = SE3( + SO3.identity(), np.array([0.0, 0.0, (0.02 / 2.0 + 0.04)]) + ) + mountplate_T_imu = SE3( + SO3(qx=0, qy=0, qz=1, qw=0), + np.array([-0.04, 0.01557, (0.015 / 2.0 + 0.01331 / 2.0)]), + ) + return ( + mountplate_T_imu.inverse() + * mountplate_T_ouster_riser + * ouster_riser_T_ouster + ) + + def imu_T_gt(self) -> SE3: + # Groundtruth provided in the IMU Frame + return SE3.identity() + + def imu_params(self) -> ImuParams: + # https://www.mouser.com/datasheet/2/1083/3dmgq7_gnssins_ds_0-1900596.pdf + return ImuParams( + gyro=4.363323129985824e-05, # From 0.15 Deg / sqrt(hr) + accel=1.962e-4, # From 20 micro-gravity / sqrt(Hz) + gyro_bias=1e-6, # TODO (dan) + accel_bias=1e-6, # TODO (dan) + bias_init=1e-8, + integration=1e-8, + gravity=np.array([0, 0, -9.81]), + brand="MicroStrain", + model="3DM-GQ7", + ) + + def lidar_params(self) -> LidarParams: + # Ouster OS 64 Rev 7 + return LidarParams( + num_rows=64, + num_columns=1024, + min_range=1.0, + max_range=200.0, + brand="Ouster", + model="OS-64 (Rev 7)", + ) + + # ------------------------- dataset info ------------------------- # + @staticmethod + def url() -> str: + return "https://arpg.github.io/cumulti/" + + @classmethod + def dataset_name(cls) -> str: + return "cumulti" + + def environment(self) -> str: + return "CU Boulder Campus" + + def vehicle(self) -> str: + return "ATV" + + # ------------------------- For downloading ------------------------- # + def files(self) -> list[str]: + components = self.seq_name.split("_") + robot_name = components[-1] + loc_name = "_".join(components[:2]) + + return [ + self.folder / f"{robot_name}_{loc_name}_imu_gps", # IMU Bag + self.folder / f"{robot_name}_{loc_name}_lidar", # LIDAR Bag + self.folder / f"{loc_name}_{robot_name}_ref.csv", # Reference Solution + ] + + def download(self): + # Sequence Naming information + components = self.seq_name.split("_") + robot_name = components[-1] + loc_name = "_".join(components[:2]) + + # File Download URLS + globus_url = r"https://g-ad45ee.3d2bab.75bc.data.globus.org" + seq_url_base_path = ( + f"{globus_url}/{loc_name}/{robot_name}/{robot_name}_{loc_name}" + ) + lidar_url = f"{seq_url_base_path}_lidar.zip" + imu_url = f"{seq_url_base_path}_imu_gps.zip" + + # Download destination zip files + lidar_zip_file = self.folder / f"{robot_name}_{loc_name}_lidar.zip" + imu_zip_file = self.folder / f"{robot_name}_{loc_name}_imu_gps.zip" + + # Download all of the data + print(f"Downloading to {self.folder}...") + self.folder.mkdir(parents=True, exist_ok=True) + for zip_file, url in [(lidar_zip_file, lidar_url), (imu_zip_file, imu_url)]: + if not zip_file.is_file(): + print(url) + _urlretrieve(url, zip_file) + else: + print(f"Archive already exists. Skipping Download.") + + # Extract from the zip only what we dont already have + print("Extracting data...") + _extract_noreplace(zip_file, self.folder) + + # Download the groundtruth + gt_url = f"{globus_url}/{loc_name}/{robot_name}/{loc_name}_{robot_name}/" + gt_file = self.folder / f"{loc_name}_{robot_name}_ref.csv" + print(gt_url) + if not gt_file.is_file(): + _urlretrieve(gt_url, gt_file) + else: + print(f"Groundtruth already exists. Skipping Download.") + + # If we have extracted everything we need then remove the zip directory + if self.is_downloaded(): + print("All data downloaded. Removing zip files...") + for zip_file in [lidar_zip_file, imu_zip_file]: + os.remove(zip_file) diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index 91fd6a02..813fbea9 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -78,19 +78,19 @@ def __init__( lidar_topic: str, imu_topic: str, lidar_params: LidarParams, - # for mcap files, we point at the directory, not the file - is_mcap: bool = False, + # for ros2 files (mcap, db3), we point at the directory, not the file + is_ros2: bool = False, # Reduce compute by telling the iterator how to format the pointcloud lidar_format: Optional[LidarFormatParams] = None, custom_col_func: Optional[Callable[[LidarMeasurement], None]] = None, ): """ Args: - path (Path): Location of rosbag file(s). If a directory is passed, all .bag files in the directory will be loaded. + path (Path): Location of rosbag file(s) or dir(s). If a directory is passed, all .bag files in the directory will be loaded. lidar_topic (str): Name of lidar topic. imu_topic (str): Name of imu topic. lidar_params (LidarParams): Lidar parameters, can be gotten from [lidar_params][evalio.datasets.Dataset.lidar_params]. - is_mcap (bool, optional): If an mcap file, will not try to glob over all rosbags. Defaults to False. + is_ros2 (bool, optional): If in ros2 format, we will glob all sub-directories. Defaults to False. lidar_format (Optional[LidarFormatParams], optional): Various parameters for how lidar data is stored. If not specified, most will try to be inferred. We strongly recommend setting this to ensure data is standardized properly. Defaults to None. custom_col_func (Optional[Callable[[LidarMeasurement], None]], optional): Function to put the point cloud in row major format. Will generally not be needed, except for strange default orderings. Defaults to None. @@ -108,12 +108,12 @@ def __init__( self.custom_col_func = custom_col_func # Glob to get all .bag files in the directory - if path.is_dir() and is_mcap is False: + if path.is_dir() and is_ros2 is False: self.path = [p for p in path.glob("*.bag") if "orig" not in str(p)] if not self.path: raise FileNotFoundError(f"No .bag files found in directory {path}") else: - self.path = [path] + self.path = [p for p in path.glob("*/")] # Open the bag file self.reader = AnyReader(self.path) From 2a686b1399916a87c69ebe58188bd8fb2aad7b4b Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Wed, 6 Aug 2025 12:13:19 -0400 Subject: [PATCH 2/8] Update Oxford Spires for new RosbagIter interface --- python/evalio/datasets/oxford_spires.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/evalio/datasets/oxford_spires.py b/python/evalio/datasets/oxford_spires.py index 0db40f35..11a89efd 100644 --- a/python/evalio/datasets/oxford_spires.py +++ b/python/evalio/datasets/oxford_spires.py @@ -52,7 +52,7 @@ def data_iter(self) -> DatasetIterator: "/hesai/pandar", "/alphasense_driver_ros/imu", self.lidar_params(), - is_mcap=True, + is_ros2==True, lidar_format=LidarFormatParams( stamp=LidarStamp.Start, point_stamp=LidarPointStamp.Start, From eee930a1db2f406c41d661961b9d18bb72d850a8 Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Fri, 8 Aug 2025 09:50:36 -0400 Subject: [PATCH 3/8] Improve RosbagIter input path deduction Should now automatically accept - A path to a ros1 or ros2 bag - A Path to a dir containing multiple ros1 or ros2 bags --- python/evalio/datasets/cumulti.py | 1 - python/evalio/datasets/loaders.py | 31 +++++++++++++++++-------- python/evalio/datasets/oxford_spires.py | 1 - 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/python/evalio/datasets/cumulti.py b/python/evalio/datasets/cumulti.py index f2b908c1..08cfe774 100644 --- a/python/evalio/datasets/cumulti.py +++ b/python/evalio/datasets/cumulti.py @@ -86,7 +86,6 @@ def data_iter(self) -> DatasetIterator: f"{robot_name}/imu/data", self.lidar_params(), lidar_format=lidar_format, - is_ros2=True, ) def ground_truth_raw(self) -> Trajectory: diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index 813fbea9..f7f49115 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -78,19 +78,16 @@ def __init__( lidar_topic: str, imu_topic: str, lidar_params: LidarParams, - # for ros2 files (mcap, db3), we point at the directory, not the file - is_ros2: bool = False, # Reduce compute by telling the iterator how to format the pointcloud lidar_format: Optional[LidarFormatParams] = None, custom_col_func: Optional[Callable[[LidarMeasurement], None]] = None, ): """ Args: - path (Path): Location of rosbag file(s) or dir(s). If a directory is passed, all .bag files in the directory will be loaded. + path (Path): Location of rosbag file(s) or dir(s). If a directory is passed containing multiple bags (ros1 or ros2), all will be loaded. lidar_topic (str): Name of lidar topic. imu_topic (str): Name of imu topic. lidar_params (LidarParams): Lidar parameters, can be gotten from [lidar_params][evalio.datasets.Dataset.lidar_params]. - is_ros2 (bool, optional): If in ros2 format, we will glob all sub-directories. Defaults to False. lidar_format (Optional[LidarFormatParams], optional): Various parameters for how lidar data is stored. If not specified, most will try to be inferred. We strongly recommend setting this to ensure data is standardized properly. Defaults to None. custom_col_func (Optional[Callable[[LidarMeasurement], None]], optional): Function to put the point cloud in row major format. Will generally not be needed, except for strange default orderings. Defaults to None. @@ -107,13 +104,27 @@ def __init__( self.lidar_format = lidar_format self.custom_col_func = custom_col_func - # Glob to get all .bag files in the directory - if path.is_dir() and is_ros2 is False: - self.path = [p for p in path.glob("*.bag") if "orig" not in str(p)] - if not self.path: - raise FileNotFoundError(f"No .bag files found in directory {path}") + # Find all bags (may be either ros1 .bag files or ros2 bag/ dirs) + if path.is_file(): + # Provide path is a ros1 .bag file + self.path = [path] else: - self.path = [p for p in path.glob("*/")] + # Path provided is a directory may be ros2 bag/ dir or contain multiple bags + bag_file_list = [p for p in path.glob("*.bag") if "orig" not in str(p)] + sub_dir_list = [d for d in path.glob("*/") if "orig" not in str(d)] + database_file_list = list(path.glob("*.mcap")) + list(path.glob("*.db3")) + + if bag_file_list: # path contains ros1 .bag files + self.path = bag_file_list + elif sub_dir_list: # path contains ros2 bag/ directories + self.path = sub_dir_list + elif database_file_list: # path is a ros2 bag/ (contains mcap or db3 file) + self.path = [path] + else: + raise ValueError( + f"Invalid rosbag path: {path}" + "\nExpected path to be one of -- \na) ros1 .bag \nb) ros2 bag/ dir or \nc) directory multiple a or b" + ) # Open the bag file self.reader = AnyReader(self.path) diff --git a/python/evalio/datasets/oxford_spires.py b/python/evalio/datasets/oxford_spires.py index 11a89efd..a4b75fb2 100644 --- a/python/evalio/datasets/oxford_spires.py +++ b/python/evalio/datasets/oxford_spires.py @@ -52,7 +52,6 @@ def data_iter(self) -> DatasetIterator: "/hesai/pandar", "/alphasense_driver_ros/imu", self.lidar_params(), - is_ros2==True, lidar_format=LidarFormatParams( stamp=LidarStamp.Start, point_stamp=LidarPointStamp.Start, From 39e7601172968b71cda117cf720702cb976c4e51 Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Fri, 8 Aug 2025 09:58:38 -0400 Subject: [PATCH 4/8] Improve Ros2 bag checks + RUFF check --- python/evalio/datasets/loaders.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index f7f49115..69309690 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -109,21 +109,20 @@ def __init__( # Provide path is a ros1 .bag file self.path = [path] else: + is_ros2_dir = lambda d: bool(list(d.glob("*.mcap")) + list(d.glob("*.db3"))) # Path provided is a directory may be ros2 bag/ dir or contain multiple bags bag_file_list = [p for p in path.glob("*.bag") if "orig" not in str(p)] - sub_dir_list = [d for d in path.glob("*/") if "orig" not in str(d)] - database_file_list = list(path.glob("*.mcap")) + list(path.glob("*.db3")) + sub_dir_list = [d for d in path.glob("*/") if "orig" not in str(d) and is_ros2_dir(d)] if bag_file_list: # path contains ros1 .bag files self.path = bag_file_list elif sub_dir_list: # path contains ros2 bag/ directories self.path = sub_dir_list - elif database_file_list: # path is a ros2 bag/ (contains mcap or db3 file) + elif is_ros2_dir(path): # path is a ros2 bag/ (contains mcap or db3 file) self.path = [path] else: raise ValueError( - f"Invalid rosbag path: {path}" - "\nExpected path to be one of -- \na) ros1 .bag \nb) ros2 bag/ dir or \nc) directory multiple a or b" + f"Invalid rosbag path: {path}\nExpected path to be one of:\na) ros1 .bag \nb) ros2 bag/ dir or \nc) directory multiple a or b" ) # Open the bag file From 60cd62060e9c5b54c2c7b1b64ee101d49c3e086e Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Fri, 8 Aug 2025 10:00:48 -0400 Subject: [PATCH 5/8] Improve naming in RosbagIter glob logic --- python/evalio/datasets/loaders.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index 69309690..d51617d4 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -109,16 +109,16 @@ def __init__( # Provide path is a ros1 .bag file self.path = [path] else: - is_ros2_dir = lambda d: bool(list(d.glob("*.mcap")) + list(d.glob("*.db3"))) + is_ros2_bag = lambda d: bool(list(d.glob("*.mcap")) + list(d.glob("*.db3"))) # Path provided is a directory may be ros2 bag/ dir or contain multiple bags - bag_file_list = [p for p in path.glob("*.bag") if "orig" not in str(p)] - sub_dir_list = [d for d in path.glob("*/") if "orig" not in str(d) and is_ros2_dir(d)] - - if bag_file_list: # path contains ros1 .bag files - self.path = bag_file_list - elif sub_dir_list: # path contains ros2 bag/ directories - self.path = sub_dir_list - elif is_ros2_dir(path): # path is a ros2 bag/ (contains mcap or db3 file) + ros1_bag_file_list = [p for p in path.glob("*.bag") if "orig" not in str(p)] + ros2_bag_dir_list = [d for d in path.glob("*/") if "orig" not in str(d) and is_ros2_bag(d)] + + if ros1_bag_file_list: # path contains ros1 .bag files + self.path = ros1_bag_file_list + elif ros2_bag_dir_list: # path contains ros2 bag/ directories + self.path = ros2_bag_dir_list + elif is_ros2_bag(path): # path is a ros2 bag/ (contains mcap or db3 file) self.path = [path] else: raise ValueError( From 0161a2dae5545e9c40c81f635987308297eaf0ba Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Fri, 8 Aug 2025 11:52:22 -0400 Subject: [PATCH 6/8] Fix ruff issues in cumulti chances --- python/evalio/datasets/cumulti.py | 4 ++-- python/evalio/datasets/loaders.py | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/python/evalio/datasets/cumulti.py b/python/evalio/datasets/cumulti.py index 08cfe774..bdf78713 100644 --- a/python/evalio/datasets/cumulti.py +++ b/python/evalio/datasets/cumulti.py @@ -208,7 +208,7 @@ def download(self): print(url) _urlretrieve(url, zip_file) else: - print(f"Archive already exists. Skipping Download.") + print("Archive already exists. Skipping Download.") # Extract from the zip only what we dont already have print("Extracting data...") @@ -221,7 +221,7 @@ def download(self): if not gt_file.is_file(): _urlretrieve(gt_url, gt_file) else: - print(f"Groundtruth already exists. Skipping Download.") + print("Groundtruth already exists. Skipping Download.") # If we have extracted everything we need then remove the zip directory if self.is_downloaded(): diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index d51617d4..a95a1ea7 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -109,7 +109,8 @@ def __init__( # Provide path is a ros1 .bag file self.path = [path] else: - is_ros2_bag = lambda d: bool(list(d.glob("*.mcap")) + list(d.glob("*.db3"))) + def is_ros2_bag(d): + return bool(list(d.glob("*.mcap")) + list(d.glob("*.db3"))) # Path provided is a directory may be ros2 bag/ dir or contain multiple bags ros1_bag_file_list = [p for p in path.glob("*.bag") if "orig" not in str(p)] ros2_bag_dir_list = [d for d in path.glob("*/") if "orig" not in str(d) and is_ros2_bag(d)] From 0c9e8f2245942ee009bb5f24c83a9a706873bc34 Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Sat, 6 Sep 2025 11:26:41 -0400 Subject: [PATCH 7/8] Update CU-Multi Config Params - Updates the Lidar - IMU extrinsics - Fixes LiDAR rate - Fixes LiDAR stamp convention --- python/evalio/datasets/cumulti.py | 33 ++++++++++++++----------------- 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/python/evalio/datasets/cumulti.py b/python/evalio/datasets/cumulti.py index bdf78713..1736885a 100644 --- a/python/evalio/datasets/cumulti.py +++ b/python/evalio/datasets/cumulti.py @@ -16,7 +16,7 @@ LidarStamp, RosbagIter, ) -from evalio.types import SE3, SO3, Trajectory +from evalio.types import SE3, Trajectory from .base import Dataset, DatasetIterator, ImuParams, LidarParams @@ -59,8 +59,9 @@ def _extract_noreplace(zip_file: Path, dest_dir: Path): class CUMulti(Dataset): """ - Dataset collected by a ground robot (AgileX - Hunter) on the University of Colorado Boulder Campus. + Dataset collected by a ground robot (AgileX - Hunter) on the University of Colorado Boulder Campus. """ + kittredge_loop_robot1 = auto() kittredge_loop_robot2 = auto() kittredge_loop_robot3 = auto() @@ -73,7 +74,7 @@ class CUMulti(Dataset): # ------------------------- For loading data ------------------------- # def data_iter(self) -> DatasetIterator: lidar_format = LidarFormatParams( - stamp=LidarStamp.End, + stamp=LidarStamp.Start, point_stamp=LidarPointStamp.Start, major=LidarMajor.Row, density=LidarDensity.AllPoints, @@ -109,21 +110,16 @@ def ground_truth_raw(self) -> Trajectory: # ------------------------- For loading params ------------------------- # def imu_T_lidar(self) -> SE3: - # Derived from the published `description/single_robot_urdf.xacro` - mountplate_T_ouster_riser = SE3( - SO3.identity(), np.array([(0.1524 * 0.15), 0.0, (0.02 / 2.0 + 0.015 / 2.0)]) - ) - ouster_riser_T_ouster = SE3( - SO3.identity(), np.array([0.0, 0.0, (0.02 / 2.0 + 0.04)]) - ) - mountplate_T_imu = SE3( - SO3(qx=0, qy=0, qz=1, qw=0), - np.array([-0.04, 0.01557, (0.015 / 2.0 + 0.01331 / 2.0)]), - ) - return ( - mountplate_T_imu.inverse() - * mountplate_T_ouster_riser - * ouster_riser_T_ouster + # Supplied by CU-Multi Authors + return SE3.fromMat( + np.array( + [ + [-1.0, 0.0, 0.0, -0.058038], + [0.0, -1.0, 0.0, 0.015573], + [0.0, 0.0, 1.0, 0.049603], + [0.0, 0.0, 0.0, 1.000000], + ] + ) ) def imu_T_gt(self) -> SE3: @@ -147,6 +143,7 @@ def imu_params(self) -> ImuParams: def lidar_params(self) -> LidarParams: # Ouster OS 64 Rev 7 return LidarParams( + rate=20, num_rows=64, num_columns=1024, min_range=1.0, From 0f3f3d6e2ba65dc36c2464d82ffb490b5ab8adf5 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 10 Sep 2025 16:44:15 -0400 Subject: [PATCH 8/8] Remove orig, add in manually loading typestores --- python/evalio/datasets/cumulti.py | 5 ++++- python/evalio/datasets/loaders.py | 25 +++++++++++++++++++------ 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/python/evalio/datasets/cumulti.py b/python/evalio/datasets/cumulti.py index 1736885a..1d0d7065 100644 --- a/python/evalio/datasets/cumulti.py +++ b/python/evalio/datasets/cumulti.py @@ -8,6 +8,8 @@ import numpy as np from tqdm.rich import tqdm +from rosbags.typesys import Stores + from evalio.datasets.loaders import ( LidarDensity, LidarFormatParams, @@ -87,6 +89,7 @@ def data_iter(self) -> DatasetIterator: f"{robot_name}/imu/data", self.lidar_params(), lidar_format=lidar_format, + type_store=Stores.ROS2_FOXY, ) def ground_truth_raw(self) -> Trajectory: @@ -168,7 +171,7 @@ def vehicle(self) -> str: return "ATV" # ------------------------- For downloading ------------------------- # - def files(self) -> list[str]: + def files(self) -> list[str | Path]: components = self.seq_name.split("_") robot_name = components[-1] loc_name = "_".join(components[:2]) diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index a95a1ea7..2f4531c8 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -21,6 +21,7 @@ ) from evalio.datasets.base import DatasetIterator, Measurement from rosbags.highlevel import AnyReader +from rosbags.typesys import Stores, get_typestore import numpy as np from dataclasses import dataclass from enum import StrEnum, auto @@ -78,6 +79,7 @@ def __init__( lidar_topic: str, imu_topic: str, lidar_params: LidarParams, + type_store: Optional[Stores] = None, # Reduce compute by telling the iterator how to format the pointcloud lidar_format: Optional[LidarFormatParams] = None, custom_col_func: Optional[Callable[[LidarMeasurement], None]] = None, @@ -88,6 +90,7 @@ def __init__( lidar_topic (str): Name of lidar topic. imu_topic (str): Name of imu topic. lidar_params (LidarParams): Lidar parameters, can be gotten from [lidar_params][evalio.datasets.Dataset.lidar_params]. + type_store (Optional[Stores], optional): Additional type store to be loaded into rosbags. Defaults to None. lidar_format (Optional[LidarFormatParams], optional): Various parameters for how lidar data is stored. If not specified, most will try to be inferred. We strongly recommend setting this to ensure data is standardized properly. Defaults to None. custom_col_func (Optional[Callable[[LidarMeasurement], None]], optional): Function to put the point cloud in row major format. Will generally not be needed, except for strange default orderings. Defaults to None. @@ -109,17 +112,19 @@ def __init__( # Provide path is a ros1 .bag file self.path = [path] else: + def is_ros2_bag(d): return bool(list(d.glob("*.mcap")) + list(d.glob("*.db3"))) + # Path provided is a directory may be ros2 bag/ dir or contain multiple bags - ros1_bag_file_list = [p for p in path.glob("*.bag") if "orig" not in str(p)] - ros2_bag_dir_list = [d for d in path.glob("*/") if "orig" not in str(d) and is_ros2_bag(d)] + ros1_bag_file_list = [p for p in path.glob("*.bag")] + ros2_bag_dir_list = [d for d in path.glob("*/") if is_ros2_bag(d)] - if ros1_bag_file_list: # path contains ros1 .bag files - self.path = ros1_bag_file_list - elif ros2_bag_dir_list: # path contains ros2 bag/ directories + if ros1_bag_file_list: # path contains ros1 .bag files + self.path = ros1_bag_file_list + elif ros2_bag_dir_list: # path contains ros2 bag/ directories self.path = ros2_bag_dir_list - elif is_ros2_bag(path): # path is a ros2 bag/ (contains mcap or db3 file) + elif is_ros2_bag(path): # path is a ros2 bag/ (contains mcap or db3 file) self.path = [path] else: raise ValueError( @@ -129,6 +134,14 @@ def is_ros2_bag(d): # Open the bag file self.reader = AnyReader(self.path) self.reader.open() + + # force load passed in type store + # there is a default_typestore parameter in AnyReader, but it won't be used if one of bags has msgdefs + # this works around that, but may be unstable + # https://gitlab.com/ternaris/rosbags/-/blob/master/src/rosbags/highlevel/anyreader.py?ref_type=heads#L125-140 + if type_store is not None: + self.reader.typestore.register(get_typestore(type_store).fielddefs) + self.connections_lidar = [ x for x in self.reader.connections if x.topic == self.lidar_topic ]