diff --git a/.vs/VSWorkspaceState.json b/.vs/VSWorkspaceState.json new file mode 100644 index 0000000..6b61141 --- /dev/null +++ b/.vs/VSWorkspaceState.json @@ -0,0 +1,6 @@ +{ + "ExpandedNodes": [ + "" + ], + "PreviewInSolutionExplorer": false +} \ No newline at end of file diff --git a/.vs/calibration.slnx/FileContentIndex/33d5e770-e14b-479c-8821-ac6f5c24ea5b.vsidx b/.vs/calibration.slnx/FileContentIndex/33d5e770-e14b-479c-8821-ac6f5c24ea5b.vsidx new file mode 100644 index 0000000..e7f24f2 Binary files /dev/null and b/.vs/calibration.slnx/FileContentIndex/33d5e770-e14b-479c-8821-ac6f5c24ea5b.vsidx differ diff --git a/.vs/calibration.slnx/FileContentIndex/611329d1-80e1-4347-8ce7-90e2b57949ad.vsidx b/.vs/calibration.slnx/FileContentIndex/611329d1-80e1-4347-8ce7-90e2b57949ad.vsidx new file mode 100644 index 0000000..a5c7a50 Binary files /dev/null and b/.vs/calibration.slnx/FileContentIndex/611329d1-80e1-4347-8ce7-90e2b57949ad.vsidx differ diff --git a/.vs/calibration.slnx/FileContentIndex/ea4ee519-a261-44d7-a9a5-6d2017bce2f3.vsidx b/.vs/calibration.slnx/FileContentIndex/ea4ee519-a261-44d7-a9a5-6d2017bce2f3.vsidx new file mode 100644 index 0000000..5b6e2d2 Binary files /dev/null and b/.vs/calibration.slnx/FileContentIndex/ea4ee519-a261-44d7-a9a5-6d2017bce2f3.vsidx differ diff --git a/.vs/calibration.slnx/FileContentIndex/f941beb5-8fe3-4823-a751-d16b10b542b1.vsidx b/.vs/calibration.slnx/FileContentIndex/f941beb5-8fe3-4823-a751-d16b10b542b1.vsidx new file mode 100644 index 0000000..691dd1f Binary files /dev/null and b/.vs/calibration.slnx/FileContentIndex/f941beb5-8fe3-4823-a751-d16b10b542b1.vsidx differ diff --git a/.vs/calibration.slnx/v18/.wsuo b/.vs/calibration.slnx/v18/.wsuo new file mode 100644 index 0000000..4f4df3f Binary files /dev/null and b/.vs/calibration.slnx/v18/.wsuo differ diff --git a/.vs/calibration.slnx/v18/DocumentLayout.backup.json b/.vs/calibration.slnx/v18/DocumentLayout.backup.json new file mode 100644 index 0000000..718aca0 --- /dev/null +++ b/.vs/calibration.slnx/v18/DocumentLayout.backup.json @@ -0,0 +1,49 @@ +{ + "Version": 1, + "WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\", + "Documents": [ + { + "AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}", + "RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}" + } + ], + "DocumentGroupContainers": [ + { + "Orientation": 0, + "VerticalTabListWidth": 256, + "DocumentGroups": [ + { + "DockedWidth": 200, + "SelectedChildIndex": 3, + "Children": [ + { + "$type": "Bookmark", + "Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}" + }, + { + "$type": "Bookmark", + "Name": "ST:0:0:{1c4feeaa-4718-4aa9-859d-94ce25d182ba}" + }, + { + "$type": "Bookmark", + "Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}" + }, + { + "$type": "Document", + "DocumentIndex": 0, + "Title": "README.md", + "DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md", + "RelativeDocumentMoniker": "README.md", + "ToolTip": "D:\\First-dev-dept\\calibration\\README.md", + "RelativeToolTip": "README.md", + "ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==", + "Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|", + "WhenOpened": "2026-07-24T03:20:21.56Z", + "EditorCaption": "" + } + ] + } + ] + } + ] +} \ No newline at end of file diff --git a/.vs/calibration.slnx/v18/DocumentLayout.json b/.vs/calibration.slnx/v18/DocumentLayout.json new file mode 100644 index 0000000..5ffab23 --- /dev/null +++ b/.vs/calibration.slnx/v18/DocumentLayout.json @@ -0,0 +1,45 @@ +{ + "Version": 1, + "WorkspaceRootPath": "D:\\First-dev-dept\\calibration\\", + "Documents": [ + { + "AbsoluteMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|D:\\First-dev-dept\\calibration\\README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}", + "RelativeMoniker": "D:0:0:{A2FE74E1-B743-11D0-AE1A-00A0C90FFFC3}|\u003CMiscFiles\u003E|solutionrelative:README.md||{EFC0BB08-EA7D-40C6-A696-C870411A895B}" + } + ], + "DocumentGroupContainers": [ + { + "Orientation": 0, + "VerticalTabListWidth": 256, + "DocumentGroups": [ + { + "DockedWidth": 200, + "SelectedChildIndex": 2, + "Children": [ + { + "$type": "Bookmark", + "Name": "ST:0:0:{3ae79031-e1bc-11d0-8f78-00a0c9110057}" + }, + { + "$type": "Bookmark", + "Name": "ST:128:0:{116d2292-e37d-41cd-a077-ebacac4c8cc4}" + }, + { + "$type": "Document", + "DocumentIndex": 0, + "Title": "README.md", + "DocumentMoniker": "D:\\First-dev-dept\\calibration\\README.md", + "RelativeDocumentMoniker": "README.md", + "ToolTip": "D:\\First-dev-dept\\calibration\\README.md", + "RelativeToolTip": "README.md", + "ViewState": "AgIAADgAAAAAAAAAAAAAABIAAAAAAAAAAAAAAA==", + "Icon": "ae27a6b0-e345-4288-96df-5eaf394ee369.001818|", + "WhenOpened": "2026-07-24T03:20:21.56Z", + "EditorCaption": "" + } + ] + } + ] + } + ] +} \ No newline at end of file diff --git a/.vs/slnx.sqlite b/.vs/slnx.sqlite new file mode 100644 index 0000000..1ec0e08 Binary files /dev/null and b/.vs/slnx.sqlite differ diff --git a/ScreenCamera_2026-07-24-09-32-29.json b/ScreenCamera_2026-07-24-09-32-29.json new file mode 100644 index 0000000..1f1b429 --- /dev/null +++ b/ScreenCamera_2026-07-24-09-32-29.json @@ -0,0 +1,41 @@ +{ + "class_name" : "PinholeCameraParameters", + "extrinsic" : + [ + 0.99504759165761958, + -0.069911734310665122, + 0.070658614068252801, + 0.0, + -0.09933361974813136, + -0.67348569005019454, + 0.73249563635925308, + 0.0, + -0.0036224748591220518, + -0.7358867947607759, + -0.67709489953226409, + 0.0, + 2.6365670299167046, + 1.9293765342813529, + 11.882725892422524, + 1.0 + ], + "intrinsic" : + { + "height" : 900, + "intrinsic_matrix" : + [ + 779.4228634059948, + 0.0, + 0.0, + 0.0, + 779.4228634059948, + 0.0, + 699.5, + 449.5, + 1.0 + ], + "width" : 1400 + }, + "version_major" : 1, + "version_minor" : 0 +} \ No newline at end of file diff --git a/ScreenCamera_2026-07-24-09-32-30.json b/ScreenCamera_2026-07-24-09-32-30.json new file mode 100644 index 0000000..1f1b429 --- /dev/null +++ b/ScreenCamera_2026-07-24-09-32-30.json @@ -0,0 +1,41 @@ +{ + "class_name" : "PinholeCameraParameters", + "extrinsic" : + [ + 0.99504759165761958, + -0.069911734310665122, + 0.070658614068252801, + 0.0, + -0.09933361974813136, + -0.67348569005019454, + 0.73249563635925308, + 0.0, + -0.0036224748591220518, + -0.7358867947607759, + -0.67709489953226409, + 0.0, + 2.6365670299167046, + 1.9293765342813529, + 11.882725892422524, + 1.0 + ], + "intrinsic" : + { + "height" : 900, + "intrinsic_matrix" : + [ + 779.4228634059948, + 0.0, + 0.0, + 0.0, + 779.4228634059948, + 0.0, + 699.5, + 449.5, + 1.0 + ], + "width" : 1400 + }, + "version_major" : 1, + "version_minor" : 0 +} \ No newline at end of file diff --git a/data/data5/RTKIMUraw.rar b/data/data5/RTKIMUraw.rar new file mode 100644 index 0000000..501bbc8 Binary files /dev/null and b/data/data5/RTKIMUraw.rar differ diff --git a/data/motiondata/imurtk_raw.rar b/data/motiondata/imurtk_raw.rar new file mode 100644 index 0000000..a1c7417 Binary files /dev/null and b/data/motiondata/imurtk_raw.rar differ diff --git a/tools/build_slam_delivery.py b/tools/build_slam_delivery.py new file mode 100644 index 0000000..a4a17e3 --- /dev/null +++ b/tools/build_slam_delivery.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 +"""Build LiDAR GT/quality tables for a continuous LiDAR + dual-RTK + IMU run.""" + +from __future__ import annotations + +import argparse +import csv +import datetime as dt +import json +import math +from pathlib import Path +from typing import Any + +import numpy as np + + +def args() -> argparse.Namespace: + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--lidar-manifest", type=Path, required=True) + p.add_argument("--rtk-jsonl", type=Path, required=True) + p.add_argument("--imu-jsonl", type=Path, required=True) + p.add_argument("--extrinsic", type=Path, required=True) + p.add_argument("--out", type=Path, required=True) + p.add_argument("--max-bracket-ms", type=float, default=150.0) + p.add_argument("--heading-std-limit-deg", type=float, default=0.5) + return p.parse_args() + + +def read_jsonl(path: Path) -> list[dict[str, Any]]: + with path.open(encoding="utf-8") as f: + return [json.loads(line) for line in f if line.strip()] + + +def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray: + a, e2 = 6378137.0, 6.69437999014e-3 + lat, lon = math.radians(lat_deg), math.radians(lon_deg) + slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon) + n = a / math.sqrt(1.0 - e2 * slat * slat) + return np.array([(n + height_m) * clat * clon, + (n + height_m) * clat * slon, + (n * (1.0 - e2) + height_m) * slat], dtype=float) + + +def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: float) -> np.ndarray: + lat, lon = math.radians(lat_deg), math.radians(lon_deg) + slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon) + r = np.array([[-slon, clon, 0.0], + [-slat * clon, -slat * slon, clat], + [clat * clon, clat * slon, slat]], dtype=float) + return r @ (ecef - origin) + + +def yaw_matrix(yaw_rad: float) -> np.ndarray: + c, s = math.cos(yaw_rad), math.sin(yaw_rad) + return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float) + + +def matrix_to_quat_xyzw(r: np.ndarray) -> np.ndarray: + # Stable branch-based conversion; output convention is x,y,z,w. + tr = float(np.trace(r)) + if tr > 0.0: + s = math.sqrt(tr + 1.0) * 2.0 + q = np.array([(r[2, 1] - r[1, 2]) / s, + (r[0, 2] - r[2, 0]) / s, + (r[1, 0] - r[0, 1]) / s, 0.25 * s]) + else: + i = int(np.argmax(np.diag(r))) + if i == 0: + s = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0 + q = np.array([0.25 * s, (r[0, 1] + r[1, 0]) / s, + (r[0, 2] + r[2, 0]) / s, (r[2, 1] - r[1, 2]) / s]) + elif i == 1: + s = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0 + q = np.array([(r[0, 1] + r[1, 0]) / s, 0.25 * s, + (r[1, 2] + r[2, 1]) / s, (r[0, 2] - r[2, 0]) / s]) + else: + s = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0 + q = np.array([(r[0, 2] + r[2, 0]) / s, + (r[1, 2] + r[2, 1]) / s, 0.25 * s, + (r[1, 0] - r[0, 1]) / s]) + if q[3] < 0.0: + q = -q + return q / np.linalg.norm(q) + + +def bracket(rows: list[dict[str, Any]], times: np.ndarray, t: int, + max_ns: int) -> tuple[dict[str, Any], dict[str, Any], float] | None: + right = int(np.searchsorted(times, t, side="left")) + if right == 0 or right >= len(times): + return None + left = right - 1 + t0, t1 = int(times[left]), int(times[right]) + if t1 <= t0 or t - t0 > max_ns or t1 - t > max_ns: + return None + return rows[left], rows[right], (t - t0) / (t1 - t0) + + +def circular_lerp_deg(a: float, b: float, u: float) -> float: + delta = (b - a + 180.0) % 360.0 - 180.0 + return (a + u * delta) % 360.0 + + +def iso_utc(ns: int) -> str: + return dt.datetime.fromtimestamp(ns / 1e9, dt.timezone.utc).isoformat(timespec="microseconds") + + +def write_imu_csv(rows: list[dict[str, Any]], path: Path) -> None: + fields = [ + "host_receive_utc_ns", "device_timestamp_ms", "pps_sync_stamp_ms", "crc_valid", + "accel_x_mps2", "accel_y_mps2", "accel_z_mps2", + "gyro_x_radps", "gyro_y_radps", "gyro_z_radps", + "mag_x_ut", "mag_y_ut", "mag_z_ut", "temperature_c", "air_pressure_pa", + "roll_deg", "pitch_deg", "yaw_deg", + "quaternion_x", "quaternion_y", "quaternion_z", "quaternion_w", + "source_chunk_sequence_first", "source_raw_file_offset", + ] + with path.open("w", encoding="utf-8", newline="") as f: + w = csv.DictWriter(f, fieldnames=fields) + w.writeheader() + for row in rows: + w.writerow({key: row.get(key) for key in fields}) + + +def main() -> int: + a = args() + a.out.mkdir(parents=True, exist_ok=True) + with a.lidar_manifest.open(encoding="utf-8-sig", newline="") as f: + lidar = [row for row in csv.DictReader(f) if not row.get("error")] + rtk = read_jsonl(a.rtk_jsonl) + imu = [row for row in read_jsonl(a.imu_jsonl) if row.get("crc_valid")] + gga = sorted([r for r in rtk if r.get("type") == "GGA" and r.get("checksum_valid") + and r.get("lat_deg") is not None], key=lambda r: int(r["host_receive_utc_ns"])) + heading = sorted([r for r in rtk if r.get("type") == "UNIHEADINGA" and r.get("checksum_valid") + and r.get("heading_valid") and r.get("raw_heading_deg") is not None], + key=lambda r: int(r["host_receive_utc_ns"])) + if not lidar or len(gga) < 2 or len(heading) < 2: + raise RuntimeError("insufficient LiDAR/GGA/heading data") + + ext = json.loads(a.extrinsic.read_text(encoding="utf-8")) + t_r_l = np.asarray(ext["matrix_4x4"], dtype=float) + if t_r_l.shape != (4, 4): + raise ValueError("extrinsic matrix_4x4 must be 4x4") + + gga_times = np.asarray([int(r["host_receive_utc_ns"]) for r in gga], dtype=np.int64) + heading_times = np.asarray([int(r["host_receive_utc_ns"]) for r in heading], dtype=np.int64) + origin_row = next(r for r in gga if int(r.get("fix_quality", -1)) == 4) + origin_lat, origin_lon, origin_alt = (float(origin_row[k]) for k in ("lat_deg", "lon_deg", "altitude_m")) + origin_ecef = geodetic_to_ecef(origin_lat, origin_lon, origin_alt) + max_ns = int(a.max_bracket_ms * 1_000_000) + pose_rows: list[dict[str, Any]] = [] + + for index, frame in enumerate(lidar): + t = int(frame["unix_time_ns"]) + gb, hb = bracket(gga, gga_times, t, max_ns), bracket(heading, heading_times, t, max_ns) + reasons: list[str] = [] + available = gb is not None and hb is not None + row: dict[str, Any] = { + "frame_index": index, "lidar_time_ns": t, "lidar_time_utc": iso_utc(t), + "lidar_file": frame["output_file"], "point_count": frame["point_count"], + "pose_available": int(available), "gt_valid": 0, "invalid_reason": "", + } + if not available: + if gb is None: reasons.append("GGA_NOT_BRACKETED") + if hb is None: reasons.append("HEADING_NOT_BRACKETED") + row.update({k: "" for k in ("x_m", "y_m", "z_m", "qx", "qy", "qz", "qw", + "rtk_x_m", "rtk_y_m", "rtk_z_m", "raw_heading_deg")}) + row["invalid_reason"] = ";".join(reasons) + pose_rows.append(row) + continue + + g0, g1, gu = gb + h0, h1, hu = hb + p0 = geodetic_to_ecef(float(g0["lat_deg"]), float(g0["lon_deg"]), float(g0["altitude_m"])) + p1 = geodetic_to_ecef(float(g1["lat_deg"]), float(g1["lon_deg"]), float(g1["altitude_m"])) + p_rtk = ecef_to_enu((1.0 - gu) * p0 + gu * p1, origin_ecef, origin_lat, origin_lon) + raw_heading = circular_lerp_deg(float(h0["raw_heading_deg"]), float(h1["raw_heading_deg"]), hu) + yaw = math.radians(90.0 - raw_heading) + t_w_r = np.eye(4) + t_w_r[:3, :3] = yaw_matrix(yaw) + t_w_r[:3, 3] = p_rtk + t_w_l = t_w_r @ t_r_l + q = matrix_to_quat_xyzw(t_w_l[:3, :3]) + + fix0, fix1 = int(g0.get("fix_quality", -1)), int(g1.get("fix_quality", -1)) + sol0, sol1 = str(h0.get("heading_solution", "")), str(h1.get("heading_solution", "")) + std0 = float(h0.get("heading_stddev_deg") or math.inf) + std1 = float(h1.get("heading_stddev_deg") or math.inf) + if fix0 != 4 or fix1 != 4: reasons.append("RTK_POSITION_NOT_FIXED") + if sol0 != "NARROW_INT" or sol1 != "NARROW_INT": reasons.append("HEADING_NOT_NARROW_INT") + if max(std0, std1) > a.heading_std_limit_deg: reasons.append("HEADING_STD_EXCEEDED") + row.update({ + "gt_valid": int(not reasons), "invalid_reason": ";".join(reasons), + "x_m": t_w_l[0, 3], "y_m": t_w_l[1, 3], "z_m": t_w_l[2, 3], + "qx": q[0], "qy": q[1], "qz": q[2], "qw": q[3], + "rtk_x_m": p_rtk[0], "rtk_y_m": p_rtk[1], "rtk_z_m": p_rtk[2], + "raw_heading_deg": raw_heading, "yaw_enu_deg": math.degrees(yaw), + "gga_fix_before": fix0, "gga_fix_after": fix1, + "heading_solution_before": sol0, "heading_solution_after": sol1, + "heading_std_max_deg": max(std0, std1), + "gga_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6, + "gga_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6, + "heading_before_dt_ms": (t - int(h0["host_receive_utc_ns"])) / 1e6, + "heading_after_dt_ms": (int(h1["host_receive_utc_ns"]) - t) / 1e6, + }) + pose_rows.append(row) + + fields = list(dict.fromkeys(k for row in pose_rows for k in row)) + pose_path = a.out / "lidar_gt_pose_enu.csv" + with pose_path.open("w", encoding="utf-8", newline="") as f: + w = csv.DictWriter(f, fieldnames=fields) + w.writeheader(); w.writerows(pose_rows) + write_imu_csv(imu, a.out / "imu_parsed.csv") + + summary = { + "coordinate_convention": "T_W_L maps raw LiDAR points to local ENU; T_W_L = T_W_RTK @ T_RTK_lidar", + "world_frame": "local ENU, origin is the first RTK FIX GGA sample", + "rtk_frame": "x is rawHeading baseline direction projected horizontally, y left, z up", + "orientation_model": "RTK pose is yaw-only; IMU orientation is not fused", + "time_basis": "LiDAR and serial host UTC; no jointly estimated clock offset/drift", + "lidar_frames": len(pose_rows), + "pose_available_frames": sum(int(r["pose_available"]) for r in pose_rows), + "gt_valid_frames": sum(int(r["gt_valid"]) for r in pose_rows), + "gt_invalid_frames": sum(not int(r["gt_valid"]) for r in pose_rows), + "imu_frames": len(imu), + "enu_origin": {"lat_deg": origin_lat, "lon_deg": origin_lon, "altitude_m": origin_alt}, + "quality_rule": "GGA endpoints fix_quality=4, heading endpoints NARROW_INT, heading std <= limit, both streams bracket LiDAR time", + "heading_std_limit_deg": a.heading_std_limit_deg, + "max_bracket_ms": a.max_bracket_ms, + "warning": "gt_valid is a quality gate, not independent proof of +/-3 cm absolute accuracy", + } + (a.out / "delivery_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8") + print(json.dumps(summary, ensure_ascii=False, indent=2)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) + + diff --git a/tools/convert_lidar_npz_to_xyz.py b/tools/convert_lidar_npz_to_xyz.py new file mode 100644 index 0000000..bc92d27 --- /dev/null +++ b/tools/convert_lidar_npz_to_xyz.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +"""Convert exported LiDAR polar NPZ frames to portable XYZ-in-metres NPZ frames.""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import numpy as np + + +def main() -> int: + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--input", type=Path, required=True) + p.add_argument("--output", type=Path, required=True) + p.add_argument("--overwrite", action="store_true") + a = p.parse_args() + sources = sorted(a.input.glob("*.npz")) + if not sources: + raise FileNotFoundError(f"no NPZ frames in {a.input}") + a.output.mkdir(parents=True, exist_ok=True) + written = skipped = 0 + for index, source in enumerate(sources, 1): + target = a.output / source.name + if target.exists() and not a.overwrite: + skipped += 1 + continue + with np.load(source, allow_pickle=False) as f: + raw = np.asarray(f["points_raw"], dtype=np.float32) + time_ns = np.asarray(f["unix_time_ns"], dtype=np.int64) + counter = np.asarray(f["frame_counter"], dtype=np.int32) + distance_m = raw[:, 0] * np.float32(0.001) + azimuth = np.deg2rad(raw[:, 1]) + altitude = np.deg2rad(raw[:, 2]) + cos_alt = np.cos(altitude) + xyz = np.column_stack((distance_m * cos_alt * np.cos(azimuth), + distance_m * cos_alt * np.sin(azimuth), + distance_m * np.sin(altitude))).astype(np.float32, copy=False) + np.savez_compressed(target, xyz_m=xyz, intensity=raw[:, 3].astype(np.float32, copy=False), + progression=raw[:, 4].astype(np.float32, copy=False), + unix_time_ns=time_ns, frame_counter=counter) + written += 1 + if index % 100 == 0 or index == len(sources): + print(f"[{index}/{len(sources)}] written={written} skipped={skipped}", flush=True) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())