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

Multipoint

Browse Source
This commit is contained in:
chk
2026-06-17 22:57:52 +02:00
parent 5f8e1a0189
commit eb403dab36
8 changed files with 667 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
scripts/5_pose_estimation.py
View File

@@ -82,6 +82,7 @@ DEFAULT_CFG: Dict[str, Any] = {
"marker_observation": "corner_pose",
"use_normals": True,
"normal_weight": 100.0,
"use_marker_weight": False,
"robust_loss": "huber",
"huber_delta_mm": 8.0,
"max_iterations": 200,
@@ -134,7 +135,8 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i
nn = np.linalg.norm(nv)
if nn > 1e-9:
nrm = nv / nn
out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams}
out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams,
"weight": float(m.get("weight", 1.0))}
return out
@@ -273,14 +275,16 @@ def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[st
res: List[float] = []
w_n = float(cfg.get("normal_weight", 30.0))
use_n = bool(cfg.get("use_normals", True))
use_mw = bool(cfg.get("use_marker_weight", False))
for mid in marker_ids:
if mid not in model or mid not in obs:
continue
mm = model[mid]
dp = np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
res.extend(dp.tolist())
if use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
res.extend(dn.tolist())
return np.asarray(res, dtype=float)