From f8cd513474162fce7d39b7d59e1291524a372cd3 Mon Sep 17 00:00:00 2001 From: MichalPelka Date: Thu, 22 Oct 2026 00:57:41 +0200 Subject: [PATCH 1/2] Add script to resample imu, and load other imus Signed-off-by: MichalPelka --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 30 +++ scripts/README.md | 8 + scripts/resample_imu.py | 212 ++++++++++++++++++ 3 files changed, 250 insertions(+) create mode 100755 scripts/resample_imu.py diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index f30488dd..0b1c9278 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -243,6 +243,36 @@ bool load_data( for (auto& chunk : imuChunks) imu_data.insert(imu_data.end(), std::make_move_iterator(chunk.begin()), std::make_move_iterator(chunk.end())); + // Units with an external IMU (e.g. Murata) leave 'imu****.csv' header-only and log the IMU + // into a subfolder, in files not aligned with the LiDAR chunks + const fs::path murataImuDir = fs::path(working_directory) / "MURATA_IMU"; + if (imu_data.empty() && fs::is_directory(murataImuDir)) + { + std::vector murataFiles; + for (const auto& entry : fs::directory_iterator(murataImuDir)) + { + std::string ext = entry.path().extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + if (entry.is_regular_file() && ext == ".csv" && entry.file_size() > 0) + murataFiles.push_back(entry.path().string()); + } + std::sort(murataFiles.begin(), murataFiles.end()); + std::cout << "no IMU data in 'imu****.csv', loading " << murataFiles.size() << " file(s) from '" << murataImuDir.string() + << "' ..\n"; + + std::vector murataChunks(murataFiles.size()); + tbb::parallel_for( + size_t(0), + murataFiles.size(), + [&](size_t i) + { + murataChunks[i] = load_imu(murataFiles[i], imuNumberToUse); + }); + for (auto& chunk : murataChunks) + imu_data.insert(imu_data.end(), std::make_move_iterator(chunk.begin()), std::make_move_iterator(chunk.end())); + + } + // Sort once after merging std::sort( imu_data.begin(), diff --git a/scripts/README.md b/scripts/README.md index 93a7ba7e..7aef4409 100644 --- a/scripts/README.md +++ b/scripts/README.md @@ -23,3 +23,11 @@ * reports whether the submodule was actually updated. Submodules without a branch configured in `.gitmodules` are reported and skipped. + +# Data utilities + +* `resample_imu.py` — resamples Mandeye IMU CSV files to a lower rate (default 200 Hz) by averaging all samples in each 1/rate window, so vibration above the new Nyquist rate is attenuated instead of aliased. Handles `imu****.csv` and external-IMU subfolders such as `MURATA_IMU/`. Given a scan folder, it writes a copy (`_imu200hz` by default; other files hard-linked when possible) with the IMU resampled, leaving the input untouched: + + ``` + python3 scripts/resample_imu.py /path/to/continousScanning_0013 --rate 200 + ``` diff --git a/scripts/resample_imu.py b/scripts/resample_imu.py new file mode 100755 index 00000000..04719e44 --- /dev/null +++ b/scripts/resample_imu.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python3 +"""Resample Mandeye IMU CSV files to a lower rate by averaging. + +Every sample inside a 1/rate window is averaged (a box filter followed by +decimation), so vibration above the new Nyquist rate is attenuated instead of +aliased. Each output row is stamped with the mean time of its samples. + +Handles the Mandeye 'imu****.csv' files (columns: timestamp gyroX gyroY gyroZ +accX accY accZ imuId timestampUnix) and the external IMU files in +'MURATA_IMU/' (same columns without imuId). Files of one series are treated +as a single stream, so windows are not cut at file boundaries. + +Usage: + resample_imu.py [-o ] [--rate 200] + resample_imu.py [ ...] -o [--rate 200] + +For a scan folder the output is a copy of the raw scan (top-level files and IMU +subfolders; other files hard-linked when possible, copied otherwise) with the +IMU files resampled; the input is never modified. +""" + +import argparse +import os +import shutil +import sys +from pathlib import Path + +TIME_COLUMNS = ("timestamp", "timestampUnix") +ID_COLUMN = "imuId" + + +def read_csv(path): + with open(path) as f: + header = f.readline().split() + rows = [line.split() for line in f if line.strip()] + return header, rows + + +def resample_series(files, rate_hz): + """Resample a list of CSV files forming one stream. + + Returns {path: (header, rows)} with each output row assigned to the file + that held the first sample of its window.""" + period_ns = int(round(1e9 / rate_hz)) + header = None + samples = [] # (timestamp_ns, file_index, row) + for i, path in enumerate(files): + h, rows = read_csv(path) + if header is None: + header = h + elif h != header: + sys.exit(f"error: '{path}' has columns {h}, expected {header}") + samples.extend((int(r[0]), i, r) for r in rows) + + out = {path: (header or [], []) for path in files} + if not samples: + return out + + ts_cols = [header.index(c) for c in TIME_COLUMNS if c in header] + id_col = header.index(ID_COLUMN) if ID_COLUMN in header else None + value_cols = [c for c in range(len(header)) if c not in ts_cols and c != id_col] + + # Group by IMU id (several IMUs may share one file), then by time window. + groups = {} + for s in samples: + imu_id = s[2][id_col] if id_col is not None else None + groups.setdefault(imu_id, []).append(s) + + for group in groups.values(): + group.sort(key=lambda s: s[0]) + start = 0 + while start < len(group): + window = group[start][0] // period_ns + end = start + while end < len(group) and group[end][0] // period_ns == window: + end += 1 + chunk = group[start:end] + n = len(chunk) + row = list(chunk[0][2]) + for c in ts_cols: + row[c] = str(sum(int(s[2][c]) for s in chunk) // n) + for c in value_cols: + row[c] = f"{sum(float(s[2][c]) for s in chunk) / n:.9g}" + out[files[chunk[0][1]]][1].append(row) + start = end + + for path in files: + out[path][1].sort(key=lambda r: int(r[0])) + return out + + +def input_rate(files): + """Sample rate per IMU, in Hz.""" + stamps = [] + ids = set() + for path in files: + header, rows = read_csv(path) + id_col = header.index(ID_COLUMN) if ID_COLUMN in header else None + stamps.extend(int(r[0]) for r in rows) + ids.update(r[id_col] for r in rows if id_col is not None) + if len(stamps) < 2 or stamps[0] == stamps[-1]: + return None + stamps.sort() + return (len(stamps) - 1) / max(len(ids), 1) / ((stamps[-1] - stamps[0]) / 1e9) + + +def write_csv(path, header, rows): + path.parent.mkdir(parents=True, exist_ok=True) + with open(path, "w") as f: + f.write(" ".join(header) + "\n") + for r in rows: + f.write(" ".join(r) + "\n") + + +def link_or_copy(src, dst): + dst.parent.mkdir(parents=True, exist_ok=True) + try: + os.link(src, dst) + except OSError: + shutil.copy2(src, dst) + + +def find_series(folder): + """IMU series in a scan folder: the 'imu****.csv' files and each + subfolder's CSV files (e.g. MURATA_IMU).""" + series = [] + mandeye = sorted(p for p in folder.glob("imu*.csv") if p.is_file()) + if mandeye: + series.append(mandeye) + for sub in sorted(p for p in folder.iterdir() if p.is_dir()): + csvs = sorted(p for p in sub.glob("*.csv") if p.is_file()) + if csvs and read_csv(csvs[0])[0][:1] == ["timestamp"]: + series.append(csvs) + return series + + +def process(series, rate_hz, out_path_for): + for files in series: + rate_in = input_rate(files) + name = files[0].parent.name if len(files) == 1 else f"{files[0].parent.name}/{files[0].name}.." + if rate_in is None: + print(f"{name}: no data, copied as is") + for f in files: + link_or_copy(f, out_path_for(f)) + continue + if rate_in <= rate_hz * 1.05: + print(f"{name}: {rate_in:.0f} Hz is not above {rate_hz} Hz, copied as is") + for f in files: + link_or_copy(f, out_path_for(f)) + continue + result = resample_series(files, rate_hz) + n_in = n_out = 0 + for f in files: + header, rows = result[f] + n_in += len(read_csv(f)[1]) + n_out += len(rows) + write_csv(out_path_for(f), header, rows) + print(f"{name}: {len(files)} file(s), {rate_in:.0f} Hz -> {rate_hz} Hz, {n_in} -> {n_out} rows") + + +def main(): + parser = argparse.ArgumentParser(description="Resample Mandeye IMU CSV files to a lower rate by averaging.") + parser.add_argument("inputs", nargs="+", type=Path, help="scan folder, or IMU CSV files") + parser.add_argument("-o", "--output", type=Path, help="output folder (default for a scan folder: _imuhz)") + parser.add_argument("--rate", type=float, default=200.0, help="output rate in Hz (default: 200)") + args = parser.parse_args() + + if args.rate <= 0: + sys.exit("error: --rate must be positive") + rate_hz = int(args.rate) if args.rate.is_integer() else args.rate + + if len(args.inputs) == 1 and args.inputs[0].is_dir(): + src = args.inputs[0].resolve() + dst = (args.output or src.with_name(f"{src.name}_imu{rate_hz}hz")).resolve() + if dst == src or src in dst.parents: + sys.exit("error: output folder must not be the scan folder or inside it") + if dst.exists() and any(dst.iterdir()): + sys.exit(f"error: output folder '{dst}' exists and is not empty") + series = find_series(src) + if not series: + sys.exit(f"error: no IMU CSV files found in '{src}'") + # Copy the raw scan only: top-level files and the IMU subfolders. Other + # subfolders (lio_result_*, preview, cache, ...) are results of earlier runs. + imu_files = {f for s in series for f in s} + imu_dirs = {f.parent for f in imu_files} - {src} + for f in sorted(src.iterdir()): + if f.is_file() and f not in imu_files: + link_or_copy(f, dst / f.name) + elif f.is_dir() and f not in imu_dirs: + print(f"skipped folder: {f.name}") + for d in imu_dirs: + for f in d.iterdir(): + if f.is_file() and f not in imu_files: + link_or_copy(f, dst / f.relative_to(src)) + process(series, rate_hz, lambda f: dst / f.relative_to(src)) + print(f"output: {dst}") + else: + if not args.output: + sys.exit("error: -o/--output is required when resampling individual files") + files = sorted(p.resolve() for p in args.inputs) + for f in files: + if not f.is_file(): + sys.exit(f"error: '{f}' is not a file") + dst = args.output.resolve() + if any(f.parent == dst for f in files): + sys.exit("error: output folder must differ from the input files' folder") + process([files], rate_hz, lambda f: dst / f.name) + print(f"output: {dst}") + + +if __name__ == "__main__": + main() From 00b8dbc9d2068545bb8020b2790a54bbd3d1764a Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Thu, 22 Oct 2026 12:09:06 +0200 Subject: [PATCH 2/2] Make clang happy Co-Authored-By: Claude Opus 5.5 --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 0b1c9278..1d8af25a 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -270,7 +270,6 @@ bool load_data( }); for (auto& chunk : murataChunks) imu_data.insert(imu_data.end(), std::make_move_iterator(chunk.begin()), std::make_move_iterator(chunk.end())); - } // Sort once after merging