From a5bf6690f87f036069064f9b259c03d723d859cd Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Fri, 8 May 2026 13:34:08 -0400 Subject: [PATCH 1/2] feat: Add small new constructor for LidarMeasurement --- cpp/bindings/types.h | 7 +++++++ cpp/evalio/types.h | 10 ++++++++++ 2 files changed, 17 insertions(+) diff --git a/cpp/bindings/types.h b/cpp/bindings/types.h index 414bb20..65c0d01 100644 --- a/cpp/bindings/types.h +++ b/cpp/bindings/types.h @@ -267,6 +267,13 @@ inline void make_types(nb::module_& m) { "List of points in the " "point cloud. Note, this is always in row major format." ) + .def_static( + "from_vec_positions", + &LidarMeasurement::from_vec_positions, + "stamp"_a, + "positions"_a, + "Construct a LidarMeasurement from a stamp and a (n,3) numpy array." + ) .def( "to_vec_positions", &LidarMeasurement::to_vec_positions, diff --git a/cpp/evalio/types.h b/cpp/evalio/types.h index 55d4e63..4f3524f 100644 --- a/cpp/evalio/types.h +++ b/cpp/evalio/types.h @@ -218,6 +218,16 @@ struct LidarMeasurement { return oss.str(); } + static LidarMeasurement + from_vec_positions(Stamp stamp, const Eigen::MatrixX3d& positions) { + std::vector pts; + pts.reserve(positions.rows()); + for (const auto& p : positions.rowwise()) { + pts.push_back(Point {.x = p.x(), .y = p.y(), .z = p.z()}); + } + return LidarMeasurement(stamp, pts); + } + std::vector to_vec_positions() const { std::vector eigen_points; eigen_points.reserve(points.size()); From c66f64ac0a9af1c380769b4df8b21e14acb22e28 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Fri, 8 May 2026 13:40:38 -0400 Subject: [PATCH 2/2] Handle missing gt data more elegantly --- pyproject.toml | 9 +++++++++ python/evalio/cli/ls.py | 4 +++- python/evalio/cli/run.py | 4 +++- python/evalio/cli/stats.py | 19 ++++++++----------- python/evalio/datasets/base.py | 9 ++++++--- python/evalio/datasets/loaders.py | 4 ++-- python/evalio/rerun.py | 13 +++++++------ 7 files changed, 38 insertions(+), 24 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 263a851..7b0b7df 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -41,6 +41,14 @@ build-backend = "scikit_build_core.build" evalio = "evalio.cli:app" # -------------- Tools -------------- # +[tool.uv] +cache-keys = [ + { file = "pyproject.toml" }, + { file = "cpp/**/*.{h,c,hpp,cpp}" }, + { file = "python/**/*.{cpp,py}" }, + { file = "CMakeLists.txt" }, +] + # building [tool.scikit-build] minimum-version = "0.12" @@ -49,6 +57,7 @@ sdist.include = ["cpp/bindings/pipelines-src/*"] [tool.scikit-build.cmake.define] EVALIO_BUILD_PYTHON = true +CMAKE_BUILD_TYPE = "RelWithDebInfo" VCPKG_INSTALLED_DIR = "./.vcpkg_installed" # VCPKG_INSTALL_OPTIONS = "--debug" diff --git a/python/evalio/cli/ls.py b/python/evalio/cli/ls.py index ac89602..9c3e6ce 100644 --- a/python/evalio/cli/ls.py +++ b/python/evalio/cli/ls.py @@ -142,7 +142,9 @@ def ls( ) else: # sequences - all_info["Sequences"].append("\n".join(d.sequences())) + all_info["Sequences"].append( + "\n".join([v.seq_name for v in d.sequences()]) + ) # downloaded downloaded = [d(s).is_downloaded() for s in d.sequences()] downloaded = "\n".join( diff --git a/python/evalio/cli/run.py b/python/evalio/cli/run.py index 5dd3919..816cdea 100644 --- a/python/evalio/cli/run.py +++ b/python/evalio/cli/run.py @@ -274,7 +274,9 @@ def run( # save ground truth if we haven't already if not (gt_file := exp.file.parent / "gt.csv").exists(): - exp.sequence.ground_truth().to_file(gt_file) + gt = exp.sequence.ground_truth() + if gt is not None: + gt.to_file(gt_file) # Figure out the status of the experiment traj = ty.Trajectory.from_file(exp.file) diff --git a/python/evalio/cli/stats.py b/python/evalio/cli/stats.py index 3509b71..4215662 100644 --- a/python/evalio/cli/stats.py +++ b/python/evalio/cli/stats.py @@ -43,10 +43,6 @@ def eval_dataset( elif isinstance(traj.metadata, ty.Experiment): all_trajs.append(cast(ty.Trajectory[ty.Experiment], traj)) - if gt_og is None: - print_warning(f"No ground truth found in {dir}, skipping.") - return None - # Setup visualization if visualize: try: @@ -67,11 +63,12 @@ def eval_dataset( spawn=False, ) rr.connect_grpc() - rr.log( - "gt", - convert(gt_og, color=GT_COLOR), - static=True, - ) + if gt_og is not None: + rr.log( + "gt", + convert(gt_og, color=GT_COLOR), + static=True, + ) # generate colors for visualization colors = distinctipy.get_colors(len(all_trajs) + 1) @@ -86,7 +83,7 @@ def eval_dataset( if traj.metadata.total_elapsed is not None: hz = len(traj) / traj.metadata.total_elapsed - if len(traj) > 0: + if len(traj) > 0 and gt_og is not None: # align to ground truth, copying ground truth by hand gt_aligned = Trajectory( stamps=[ty.Stamp(s) for s in gt_og.stamps], @@ -296,7 +293,7 @@ def evaluate_typer( else: from asteval import Interpreter - filter_method = lambda r: Interpreter(user_symbols=r).eval( + filter_method = lambda r: Interpreter(user_symbols=r).eval( # type: ignore filter_str, raise_errors=True ) diff --git a/python/evalio/datasets/base.py b/python/evalio/datasets/base.py index adc7249..79ec1aa 100644 --- a/python/evalio/datasets/base.py +++ b/python/evalio/datasets/base.py @@ -84,14 +84,14 @@ def data_iter(self) -> DatasetIterator: ... # Return the ground truth in the ground truth frame - def ground_truth_raw(self) -> Trajectory: + def ground_truth_raw(self) -> Optional[Trajectory]: """ Retrieves the raw ground truth trajectory, as represented in the ground truth frame. Returns: The raw ground truth trajectory data. """ - ... + return None # ------------------------- For loading params ------------------------- # def imu_T_lidar(self) -> SE3: @@ -210,13 +210,16 @@ def is_downloaded(self) -> bool: return True - def ground_truth(self) -> Trajectory[GroundTruth]: + def ground_truth(self) -> Optional[Trajectory[GroundTruth]]: """Get the ground truth trajectory in the **IMU** frame, rather than the ground truth frame as returned in [ground_truth_raw][evalio.datasets.Dataset.ground_truth_raw]. Returns: The ground truth trajectory in the IMU frame. """ gt_traj = self.ground_truth_raw() + if gt_traj is None: + return None + gt_T_imu = self.imu_T_gt().inverse() # Convert to IMU frame diff --git a/python/evalio/datasets/loaders.py b/python/evalio/datasets/loaders.py index e672552..2a8eebb 100644 --- a/python/evalio/datasets/loaders.py +++ b/python/evalio/datasets/loaders.py @@ -368,8 +368,8 @@ def _step(iter: Iterator[T]) -> Optional[T]: return None def __iter__(self) -> Iterator[Measurement]: - self.next_imu = next(self.iter_imu) - self.next_lidar = next(self.iter_lidar) + self.next_imu = self._step(self.iter_imu) + self.next_lidar = self._step(self.iter_lidar) return self def __next__(self) -> Measurement: diff --git a/python/evalio/rerun.py b/python/evalio/rerun.py index 60f5455..fa79e2e 100644 --- a/python/evalio/rerun.py +++ b/python/evalio/rerun.py @@ -127,7 +127,8 @@ def new_dataset(self, dataset: Dataset): self.lidar_params = dataset.lidar_params() self.imu_T_lidar = dataset.imu_T_lidar() - self.rec.log("gt", convert(self.gt, color=GT_COLOR), static=True) + if self.gt is not None: + self.rec.log("gt", convert(self.gt, color=GT_COLOR), static=True) # reset other variables self.pn = None @@ -165,7 +166,7 @@ def log_scan(self, data: LidarMeasurement): if self.args is None: return - if self.lidar_params is None or self.gt is None: + if self.lidar_params is None: raise ValueError("You needed to add a dataset before stepping!") if self.pn is None or self.trajectory is None or self.colors is None: raise ValueError("You needed to add a pipeline before stepping!") @@ -192,13 +193,13 @@ def log_pose(self, stamp: Stamp, pose: SE3): if self.args is None: return - if self.lidar_params is None or self.gt is None: + if self.lidar_params is None: raise ValueError("You needed to add a dataset before stepping!") if self.pn is None or self.trajectory is None or self.colors is None: raise ValueError("You needed to add a pipeline before stepping!") # Find transform between ground truth and imu origins - if self.gt_o_T_imu_o is None: + if self.gt_o_T_imu_o is None and self.gt is not None: # If we haven't hit ground truth data or the pose isn't being updated, wait if stamp < self.gt.stamps[0] or pose == SE3.identity(): pass @@ -231,7 +232,7 @@ def log_map(self, stamp: Stamp, map: dict[str, NDArray[np.float64]]): if self.args is None: return - if self.lidar_params is None or self.gt is None: + if self.lidar_params is None: raise ValueError("You needed to add a pipeline before stepping!") if self.pn is None or self.trajectory is None or self.colors is None: raise ValueError("You needed to add a pipeline before stepping!") @@ -247,7 +248,7 @@ def log_features(self, stamp: Stamp, features: dict[str, NDArray[np.float64]]): if self.args is None: return - if self.lidar_params is None or self.gt is None: + if self.lidar_params is None: raise ValueError("You needed to add a pipeline before stepping!") if self.pn is None or self.trajectory is None or self.colors is None: raise ValueError("You needed to add a pipeline before stepping!")