Python Scripte in HomingApp
This commit is contained in:
chk
1217
scripts/4_robotState_estimation_v6.py
Normal file
1217
scripts/4_robotState_estimation_v6.py
Normal file
File diff suppressed because it is too large
Load Diff
360
scripts/4b_revolute_angle.py
Normal file
360
scripts/4b_revolute_angle.py
Normal file
@@ -0,0 +1,360 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
4b_revolute_angle.py
|
||||
--------------------
|
||||
Generic revolute-joint angle estimator.
|
||||
|
||||
For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute',
|
||||
this script estimates the rotation angle using the pairwise-vector method:
|
||||
|
||||
For every PAIR (m1, m2) of markers belonging to the target link:
|
||||
v_model = local_pos_m2 - local_pos_m1 (in link's own frame)
|
||||
v_obs = world_pos_m2 - world_pos_m1 (in world frame)
|
||||
|
||||
Both vectors are projected perpendicular to the joint axis (in world frame),
|
||||
and the signed angle from v_model_perp to v_obs_perp is measured.
|
||||
|
||||
The joint axis in world frame is computed via FK using the already-known
|
||||
joint values (from 4a, 4b-prev …), so it is ALWAYS correct — even for
|
||||
deeply-nested joints whose world-frame axis differs from their local axis.
|
||||
|
||||
Pair weights = baseline_model × baseline_obs (longer baselines → more reliable).
|
||||
|
||||
How to use sequentially
|
||||
-----------------------
|
||||
Run 4b once per revolute joint, from root to tip:
|
||||
|
||||
python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Arm1 --x-mm 180
|
||||
python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Ellbow --from-state state.json
|
||||
python 4b_revolute_angle.py --robot r.json --aruco obs.json --link Arm2 --from-state state.json
|
||||
|
||||
The --from-state flag reads the accumulated joint state JSON so you don't have
|
||||
to pass every preceding value on the command line.
|
||||
|
||||
Output JSON
|
||||
-----------
|
||||
{
|
||||
"link": "Arm1",
|
||||
"joint": "y",
|
||||
"mean_angle_deg": 86.3,
|
||||
"circular_std_deg": 0.7,
|
||||
"num_pairs": 6,
|
||||
"joint_origin_world_mm": [290, 108, 61],
|
||||
"joint_axis_world": [-1, 0, 0],
|
||||
...
|
||||
}
|
||||
|
||||
The file also contains the full accumulated state so the next 4b invocation
|
||||
can read it via --from-state.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
from itertools import combinations
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
from robot_fk import RobotFK
|
||||
|
||||
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# I/O
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _load_json(path: Path) -> dict:
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
return json.load(f)
|
||||
|
||||
|
||||
def _load_observed_mm(aruco_json: dict) -> Dict[int, np.ndarray]:
|
||||
result: Dict[int, np.ndarray] = {}
|
||||
for m in aruco_json.get("markers", []):
|
||||
mid = int(m.get("marker_id", m.get("id", -1)))
|
||||
if mid < 0:
|
||||
continue
|
||||
if "position_mm" in m:
|
||||
result[mid] = np.array(m["position_mm"], dtype=float)
|
||||
elif "position_m" in m:
|
||||
result[mid] = np.array(m["position_m"], dtype=float) * 1000.0
|
||||
return result
|
||||
|
||||
|
||||
def _load_state(path: Path) -> Dict[str, float]:
|
||||
"""Load accumulated joint state from a previous 4b output JSON."""
|
||||
raw = _load_json(path)
|
||||
state = raw.get("accumulated_state", raw.get("state", {}))
|
||||
return {k: float(v) for k, v in state.items() if k in STATE_KEYS}
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Angle maths
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def _project_perp(v: np.ndarray, axis: np.ndarray) -> np.ndarray:
|
||||
"""Remove the component of v along axis."""
|
||||
a = axis / (np.linalg.norm(axis) + 1e-15)
|
||||
return v - np.dot(v, a) * a
|
||||
|
||||
|
||||
def _signed_angle_rad(v_from: np.ndarray, v_to: np.ndarray,
|
||||
axis: np.ndarray) -> float:
|
||||
"""Signed angle rotating v_from onto v_to around axis (radians)."""
|
||||
a = axis / (np.linalg.norm(axis) + 1e-15)
|
||||
return math.atan2(float(np.dot(a, np.cross(v_from, v_to))),
|
||||
float(np.dot(v_from, v_to)))
|
||||
|
||||
|
||||
def _wrap(angle: float) -> float:
|
||||
"""Wrap to (−π, π]."""
|
||||
return (angle + math.pi) % (2.0 * math.pi) - math.pi
|
||||
|
||||
|
||||
def _circular_mean_deg(angles_rad: np.ndarray,
|
||||
weights: np.ndarray) -> Tuple[float, float, float]:
|
||||
"""Returns mean_deg, circular_variance, circular_std_deg."""
|
||||
w = np.clip(weights, 0, None)
|
||||
if w.sum() < 1e-15:
|
||||
w = np.ones_like(w)
|
||||
s, c = np.sum(w * np.sin(angles_rad)), np.sum(w * np.cos(angles_rad))
|
||||
mean = math.atan2(s, c)
|
||||
R = math.sqrt(s*s + c*c) / w.sum()
|
||||
R = float(np.clip(R, 0, 1))
|
||||
c_var = 1.0 - R
|
||||
c_std = math.sqrt(max(0.0, -2.0 * math.log(max(R, 1e-15))))
|
||||
return math.degrees(mean), c_var, math.degrees(c_std)
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Core estimator (generic — works for any revolute joint)
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def estimate_revolute_angle(
|
||||
fk: RobotFK,
|
||||
observed_mm: Dict[int, np.ndarray],
|
||||
link_name: str,
|
||||
known_state: Dict[str, float],
|
||||
min_baseline_mm: float = 15.0,
|
||||
verbose: bool = True,
|
||||
) -> dict:
|
||||
"""
|
||||
Estimate the revolute joint angle for `link_name`.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
fk : RobotFK
|
||||
observed_mm : world marker positions from step 3
|
||||
link_name : e.g. "Arm1", "Ellbow", "Arm2"
|
||||
known_state : already-estimated joint values (e.g. {"x": 180.0, "y": 86.0})
|
||||
The target joint variable should NOT be in this dict.
|
||||
min_baseline_mm : skip pairs with model or observed baseline shorter than this
|
||||
verbose : print report
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict with estimation results + updated accumulated_state
|
||||
"""
|
||||
|
||||
# ── sanity checks ─────────────────────────────────────────
|
||||
links = fk.links
|
||||
if link_name not in links:
|
||||
return {"status": "failed",
|
||||
"reason": f"Link '{link_name}' not in robot.json"}
|
||||
|
||||
ji = links[link_name].get("jointToParent", {}) or {}
|
||||
jtype = str(ji.get("type", "")).lower()
|
||||
if jtype != "revolute":
|
||||
return {"status": "failed",
|
||||
"reason": f"Joint of '{link_name}' is not revolute (type='{jtype}')"}
|
||||
|
||||
var = str(ji.get("variable", "")).lower()
|
||||
|
||||
# ── FK: joint origin and axis in world ───────────────────
|
||||
# Use known_state with the TARGET joint at 0
|
||||
zero_state = dict(known_state)
|
||||
zero_state[var] = 0.0
|
||||
|
||||
origin_world = fk.joint_origin_world(link_name, zero_state)
|
||||
axis_world = fk.joint_axis_world(link_name, zero_state)
|
||||
|
||||
# ── collect matched markers ───────────────────────────────
|
||||
model_local: Dict[int, np.ndarray] = {}
|
||||
for m in links[link_name].get("markers", []):
|
||||
mid = int(m.get("id", -1))
|
||||
if mid >= 0 and "position" in m:
|
||||
model_local[mid] = np.array(m["position"], dtype=float)
|
||||
|
||||
matched = {mid: (model_local[mid], observed_mm[mid])
|
||||
for mid in model_local if mid in observed_mm}
|
||||
|
||||
if len(matched) < 2:
|
||||
return {
|
||||
"status": "failed",
|
||||
"reason": (f"Need ≥2 matched markers, found {len(matched)}: "
|
||||
f"{list(matched.keys())}. "
|
||||
f"Model marker IDs: {list(model_local.keys())}"),
|
||||
}
|
||||
|
||||
# ── pairwise estimation ───────────────────────────────────
|
||||
ids = sorted(matched.keys())
|
||||
angle_rad_list: List[float] = []
|
||||
weight_list: List[float] = []
|
||||
per_pair: List[dict] = []
|
||||
|
||||
for id1, id2 in combinations(ids, 2):
|
||||
l1, o1 = matched[id1]
|
||||
l2, o2 = matched[id2]
|
||||
|
||||
v_model = l2 - l1 # local frame, both in same link
|
||||
v_obs = o2 - o1 # world frame
|
||||
|
||||
v_model_perp = _project_perp(v_model, axis_world)
|
||||
v_obs_perp = _project_perp(v_obs, axis_world)
|
||||
|
||||
bl_model = float(np.linalg.norm(v_model_perp))
|
||||
bl_obs = float(np.linalg.norm(v_obs_perp))
|
||||
|
||||
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
|
||||
per_pair.append({
|
||||
"marker_ids": [id1, id2],
|
||||
"skipped": True,
|
||||
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
|
||||
})
|
||||
continue
|
||||
|
||||
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
|
||||
w = bl_model * bl_obs
|
||||
|
||||
angle_rad_list.append(angle)
|
||||
weight_list.append(w)
|
||||
per_pair.append({
|
||||
"marker_ids": [id1, id2],
|
||||
"skipped": False,
|
||||
"angle_deg": math.degrees(angle),
|
||||
"baseline_model_mm": bl_model,
|
||||
"baseline_obs_mm": bl_obs,
|
||||
"weight": w,
|
||||
})
|
||||
|
||||
if not angle_rad_list:
|
||||
return {
|
||||
"status": "failed",
|
||||
"reason": "All pairs below min_baseline_mm. "
|
||||
"Try --min-baseline 5 or check step-3 output.",
|
||||
}
|
||||
|
||||
mean_deg, c_var, c_std_deg = _circular_mean_deg(
|
||||
np.array(angle_rad_list), np.array(weight_list)
|
||||
)
|
||||
|
||||
# ── verbose report ────────────────────────────────────────
|
||||
if verbose:
|
||||
print(f"\n── 4b: '{link_name}' angle ({var}) ──────────────────────")
|
||||
print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm")
|
||||
print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]")
|
||||
print(f" Matched markers: {list(matched.keys())}")
|
||||
print(f" Pairs used: {len(angle_rad_list)} / {len(list(combinations(ids, 2)))}")
|
||||
print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °")
|
||||
if c_std_deg > 5.0:
|
||||
print(f" [WARN] high spread – step-3 errors or marker overlap")
|
||||
print(f"\n Pair detail:")
|
||||
for pp in per_pair:
|
||||
if pp["skipped"]:
|
||||
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: SKIPPED – {pp['reason']}")
|
||||
else:
|
||||
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: "
|
||||
f"{pp['angle_deg']:+7.2f}° "
|
||||
f"bl_model={pp['baseline_model_mm']:.1f} "
|
||||
f"bl_obs={pp['baseline_obs_mm']:.1f}")
|
||||
|
||||
# ── build accumulated state ───────────────────────────────
|
||||
accumulated = dict(known_state)
|
||||
accumulated[var] = mean_deg
|
||||
|
||||
return {
|
||||
"status": "ok",
|
||||
"link": link_name,
|
||||
"joint": var,
|
||||
"joint_origin_world_mm": origin_world.tolist(),
|
||||
"joint_axis_world": axis_world.tolist(),
|
||||
"mean_angle_deg": mean_deg,
|
||||
"circular_variance": c_var,
|
||||
"circular_std_deg": c_std_deg,
|
||||
"num_pairs_used": len(angle_rad_list),
|
||||
"num_markers_matched": len(matched),
|
||||
"per_pair": per_pair,
|
||||
"accumulated_state": accumulated,
|
||||
}
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# CLI
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def main() -> int:
|
||||
p = argparse.ArgumentParser(
|
||||
description="4b: estimate revolute joint angle for any link")
|
||||
p.add_argument("--robot", type=Path, default=Path("robot.json"))
|
||||
p.add_argument("--aruco", type=Path,
|
||||
default=Path("aruco_positions_optimized.json"))
|
||||
p.add_argument("--link", type=str, required=True,
|
||||
help="Link name, e.g. Arm1, Ellbow, Arm2")
|
||||
p.add_argument("--from-state", type=Path, default=None,
|
||||
help="JSON from a previous 4b run (carries accumulated state)")
|
||||
|
||||
# Manual joint-value overrides (for use without --from-state)
|
||||
for k in STATE_KEYS:
|
||||
p.add_argument(f"--{k}-mm" if k in ("x", "e") else f"--{k}-deg",
|
||||
type=float, default=None,
|
||||
help=f"Known value for joint '{k}'"
|
||||
+ (" (mm)" if k in ("x", "e") else " (deg)"))
|
||||
|
||||
p.add_argument("--min-baseline", type=float, default=15.0,
|
||||
help="Skip pairs with perpendicular baseline < this (mm)")
|
||||
p.add_argument("--output", type=Path, default=None,
|
||||
help="Save result JSON (readable by next 4b as --from-state)")
|
||||
args = p.parse_args()
|
||||
|
||||
# Assemble known state
|
||||
if args.from_state:
|
||||
known_state = _load_state(args.from_state)
|
||||
print(f" Loaded state from {args.from_state}: {known_state}")
|
||||
else:
|
||||
known_state = {}
|
||||
|
||||
# CLI overrides
|
||||
for k in STATE_KEYS:
|
||||
attr = f"{k}_mm" if k in ("x", "e") else f"{k}_deg"
|
||||
v = getattr(args, attr, None)
|
||||
if v is not None:
|
||||
known_state[k] = float(v)
|
||||
|
||||
fk = RobotFK.from_file(args.robot)
|
||||
aruco_json = _load_json(args.aruco)
|
||||
observed_mm = _load_observed_mm(aruco_json)
|
||||
|
||||
result = estimate_revolute_angle(
|
||||
fk = fk,
|
||||
observed_mm = observed_mm,
|
||||
link_name = args.link,
|
||||
known_state = known_state,
|
||||
min_baseline_mm = args.min_baseline,
|
||||
verbose = True,
|
||||
)
|
||||
|
||||
if args.output and result["status"] == "ok":
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
with args.output.open("w", encoding="utf-8") as f:
|
||||
json.dump(result, f, indent=2)
|
||||
print(f"\n Saved → {args.output}")
|
||||
|
||||
return 0 if result["status"] == "ok" else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
218
scripts/5_camera_z_refine.py
Normal file
218
scripts/5_camera_z_refine.py
Normal file
@@ -0,0 +1,218 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
5_camera_z_refine.py
|
||||
-----------------------
|
||||
Refines camera z-positions using the FK-predicted z of the Ellbow link.
|
||||
|
||||
Background
|
||||
----------
|
||||
Camera poses from step 2 are estimated from board markers alone. Board
|
||||
markers are roughly coplanar (z ≈ 0), so the camera's z-height above the
|
||||
board is poorly constrained and can be 20–40 % off.
|
||||
|
||||
Once we have the elbow angle (step 3b, from 4b_revolute_angle.py),
|
||||
FK predicts where the Ellbow markers *should* be in world space. The
|
||||
difference between FK-predicted z and triangulated z gives a global
|
||||
z-offset for the cameras.
|
||||
|
||||
Algorithm
|
||||
---------
|
||||
1. Load elbow-angle result (v8_ellbow_angle.json) → joint state dict
|
||||
2. Run FK with that state → predicted world positions for Ellbow markers (mm)
|
||||
3. Load triangulated Ellbow marker positions from aruco_positions_optimized.json
|
||||
4. delta_z_mm = median(fk_z - triangulated_z) over matched markers
|
||||
5. For each camera pose file:
|
||||
a. camera center world C = -R_wc.T @ t_wc
|
||||
b. C_new = C + [0, 0, delta_z_mm / 1000]
|
||||
c. t_wc_new = -R_wc @ C_new
|
||||
d. Save as *_camera_pose_v8.json
|
||||
6. Save v8_z_correction.json summary
|
||||
|
||||
Usage
|
||||
-----
|
||||
python 5_camera_z_refine.py \\
|
||||
--angle path/to/v8_ellbow_angle.json \\
|
||||
--robot path/to/robot.json \\
|
||||
--aruco path/to/aruco_positions_optimized.json \\
|
||||
-pose path/to/render_a_camera_pose.json \\
|
||||
-pose path/to/render_b_camera_pose.json \\
|
||||
--outDir path/to/output_dir \\
|
||||
[--elbowLink Ellbow]
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import copy
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
# robot_fk lives in the same directory as this script
|
||||
sys.path.insert(0, str(Path(__file__).parent))
|
||||
from robot_fk import RobotFK
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# I/O helpers
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def load_json(path: str) -> dict:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
return json.load(f)
|
||||
|
||||
|
||||
def save_json(path: str, data: dict) -> None:
|
||||
os.makedirs(os.path.dirname(path) or ".", exist_ok=True)
|
||||
with open(path, "w", encoding="utf-8") as f:
|
||||
json.dump(data, f, indent=2)
|
||||
|
||||
|
||||
def load_triangulated_mm(aruco_json: dict) -> Dict[int, np.ndarray]:
|
||||
"""Read marker world positions (mm) from aruco_positions_*.json."""
|
||||
result: Dict[int, np.ndarray] = {}
|
||||
for m in aruco_json.get("markers", []):
|
||||
mid = int(m.get("marker_id", m.get("id", -1)))
|
||||
if mid < 0:
|
||||
continue
|
||||
if "position_mm" in m:
|
||||
result[mid] = np.array(m["position_mm"], dtype=float)
|
||||
elif "position_m" in m:
|
||||
result[mid] = np.array(m["position_m"], dtype=float) * 1000.0
|
||||
return result
|
||||
|
||||
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
# Main
|
||||
# ──────────────────────────────────────────────────────────────
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="5v8: refine camera z-positions using FK-predicted Ellbow marker z"
|
||||
)
|
||||
parser.add_argument("--angle", required=True,
|
||||
help="v8_ellbow_angle.json from 4b_revolute_angle.py")
|
||||
parser.add_argument("--robot", required=True, help="robot.json")
|
||||
parser.add_argument("--aruco", required=True,
|
||||
help="aruco_positions_optimized.json (first-pass triangulation)")
|
||||
parser.add_argument("-pose", "--poses", action="append", required=True,
|
||||
help="*_camera_pose.json files (repeat for each camera)")
|
||||
parser.add_argument("--outDir", required=True, help="Output directory")
|
||||
parser.add_argument("--elbowLink", default="Ellbow",
|
||||
help="Link name used for z estimation (default: Ellbow)")
|
||||
args = parser.parse_args()
|
||||
|
||||
os.makedirs(args.outDir, exist_ok=True)
|
||||
|
||||
# ── 1. Load angle / state ────────────────────────────────
|
||||
angle_data = load_json(args.angle)
|
||||
if angle_data.get("status") != "ok":
|
||||
print(f"[ERROR] Angle estimation was not ok: {angle_data.get('reason', '?')}")
|
||||
sys.exit(1)
|
||||
|
||||
accumulated_state = angle_data.get("accumulated_state", {})
|
||||
elbow_link = angle_data.get("link", args.elbowLink)
|
||||
|
||||
print(f"[INFO] Joint state from 4b: {accumulated_state}")
|
||||
print(f"[INFO] Link used for z-ref: {elbow_link}")
|
||||
|
||||
# ── 2. FK prediction ─────────────────────────────────────
|
||||
fk = RobotFK.from_file(args.robot)
|
||||
T = fk.compute(accumulated_state)
|
||||
all_fk = fk.all_markers_world(T) # marker_id -> {world_mm, link, …}
|
||||
|
||||
elbow_fk = {mid: v for mid, v in all_fk.items() if v["link"] == elbow_link}
|
||||
|
||||
if not elbow_fk:
|
||||
print(f"[ERROR] No FK markers found for link '{elbow_link}'")
|
||||
sys.exit(1)
|
||||
|
||||
print(f"[INFO] FK markers for '{elbow_link}': {sorted(elbow_fk.keys())}")
|
||||
|
||||
# ── 3. Load triangulated positions ───────────────────────
|
||||
aruco_data = load_json(args.aruco)
|
||||
triangulated = load_triangulated_mm(aruco_data)
|
||||
|
||||
# ── 4. Compute z-deltas ──────────────────────────────────
|
||||
z_deltas: List[float] = []
|
||||
print("\n[INFO] Per-marker z comparison (FK vs triangulated):")
|
||||
for mid in sorted(elbow_fk.keys()):
|
||||
fk_z = float(elbow_fk[mid]["world_mm"][2])
|
||||
if mid not in triangulated:
|
||||
print(f" Marker {mid:4d}: FK z={fk_z:8.1f} mm [NOT triangulated – skip]")
|
||||
continue
|
||||
obs_z = float(triangulated[mid][2])
|
||||
delta = fk_z - obs_z
|
||||
z_deltas.append(delta)
|
||||
print(f" Marker {mid:4d}: FK z={fk_z:8.1f} mm obs z={obs_z:8.1f} mm Δz={delta:+7.1f} mm")
|
||||
|
||||
if not z_deltas:
|
||||
print(f"\n[ERROR] No matched markers for '{elbow_link}' — cannot compute z correction.")
|
||||
sys.exit(1)
|
||||
|
||||
delta_z_mm = float(np.median(z_deltas))
|
||||
print(f"\n[INFO] z-correction (median): {delta_z_mm:+.1f} mm "
|
||||
f"(from {len(z_deltas)} markers)")
|
||||
if abs(delta_z_mm) < 1.0:
|
||||
print("[INFO] Correction < 1 mm — cameras already well-calibrated in z.")
|
||||
|
||||
# ── 5. Apply correction to each camera pose ──────────────
|
||||
corrected_files: List[str] = []
|
||||
|
||||
for pose_file in args.poses:
|
||||
pose_data = load_json(pose_file)
|
||||
|
||||
pose_section = pose_data.get("camera_pose", {}) or {}
|
||||
w2c = pose_section.get("world_to_camera", {}) or {}
|
||||
|
||||
R_wc = np.array(w2c.get("rotation_matrix", []), dtype=float).reshape(3, 3)
|
||||
t_wc = np.array(w2c.get("translation_m", []), dtype=float).reshape(3)
|
||||
|
||||
# Camera centre in world (metres)
|
||||
C_world = -R_wc.T @ t_wc
|
||||
C_world_new = C_world + np.array([0.0, 0.0, delta_z_mm / 1000.0])
|
||||
|
||||
t_wc_new = -R_wc @ C_world_new
|
||||
|
||||
pose_new = copy.deepcopy(pose_data)
|
||||
pose_new["camera_pose"]["world_to_camera"]["translation_m"] = \
|
||||
[float(v) for v in t_wc_new.tolist()]
|
||||
pose_new["camera_pose"]["camera_in_world"]["position_m"] = \
|
||||
[float(v) for v in C_world_new.tolist()]
|
||||
pose_new["camera_pose"]["camera_in_world"]["position_mm"] = \
|
||||
[float(v * 1000.0) for v in C_world_new.tolist()]
|
||||
pose_new["v8_z_correction_mm"] = delta_z_mm
|
||||
pose_new["created_utc"] = time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime())
|
||||
|
||||
basename = os.path.basename(pose_file)
|
||||
out_name = basename.replace("_camera_pose.json", "_camera_pose_v8.json")
|
||||
if out_name == basename:
|
||||
out_name = os.path.splitext(basename)[0] + "_v8.json"
|
||||
|
||||
out_path = os.path.join(args.outDir, out_name)
|
||||
save_json(out_path, pose_new)
|
||||
print(f"[INFO] Saved: {out_path}")
|
||||
corrected_files.append(out_path)
|
||||
|
||||
# ── 6. Save correction summary ───────────────────────────
|
||||
summary = {
|
||||
"schema_version": "1.0",
|
||||
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"elbow_link": elbow_link,
|
||||
"z_correction_mm": delta_z_mm,
|
||||
"n_markers_used": len(z_deltas),
|
||||
"z_deltas_mm": z_deltas,
|
||||
"corrected_pose_files": corrected_files,
|
||||
}
|
||||
summary_path = os.path.join(args.outDir, "v8_z_correction.json")
|
||||
save_json(summary_path, summary)
|
||||
print(f"\n[INFO] Correction summary → {summary_path}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
145
scripts/9_evaluateMarker.py
Normal file
145
scripts/9_evaluateMarker.py
Normal file
@@ -0,0 +1,145 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
9_evaluateMarker.py
|
||||
===================
|
||||
Compare reconstructed markers against ground-truth (render_*.json).
|
||||
|
||||
Reports, per link and overall:
|
||||
* 3D position error (mm)
|
||||
* orientation error (deg) between measured and GT normal — only meaningful
|
||||
when the detected file carries a MEASURED normal (aruco_marker_poses.json).
|
||||
|
||||
Backwards compatible: the original positional CLI
|
||||
python 9_evaluateMarker.py detected.json original.json
|
||||
still works and prints the familiar summary; --out adds a JSON report.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
from collections import defaultdict
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
|
||||
def dist3(a, b) -> float:
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
def angle_deg(a, b) -> Optional[float]:
|
||||
na = math.sqrt(sum(x * x for x in a))
|
||||
nb = math.sqrt(sum(x * x for x in b))
|
||||
if na < 1e-9 or nb < 1e-9:
|
||||
return None
|
||||
c = sum(x * y for x, y in zip(a, b)) / (na * nb)
|
||||
c = max(-1.0, min(1.0, c))
|
||||
ang = math.degrees(math.acos(c))
|
||||
return min(ang, 180.0 - ang) # flip-invariant
|
||||
|
||||
|
||||
def _pct(values: List[float], q: float) -> float:
|
||||
if not values:
|
||||
return 0.0
|
||||
s = sorted(values)
|
||||
idx = max(0, min(len(s) - 1, int(q * len(s)) - 1))
|
||||
return s[idx]
|
||||
|
||||
|
||||
def analyze(detected_file: str, original_file: str) -> Dict[str, Any]:
|
||||
detected = json.load(open(detected_file, "r", encoding="utf-8"))
|
||||
original = json.load(open(original_file, "r", encoding="utf-8"))
|
||||
|
||||
det_markers = detected.get("markers", detected if isinstance(detected, list) else [])
|
||||
det_by_id = {int(m.get("marker_id", m.get("id", -1))): m for m in det_markers}
|
||||
orig_by_id = {int(m["id"]): m for m in original}
|
||||
|
||||
det_ids = set(det_by_id) - {-1}
|
||||
orig_ids = set(orig_by_id)
|
||||
recognized = det_ids & orig_ids
|
||||
missing = orig_ids - det_ids
|
||||
|
||||
per_link_pos: Dict[str, List[float]] = defaultdict(list)
|
||||
per_link_ang: Dict[str, List[float]] = defaultdict(list)
|
||||
rows = []
|
||||
for mid in sorted(recognized):
|
||||
o = orig_by_id[mid]
|
||||
d = det_by_id[mid]
|
||||
link = o.get("link", d.get("link", "?"))
|
||||
pos_err = dist3(o["position_m"], d["position_m"]) * 1000.0 # mm
|
||||
per_link_pos[link].append(pos_err)
|
||||
ang_err = None
|
||||
if o.get("normal") is not None and d.get("normal") is not None:
|
||||
ang_err = angle_deg(o["normal"], d["normal"])
|
||||
if ang_err is not None:
|
||||
per_link_ang[link].append(ang_err)
|
||||
rows.append({"marker_id": mid, "link": link, "pos_err_mm": pos_err, "normal_err_deg": ang_err})
|
||||
|
||||
all_pos = [r["pos_err_mm"] for r in rows]
|
||||
all_ang = [r["normal_err_deg"] for r in rows if r["normal_err_deg"] is not None]
|
||||
|
||||
return {
|
||||
"n_original": len(orig_ids),
|
||||
"n_recognized": len(recognized),
|
||||
"n_missing": len(missing),
|
||||
"recognition_rate": (len(recognized) / len(orig_ids) * 100.0) if orig_ids else 0.0,
|
||||
"per_link": {
|
||||
ln: {
|
||||
"n": len(per_link_pos[ln]),
|
||||
"pos_mean_mm": sum(per_link_pos[ln]) / len(per_link_pos[ln]),
|
||||
"pos_max_mm": max(per_link_pos[ln]),
|
||||
"normal_mean_deg": (sum(per_link_ang[ln]) / len(per_link_ang[ln])) if per_link_ang.get(ln) else None,
|
||||
"normal_max_deg": max(per_link_ang[ln]) if per_link_ang.get(ln) else None,
|
||||
} for ln in sorted(per_link_pos, key=lambda k: -len(per_link_pos[k]))
|
||||
},
|
||||
"overall": {
|
||||
"pos_mean_mm": (sum(all_pos) / len(all_pos)) if all_pos else 0.0,
|
||||
"pos_p90_mm": _pct(all_pos, 0.9),
|
||||
"pos_max_mm": max(all_pos) if all_pos else 0.0,
|
||||
"normal_mean_deg": (sum(all_ang) / len(all_ang)) if all_ang else None,
|
||||
"normal_p90_deg": _pct(all_ang, 0.9) if all_ang else None,
|
||||
"normal_max_deg": max(all_ang) if all_ang else None,
|
||||
},
|
||||
"rows": rows,
|
||||
}
|
||||
|
||||
|
||||
def print_report(r: Dict[str, Any]) -> None:
|
||||
# familiar summary (backwards compatible wording)
|
||||
print(f"Erkannte Marker: {r['n_recognized']}")
|
||||
print(f"Nicht erkannte Marker: {r['n_missing']}")
|
||||
print(f"Gesamtzahl der Original-Marker: {r['n_original']}")
|
||||
print(f"Erkennungsrate: {r['recognition_rate']:.2f}%")
|
||||
o = r["overall"]
|
||||
print(f"Gemittelter 3D-Abstand: {o['pos_mean_mm']/1000.0:.4f}m")
|
||||
print(f"90%-Radius: {o['pos_p90_mm']/1000.0:.4f}m")
|
||||
print(f"Schlechtester Abstand: {o['pos_max_mm']/1000.0:.4f}m")
|
||||
if o["normal_mean_deg"] is not None:
|
||||
print(f"Normalen-Fehler: mean {o['normal_mean_deg']:.2f}deg / p90 {o['normal_p90_deg']:.2f}deg / max {o['normal_max_deg']:.2f}deg")
|
||||
|
||||
print("\nPro Glied:")
|
||||
print(f" {'link':>10} | {'n':>3} | {'pos mean/max [mm]':>18} | {'normal mean/max [deg]':>21}")
|
||||
print(" " + "-" * 60)
|
||||
for ln, s in r["per_link"].items():
|
||||
nd = f"{s['normal_mean_deg']:6.2f} /{s['normal_max_deg']:6.2f}" if s["normal_mean_deg"] is not None else " - "
|
||||
print(f" {ln:>10} | {s['n']:>3} | {s['pos_mean_mm']:7.2f} /{s['pos_max_mm']:7.2f} | {nd:>21}")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
ap = argparse.ArgumentParser(description="Analysiert die Markererkennung (Position + Orientierung).")
|
||||
ap.add_argument("detected_file", help="aruco_marker_poses.json oder aruco_positions_*.json")
|
||||
ap.add_argument("original_file", help="Ground-Truth render_*.json")
|
||||
ap.add_argument("--out", default=None, help="optional JSON-Report")
|
||||
args = ap.parse_args()
|
||||
|
||||
r = analyze(args.detected_file, args.original_file)
|
||||
if r["n_recognized"] == 0:
|
||||
print("Keine gemeinsamen Marker gefunden, um die Genauigkeit zu bewerten.")
|
||||
return
|
||||
print_report(r)
|
||||
if args.out:
|
||||
json.dump(r, open(args.out, "w", encoding="utf-8"), indent=2)
|
||||
print(f"\n[INFO] wrote {args.out}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user