新增独立RTK与IMU外参标定流程及质量验证

imu_lidar/imu_io.py

@@ -15,6 +15,7 @@ Accepted inputs
 
 from __future__ import annotations
 
+import csv
 from pathlib import Path
 
 import numpy as np
@@ -36,16 +37,17 @@ def load_imu_samples(path: Path | str) -> ImuSeries:
 
 
 def _load_imu_csv(path: Path) -> ImuSeries:
-    data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
-    if data.ndim == 0:
-        data = np.array([data])
-    names = set(data.dtype.names or ())
-    required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
-    if not required.issubset(names):
-        raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
-    t = np.asarray(data["t"], dtype=float).reshape(-1)
-    gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
-    acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
+    required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
+    with path.open("r", encoding="utf-8-sig", newline="") as handle:
+        header = next(csv.reader(handle), [])
+    names = set(header)
+    if not set(required_order).issubset(names):
+        raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
+    usecols = [header.index(name) for name in required_order]
+    data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
+    t = np.asarray(data[:, 0], dtype=float).reshape(-1)
+    gyro = np.asarray(data[:, 1:4], dtype=float)
+    acc = np.asarray(data[:, 4:7], dtype=float)
     order = np.argsort(t)
     return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])