Multipoint Schritt 3

scripts/5_pose_estimation.py

@@ -20,9 +20,15 @@ Four switchable methods (robot.json -> pose_estimation.method):
   global_ba         : all variables jointly, position + normal residuals, robust loss
   hybrid            : sequential_fk init -> global_ba refine   (default, most stable)
 
-Observation input:
-  marker_observation = "corner_pose"  -> aruco_marker_poses.json (pos + measured normal)
-  marker_observation = "center_point" -> aruco_positions_*.json   (pos only)
+Observation input (robot.json -> pose_estimation.marker_observation):
+  "corner_pose"   (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
+  "corner_points"           -> aruco_marker_poses.json: 12 corner residuals for
+                               robot-link markers (4 triangulated corners vs FK
+                               corners; no separate normal), 1 center residual for
+                               root-link (Board: floor/rail) markers whose spin is
+                               uncalibrated. Robust loss acts per corner. Opt-in;
+                               needs `corners_m`. Links via corner_point_links.
+  "center_point"            -> aruco_positions_*.json: position only
 
 Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
   --from-state <json>        seed/init state (flat {var: value}, or the
@@ -89,6 +95,14 @@ DEFAULT_CFG: Dict[str, Any] = {
     "min_cameras_per_marker": 2,
     "finger_block_joints": ["b", "c", "e"],
     "per_link_method": {},
+    # Nur im marker_observation="corner_points"-Modus relevant: welche Links die
+    # 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
+    # der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
+    # Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
+    # Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
+    # zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
+    # z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
+    "corner_point_links": None,
     # One switch: if set to a link name (e.g. "Arm1"), that link's
     # jointToParent.origin Y/Z is fit together with the normal pose (same
     # global_ba solve) and the result is written back into robot.json
@@ -111,8 +125,12 @@ def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
 def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
     """
     Load marker observations. Accepts aruco_marker_poses.json (with measured
-    normal + num_cameras) or aruco_positions_*.json (position only).
-    Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int}
+    normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
+    (position only).
+    Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
+                           link:str, n_cams:int, weight:float}
+    corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
+    "corner_points"-Modus in residual_vector().
     """
     data = json.load(open(path, "r", encoding="utf-8"))
     out: Dict[int, Dict[str, Any]] = {}
@@ -135,7 +153,14 @@ 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,
+        corners_mm = None
+        cm = m.get("corners_m")
+        if cm is not None:
+            arr = np.array(cm, dtype=float)
+            if arr.shape == (4, 3):
+                corners_mm = arr * 1000.0
+        out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
+                    "link": m.get("link", "?"), "n_cams": n_cams,
                     "weight": float(m.get("weight", 1.0))}
     return out
 
@@ -268,14 +293,64 @@ def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, n
     return fk.all_markers_world(T)  # mid -> {world_mm, normal_world, link, local_mm}
 
 
+def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
+    """
+    Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
+    Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
+    (der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
+    ein Center-Residuum).
+    """
+    explicit = cfg.get("corner_point_links")
+    if isinstance(explicit, list) and explicit:
+        return set(explicit)
+    links = fk.links
+    roots = {ln for ln, ld in links.items()
+             if not ld.get("parent") or ld.get("parent") not in links}
+    return set(links.keys()) - roots
+
+
 def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
                     marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
-    """Position (mm) + optional normal (scaled) residuals over the given markers."""
+    """
+    Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
+
+      "corner_pose" (Default): 3 Position (mm) + optional 3 Normale
+                               (×normal_weight) je Marker — wie bisher.
+      "corner_points":         12 Eck-Residuen (4 Ecken × xyz, mm) je Marker auf
+                               einem Roboter-Link (corner_point_links), KEINE
+                               separate Normale (Orientierung steckt in den
+                               Ecken). Mehr unabhängige Messpunkte, robuster
+                               Verlust greift auf Eck-Ebene. Marker auf dem
+                               Root-Link (Board: Boden-/Rail-Marker mit
+                               unkalibriertem Spin) oder ohne `corners_mm`
+                               nutzen EIN Center-Residuum (3, "ein Punkt pro
+                               Marker") — gleiche mm-Skala → ein huber_delta_mm.
+    """
     model = model_markers(fk, state)
     res: List[float] = []
+    use_mw = bool(cfg.get("use_marker_weight", False))
+    obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
+
+    if obs_mode == "corner_points":
+        corner_links = _resolve_corner_links(fk, cfg)
+        for mid in marker_ids:
+            if mid not in model or mid not in obs:
+                continue
+            mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
+            mm = model[mid]
+            oc = obs[mid].get("corners_mm")
+            mc = mm.get("corners_world")
+            if mm.get("link") in corner_links and oc is not None and mc is not None:
+                dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw   # (4,3)
+                res.extend(dc.reshape(-1).tolist())                         # 12 Werte
+            else:
+                dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
+                res.extend(dp.tolist())                                     # Center (1 Punkt)
+        return np.asarray(res, dtype=float)
+
+    # Default: Center (mm) + optionale Normale (skaliert)
     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
@@ -646,6 +721,9 @@ def main() -> None:
     ap.add_argument("-robot", "--robot", required=True)
     ap.add_argument("-out", "--out", default=None)
     ap.add_argument("--method", default=None, help="override robot.json method")
+    ap.add_argument("--marker-observation", default=None, dest="marker_observation",
+                    help="override robot.json marker_observation "
+                         "(corner_pose | corner_points | center_point)")
     ap.add_argument("--from-state", default=None, metavar="JSON",
                     help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
                          "written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
@@ -656,6 +734,8 @@ def main() -> None:
     cfg = load_pose_cfg(robot_data)
     if args.method:
         cfg["method"] = args.method
+    if args.marker_observation:
+        cfg["marker_observation"] = args.marker_observation
 
     fk = RobotFK(robot_data)
     obs = load_observations(args.markers, cfg.get("use_normals", True),