ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Liste CSV

Browse Source
This commit is contained in:
chk
2026-06-10 16:25:30 +02:00
parent b1e4a8b0be
commit 97ff7eb33f
3 changed files with 256 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
scripts/3b_corner_marker_poses.py
View File

@@ -22,8 +22,10 @@ und mit 9_evaluateMarker.py (position_m), erweitert um die gemessene Orientierun
from __future__ import annotations
import argparse
import csv
import glob
import json
import math
import os
import re
import time
@@ -65,14 +67,18 @@ def load_cameras(eval_dir: str) -> Dict[str, dict]:
return cams
def load_marker_links(robot_path: str) -> Dict[int, str]:
def load_marker_info(robot_path: str) -> Dict[int, dict]:
"""Returns {marker_id: {"link": ..., "position_mm": [x,y,z]}} for every marker in robot.json."""
robot = json.load(open(robot_path, "r", encoding="utf-8"))
out: Dict[int, str] = {}
out: Dict[int, dict] = {}
for link_name, link in (robot.get("links", {}) or {}).items():
for mk in link.get("markers", []) or []:
mid = int(mk.get("id", -1))
if mid >= 0:
out[mid] = link_name
out[mid] = {
"link": link_name,
"position_mm": mk.get("position"), # [x_mm, y_mm, z_mm] robot coords
}
return out
@@ -122,8 +128,8 @@ def main() -> None:
if len(cams) < 2:
print("[ERROR] need >=2 cameras")
return
links = load_marker_links(args.robot)
print(f"[INFO] Cameras: {sorted(cams.keys())} | marker-link entries: {len(links)}")
marker_info = load_marker_info(args.robot)
print(f"[INFO] Cameras: {sorted(cams.keys())} | marker-info entries: {len(marker_info)}")
marker_cams: Dict[int, List[str]] = {}
for cid, cam in cams.items():
@@ -151,7 +157,7 @@ def main() -> None:
markers_out.append({
"marker_id": int(mid),
"link": links.get(mid, "unknown"),
"link": marker_info.get(mid, {}).get("link", "unknown"),
"position_m": [float(v) for v in center],
"position_mm": [float(v * 1000.0) for v in center],
"normal": [float(v) for v in normal],
@@ -184,6 +190,56 @@ def main() -> None:
json.dump(output, open(out_path, "w", encoding="utf-8"), indent=2)
print(f"[INFO] {len(markers_out)} marker poses -> {out_path}")
# ── CSV (Debug-Daten) ────────────────────────────────────────────────────────
csv_path = os.path.splitext(out_path)[0] + ".csv"
with open(csv_path, "w", newline="", encoding="utf-8") as fcsv:
w = csv.writer(fcsv)
# Marker-Tabelle
w.writerow(["marker_id", "link", "num_cameras",
"x_mm", "y_mm", "z_mm",
"nx", "ny", "nz",
"model_x_mm", "model_y_mm", "model_z_mm",
"dist_to_model_mm", "delta_z_mm",
"edge_length_mm"])
for m in markers_out:
mid = m["marker_id"]
link = m["link"]
mi = marker_info.get(mid, {})
mpos = mi.get("position_mm") # Modellposition aus robot.json
px, py, pz = m["position_mm"]
mnx, mny, mnz = m["normal"]
if mpos and link == "Board":
mx, my, mz = mpos
ddx, ddy, ddz = px - mx, py - my, pz - mz
dist_val = round(math.sqrt(ddx**2 + ddy**2 + ddz**2), 3)
dz_val = round(ddz, 3)
row_model = [round(mx, 2), round(my, 2), round(mz, 2), dist_val, dz_val]
else:
row_model = ["", "", "", "", ""]
w.writerow([
mid, link, m["num_cameras"],
round(px, 2), round(py, 2), round(pz, 2),
round(mnx, 5), round(mny, 5), round(mnz, 5),
*row_model,
round(m["edge_length_mm"], 2),
])
# Kamera-Tabelle (nach einer Leerzeile)
w.writerow([])
w.writerow(["camera_id", "x_mm", "y_mm", "z_mm", "dir_x", "dir_y", "dir_z"])
for c in cameras_out:
cx, cy, cz = c["position_mm"]
dx, dy, dz = c["direction"]
w.writerow([
c["camera_id"],
round(cx, 2), round(cy, 2), round(cz, 2),
round(dx, 5), round(dy, 5), round(dz, 5),
])
print(f"[INFO] CSV -> {csv_path}")
if __name__ == "__main__":
main()