lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

支持 HI13/H32 主机 UTC 桥接对齐、多会话联合标定与 CAD 平移先验。

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 13:26:32 +08:00
co-authored by Cursor
parent 30f7e66db3
commit 2237be77a4
20 changed files with 1830 additions and 347 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/export_rscap_to_v1.py
+212 -63
View File
@@ -1,13 +1,18 @@
#!/usr/bin/env python3
"""Export N300 IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
"""Export IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
Supported LiDAR sources (exactly one required):
IMU sources:
- ``--imu-kind hi13`` (HI13R4 / HI91) or ``n300`` or ``auto``
- one or more ``--imu-rscap`` files (concatenated)
- ``--lidar-dlog``: Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48``
(raw MSOP + DIFOP DObjects; preferred for new recordings)
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap`` (MSOP-only defaults for angles)
LiDAR sources (exactly one):
- ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP)
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap``
Output layout under --out:
Optional host-time window (local wall clock, DateTime.Now.Ticks convention):
- ``--host-start`` / ``--host-end`` e.g. ``2026-08-08T17:40:05``
Output under ``--out``:
imu.csv
lidar/
@@ -15,8 +20,9 @@ Output layout under --out:
frames/frame_XXXXX.npz
export_summary.json
Timestamps written into the intermediate format are **device times**
(N300 device_timestamp_us, H32 MSOP device timestamp), not host receive time.
Device times stay in ``t`` / ``t_start``/``t_end``. Host UTC receive times are
also written so LiDAR–IMU alignment can bridge clocks without forcing first-frame
device coincidence.
"""
from __future__ import annotations
@@ -34,26 +40,48 @@ if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from tools.h32_dlog.load_session import load_h32_dlog_lidar
from tools.h32_dlog.timeutil import (
local_wall_to_dotnet_ticks,
local_wall_to_utc_dotnet_ticks,
utc_dotnet_ticks_to_unix_s,
)
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
from tools.rscap_v2.h32_msop import iter_h32_frames, iter_h32_frames_from_packets
from tools.rscap_v2.n300_imu import iter_n300_imu_samples, samples_to_arrays
from tools.rscap_v2.hi13_imu import iter_hi13_imu_samples
from tools.rscap_v2.n300_imu import ImuSample, iter_n300_imu_samples, samples_to_arrays
def write_imu_csv(path: Path, t: np.ndarray, gyro: np.ndarray, accel: np.ndarray) -> None:
def write_imu_csv(path: Path, samples: list[ImuSample]) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle)
writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"])
for index in range(t.shape[0]):
writer.writerow(
[
"t",
"gx",
"gy",
"gz",
"ax",
"ay",
"az",
"t_host_utc_s",
"receive_utc_ticks",
]
)
for sample in samples:
ticks = int(sample.host_receive_utc_ticks)
t_host = utc_dotnet_ticks_to_unix_s(ticks) if ticks > 0 else float("nan")
writer.writerow(
[
f"{t[index]:.9f}",
f"{gyro[index, 0]:.12g}",
f"{gyro[index, 1]:.12g}",
f"{gyro[index, 2]:.12g}",
f"{accel[index, 0]:.12g}",
f"{accel[index, 1]:.12g}",
f"{accel[index, 2]:.12g}",
f"{sample.t_s:.9f}",
f"{sample.gyro_rad_s[0]:.12g}",
f"{sample.gyro_rad_s[1]:.12g}",
f"{sample.gyro_rad_s[2]:.12g}",
f"{sample.accel_m_s2[0]:.12g}",
f"{sample.accel_m_s2[1]:.12g}",
f"{sample.accel_m_s2[2]:.12g}",
f"{t_host:.9f}" if ticks > 0 else "",
ticks,
]
)
@@ -64,22 +92,45 @@ def write_lidar_session(root: Path, frames) -> dict:
index_path = root / "frames_index.csv"
with index_path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle)
writer.writerow(["frame_id", "filename", "t_start", "t_end"])
writer.writerow(
[
"frame_id",
"filename",
"t_start",
"t_end",
"host_receive_utc_ticks",
"t_host_utc_s",
"host_receive_utc_end_ticks",
"t_host_utc_end_s",
]
)
point_counts = []
host_ok = 0
for index, frame in enumerate(frames):
rel = f"frames/frame_{index:05d}.npz"
np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32))
h0 = int(getattr(frame, "host_receive_utc_ticks_start", 0) or 0)
h1 = int(getattr(frame, "host_receive_utc_ticks_end", 0) or 0)
t_host0 = utc_dotnet_ticks_to_unix_s(h0) if h0 > 0 else float("nan")
t_host1 = utc_dotnet_ticks_to_unix_s(h1) if h1 > 0 else float("nan")
if h0 > 0:
host_ok += 1
writer.writerow(
[
index,
rel,
f"{frame.t_start_s:.9f}",
f"{frame.t_end_s:.9f}",
h0,
f"{t_host0:.9f}" if h0 > 0 else "",
h1,
f"{t_host1:.9f}" if h1 > 0 else "",
]
)
point_counts.append(int(frame.points_xyz.shape[0]))
return {
"frames": len(frames),
"frames_with_host_utc": host_ok,
"points_min": int(min(point_counts)) if point_counts else 0,
"points_max": int(max(point_counts)) if point_counts else 0,
"points_mean": float(np.mean(point_counts)) if point_counts else 0.0,
@@ -88,15 +139,63 @@ def write_lidar_session(root: Path, frames) -> dict:
}
def detect_imu_kind(paths: list[Path], explicit: str) -> str:
if explicit != "auto":
return explicit
joined = " ".join(path.name.lower() for path in paths)
if "hi13" in joined or "hipnuc" in joined:
return "hi13"
if "n300" in joined or "wheeltec" in joined:
return "n300"
return "hi13"
def load_imu_samples(
paths: list[Path],
*,
kind: str,
host_ticks_min: int | None,
host_ticks_max: int | None,
) -> tuple[list[ImuSample], list[dict], str]:
samples: list[ImuSample] = []
captures_meta: list[dict] = []
for path in paths:
capture = read_capture(path)
captures_meta.append(file_summary(capture))
if kind == "hi13":
part = iter_hi13_imu_samples(
capture,
host_utc_ticks_min=host_ticks_min,
host_utc_ticks_max=host_ticks_max,
)
elif kind == "n300":
part = iter_n300_imu_samples(capture)
if host_ticks_min is not None or host_ticks_max is not None:
part = [
sample
for sample in part
if (host_ticks_min is None or sample.host_receive_utc_ticks >= host_ticks_min)
and (host_ticks_max is None or sample.host_receive_utc_ticks <= host_ticks_max)
]
else:
raise ValueError(f"unsupported imu kind: {kind}")
samples.extend(part)
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
return samples, captures_meta, kind
def export_session(
*,
imu_rscap: Path,
imu_rscap: list[Path] | Path,
out: Path,
lidar_rscap: Path | None = None,
lidar_dlog: Path | None = None,
imu_kind: str = "auto",
msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw",
require_difop: bool = False,
host_start: str | None = None,
host_end: str | None = None,
frame_stride: int = 1,
max_points_per_frame: int | None = 80000,
min_range_m: float = 0.3,
@@ -106,24 +205,41 @@ def export_session(
if (lidar_rscap is None) == (lidar_dlog is None):
raise ValueError("provide exactly one of lidar_rscap or lidar_dlog")
imu_paths = [imu_rscap] if isinstance(imu_rscap, Path) else list(imu_rscap)
if not imu_paths:
raise ValueError("at least one --imu-rscap is required")
# LiDAR DObject tic uses DateTime.Now; IMU/MSOP host fields use UTC.
lidar_ticks_min = local_wall_to_dotnet_ticks(host_start) if host_start else None
lidar_ticks_max = local_wall_to_dotnet_ticks(host_end) if host_end else None
imu_ticks_min = local_wall_to_utc_dotnet_ticks(host_start) if host_start else None
imu_ticks_max = local_wall_to_utc_dotnet_ticks(host_end) if host_end else None
kind = detect_imu_kind(imu_paths, imu_kind)
out.mkdir(parents=True, exist_ok=True)
imu_capture = read_capture(imu_rscap)
samples = iter_n300_imu_samples(imu_capture)
t, gyro, accel = samples_to_arrays(samples)
samples, imu_captures, kind = load_imu_samples(
imu_paths,
kind=kind,
host_ticks_min=imu_ticks_min,
host_ticks_max=imu_ticks_max,
)
t, _gyro, _accel = samples_to_arrays(samples)
imu_csv = out / "imu.csv"
write_imu_csv(imu_csv, t, gyro, accel)
write_imu_csv(imu_csv, samples)
imu_host_ok = sum(1 for sample in samples if sample.host_receive_utc_ticks > 0)
lidar_meta: dict
if lidar_dlog is not None:
session = load_h32_dlog_lidar(
lidar_dlog,
msop_object=msop_object,
difop_object=difop_object,
require_difop=require_difop,
host_ticks_min=lidar_ticks_min,
host_ticks_max=lidar_ticks_max,
)
frames = iter_h32_frames_from_packets(
session.msop_packets,
host_utc_ticks=session.msop_host_utc_ticks,
min_frame_points=min_frame_points,
frame_stride=frame_stride,
min_range_m=min_range_m,
@@ -134,16 +250,20 @@ def export_session(
)
lidar_meta = {
"source": "dlog",
"lidar_dlog": str(session.dlog_root),
"lidar_dlog": session.dlog_root,
"msop_object": session.msop_object,
"difop_object": session.difop_object,
"msop_packets": len(session.msop_packets),
"msop_packets_with_host_utc": sum(1 for ticks in session.msop_host_utc_ticks if ticks > 0),
"msop_batches": session.msop_batch_count,
"difop_records": session.difop_record_count,
"session_id": session.session_id,
"lidar_ip": session.lidar_ip,
"angle_source": session.angle_source,
"timestamp_note": "h32_msop_device_timestamp -> seconds (from MSOP bytes)",
"timestamp_note": (
"device: h32_msop_device_timestamp -> seconds; "
"host: MSOP HostReceiveUtcTicks -> unix seconds"
),
}
else:
assert lidar_rscap is not None
@@ -161,25 +281,46 @@ def export_session(
"lidar_rscap": str(lidar_rscap),
"capture": file_summary(lidar_capture),
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
"timestamp_note": "h32_msop_device_timestamp_ms -> seconds",
"timestamp_note": (
"device: h32_msop_device_timestamp_ms -> seconds; "
"host: rscap receive_utc_ticks -> unix seconds"
),
}
lidar_dir = out / "lidar"
lidar_stats = write_lidar_session(lidar_dir, frames)
imu_time_note = (
"hi13_device_timestamp_ms -> seconds"
if kind == "hi13"
else "n300_device_timestamp_us -> seconds"
)
summary = {
"imu_rscap": str(imu_rscap),
"imu_rscap": [str(path) for path in imu_paths],
"imu_kind": kind,
"out": str(out),
"host_window": {
"host_start": host_start,
"host_end": host_end,
"lidar_ticks_min": lidar_ticks_min,
"lidar_ticks_max": lidar_ticks_max,
"imu_ticks_min": imu_ticks_min,
"imu_ticks_max": imu_ticks_max,
"note": "local wall cut; lidar DObject tic=DateTime.Now, IMU/MSOP host=UTC",
},
"timestamp_policy": {
"imu": "n300_device_timestamp_us -> seconds",
"lidar": lidar_meta["timestamp_note"],
"host_utc": "not used as calibration timeline",
"imu_device": imu_time_note,
"imu_host": "rscap receive_utc_ticks -> t_host_utc_s",
"lidar_device": "MSOP device timestamp -> t_start/t_end",
"lidar_host": "MSOP HostReceiveUtcTicks -> t_host_utc_s",
"calibration_align": "bridge via host UTC; do not force first device samples to coincide",
},
"imu": {
"samples": int(t.shape[0]),
"samples_with_host_utc": imu_host_ok,
"t_start": float(t[0]) if t.size else None,
"t_end": float(t[-1]) if t.size else None,
"capture": file_summary(imu_capture),
"captures": imu_captures,
},
"lidar": {
**lidar_stats,
@@ -201,41 +342,38 @@ def export_session(
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap")
parser.add_argument(
"--imu-rscap",
type=Path,
action="append",
required=True,
help="IMU V2 .rscap (repeatable)",
)
parser.add_argument(
"--imu-kind",
choices=("auto", "hi13", "n300"),
default="auto",
help="IMU decoder (default: auto from filename)",
)
lidar = parser.add_mutually_exclusive_group(required=True)
lidar.add_argument(
"--lidar-dlog",
type=Path,
help="H32 Medulla dlog root (dobject/ + dobject_recording/), preferred",
help="H32 dlog directory or recovered zip (indices.log + data.bin)",
)
lidar.add_argument(
"--lidar-rscap",
type=Path,
help="Legacy H32 MSOP V2 .rscap (no DIFOP; default vertical angles)",
)
parser.add_argument(
"--msop-object",
default="frontlidar-msop-raw",
help="DObject name for raw MSOP batches (dlog path)",
)
parser.add_argument(
"--difop-object",
default="frontlidar-difop-raw",
help="DObject name for raw DIFOP packets (dlog path)",
)
parser.add_argument(
"--require-difop",
action="store_true",
help="Fail if dlog has no valid DIFOP channel angles",
)
parser.add_argument("--out", type=Path, required=True, help="Output session directory")
parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame")
parser.add_argument(
"--max-points-per-frame",
type=int,
default=80000,
help="Uniform downsample cap per frame; 0 disables",
help="Legacy H32 MSOP V2 .rscap",
)
parser.add_argument("--msop-object", default="frontlidar-msop-raw")
parser.add_argument("--difop-object", default="frontlidar-difop-raw")
parser.add_argument("--require-difop", action="store_true")
parser.add_argument("--host-start", type=str, default=None, help="Local wall start, e.g. 2026-08-08T17:40:05")
parser.add_argument("--host-end", type=str, default=None, help="Local wall end, e.g. 2026-08-08T17:45:15")
parser.add_argument("--out", type=Path, required=True)
parser.add_argument("--frame-stride", type=int, default=1)
parser.add_argument("--max-points-per-frame", type=int, default=80000)
parser.add_argument("--min-range-m", type=float, default=0.3)
parser.add_argument("--max-range-m", type=float, default=120.0)
parser.add_argument("--min-frame-points", type=int, default=100)
@@ -245,9 +383,12 @@ def main() -> int:
imu_rscap=args.imu_rscap,
lidar_rscap=args.lidar_rscap,
lidar_dlog=args.lidar_dlog,
imu_kind=args.imu_kind,
msop_object=args.msop_object,
difop_object=args.difop_object,
require_difop=args.require_difop,
host_start=args.host_start,
host_end=args.host_end,
out=args.out,
frame_stride=args.frame_stride,
max_points_per_frame=max_points,
@@ -258,10 +399,14 @@ def main() -> int:
print(
json.dumps(
{
"imu_kind": summary["imu_kind"],
"imu_samples": summary["imu"]["samples"],
"imu_host_utc": summary["imu"]["samples_with_host_utc"],
"lidar_frames": summary["lidar"]["frames"],
"lidar_host_utc": summary["lidar"]["frames_with_host_utc"],
"lidar_source": summary["lidar"]["source"],
"angle_source": summary["lidar"]["angle_source"],
"host_window": summary["host_window"],
"imu_csv": summary["outputs"]["imu_csv"],
"lidar_session": summary["outputs"]["lidar_session"],
"export_summary": str(Path(args.out) / "export_summary.json"),
@@ -271,9 +416,13 @@ def main() -> int:
)
)
if summary["imu"]["samples"] == 0:
raise SystemExit("no valid N300 IMU samples decoded")
raise SystemExit("no valid IMU samples decoded in window")
if summary["lidar"]["frames"] == 0:
raise SystemExit("no valid H32 frames decoded")
raise SystemExit("no valid H32 frames decoded in window")
if summary["lidar"]["frames_with_host_utc"] == 0:
raise SystemExit("no LiDAR frames with MSOP HostReceiveUtcTicks; cannot host-bridge align")
if summary["imu"]["samples_with_host_utc"] == 0:
raise SystemExit("no IMU samples with host receive UTC; cannot host-bridge align")
return 0