Multipoint

scripts/3b_corner_marker_poses.py

@@ -59,11 +59,14 @@ def load_cameras(eval_dir: str) -> Dict[str, dict]:
         R = np.array(w2c["rotation_matrix"], dtype=float).reshape(3, 3)
         t = np.array(w2c["translation_m"], dtype=float).reshape(3)
         markers: Dict[int, np.ndarray] = {}
+        confidence: Dict[int, float] = {}
         for d in det.get("detections", []):
             pts = d.get("image_points_px")
             if pts is not None:
-                markers[int(d["marker_id"])] = np.array(pts, dtype=float).reshape(4, 2)
-        cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers)
+                mid = int(d["marker_id"])
+                markers[mid] = np.array(pts, dtype=float).reshape(4, 2)
+                confidence[mid] = float(d.get("confidence", 1.0))
+        cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers, confidence=confidence)
     return cams
 
 
@@ -156,6 +159,9 @@ def main() -> None:
         normal, center = corner_plane_normal(corners3d)
         edge_mm = float(np.mean([np.linalg.norm(corners3d[(i + 1) % 4] - corners3d[i]) for i in range(4)]) * 1000.0)
 
+        confidences = [cams[c]["confidence"].get(mid, 1.0) for c in cam_ids]
+        weight = float(np.mean(confidences))
+
         markers_out.append({
             "marker_id": int(mid),
             "link": marker_info.get(mid, {}).get("link", "unknown"),
@@ -166,6 +172,7 @@ def main() -> None:
             "corners_m": [[float(v) for v in c] for c in corners3d],
             "num_cameras": len(cam_ids),
             "edge_length_mm": edge_mm,
+            "weight": round(weight, 4),
         })
 
     # camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2]

scripts/4_yAxis_rotation_reconstruction.py

@@ -161,10 +161,20 @@ def compute_rotation_axis(
     for mid in common_ids:
         # ── Mindest-Bewegungs-Filter ───────────────────────────────────────────
         # Marker die sich kaum bewegen liefern degenerate Umkreismittelpunkte.
-        # Wir vergleichen die Zentren (position_mm) der drei Messungen.
-        cA = np.array(mA[mid].get('position_mm', [0, 0, 0]), dtype=float)
-        cB = np.array(mB[mid].get('position_mm', [0, 0, 0]), dtype=float)
-        cC = np.array(mC[mid].get('position_mm', [0, 0, 0]), dtype=float)
+        # Wir vergleichen die Zentren der drei Messungen.
+        # Fehlt position_mm in einer Messung (z.B. Einzelkamera-Marker) → überspringen.
+        cA_raw = mA[mid].get('position_mm')
+        cB_raw = mB[mid].get('position_mm')
+        cC_raw = mC[mid].get('position_mm')
+        if cA_raw is None or cB_raw is None or cC_raw is None:
+            skipped.append({
+                'marker_id': mid,
+                'reason': 'fehlende position_mm in mindestens einer Messung (z.B. Einzelkamera-Marker)',
+            })
+            continue
+        cA = np.array(cA_raw, dtype=float)
+        cB = np.array(cB_raw, dtype=float)
+        cC = np.array(cC_raw, dtype=float)
         max_movement = max(
             np.linalg.norm(cB - cA),
             np.linalg.norm(cC - cB),

scripts/5_pose_estimation.py

@@ -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)