diff --git a/pipeline/2_Multiview.py b/pipeline/2_Multiview.py index 666bc1d..712a981 100644 --- a/pipeline/2_Multiview.py +++ b/pipeline/2_Multiview.py @@ -1,39 +1,34 @@ #!/usr/bin/env python3 - """ -============================================================ -STEP 2b — Simultane Multiview-Optimierung für Roboterpose -============================================================ +Phase 1 — robust multiview robot pose estimation from aruco_detection.json + robot.json -Ziel: - Aus mehreren ArUco-Detektionsdateien die gemeinsame - Roboterpose (x,y,z,a,b,c,e) schätzen und jede Kamera-Pose - sowie Marker-Weltpositionen ausgeben. +This version keeps the original kinematic model and optimizer structure, but changes: +- observation weighting to a saturating factor model: min(1, q + f) +- quality indicators are normalized to 0..1 +- blur/sharpness is supported but disabled by default (f=1) +- homography skew quality is added +- summary is built from the final output, so values stay consistent +- duplicate marker ids are warned about instead of being silently overwritten -Eingabe: - --robot ../robot.json - --detections render_1a_aruco_detection.json render_1b_aruco_detection.json ... - --outDir . - -Ausgabe: - multiview_pose.json - -Hinweis: - Dieses Skript verwendet die Markerpositionen aus robot.json - als kinematische Constraints und optimiert gleichzeitig: - - Roboterzustand (x,y,z,a,b,c,e) - - Kameraextrinsische Parameter pro Bild +Input: + --robot robot.json + --detections render_1a_aruco_detection.json ... + --outDir output +Output: + multiview_pose.json + multiview_pose_summary.json (optional) """ import argparse -import datetime +import datetime as _dt import json import math import os import time +from dataclasses import dataclass, field from pathlib import Path -from typing import Any, Dict, List, Tuple +from typing import Any, Dict, List, Tuple, Optional import cv2 import numpy as np @@ -41,106 +36,25 @@ from scipy.optimize import least_squares STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"] -# ------------------------------------------------------------------ -# Constraint definitions and validation -# ------------------------------------------------------------------ -class ConstraintResult: - """Result of validating/applying a single constraint""" - def __init__(self, name: str, enabled: bool, reason: str = ""): - self.name = name - self.enabled = enabled - self.reason = reason - self.residuals = [] +# ----------------------------------------------------------------------------- +# Small helpers +# ----------------------------------------------------------------------------- - def __str__(self) -> str: - status = "✓ ENABLED" if self.enabled else "✗ DISABLED" - return f"{self.name:40s} {status:12s} {self.reason}" +def clamp01(x: float) -> float: + return float(max(0.0, min(1.0, x))) -def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]: - """ - Validate which constraints can be applied based on robot geometry. - Returns a dict of constraint_name -> ConstraintResult - """ - results = {} - - # --- Constraint 1: Rigid body distances within each link --- - rigid_body_result = ConstraintResult("RigidBodyDistances", False) - try: - rigid_body_count = 0 - for link_name in ['Arm1', 'Ellbow', 'Arm2']: - link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] - if len(link_markers) >= 2: - rigid_body_count += 1 - if rigid_body_count >= 2: - rigid_body_result.enabled = True - rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each" - else: - rigid_body_result.reason = "Not enough rigid links with multiple markers" - except Exception as e: - rigid_body_result.reason = f"Error: {str(e)}" - results['RigidBodyDistances'] = rigid_body_result - - # --- Constraint 2: Fixed X-distances between links (rotation around X-axis) --- - inter_link_x_result = ConstraintResult("InterLinkXDistances", False) - try: - links_with_markers = set(m['link_name'] for m in robot_markers.values()) - x_rotated_links = [] - for link_name in ['Arm1', 'Ellbow']: - if link_name in links_with_markers: - link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] - if len(link_markers) >= 1: - x_rotated_links.append(link_name) - if len(x_rotated_links) >= 2: - inter_link_x_result.enabled = True - inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}" - else: - inter_link_x_result.reason = "Not enough X-rotation links" - except Exception as e: - inter_link_x_result.reason = f"Error: {str(e)}" - results['InterLinkXDistances'] = inter_link_x_result - - # --- Sanity check (not a hard constraint): Arm2 sin(a) dependency --- - arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)") - try: - arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] - if len(arm2_markers) >= 2: - z_values = set(m['position_m'][2] for m in arm2_markers) - if len(z_values) > 1: - arm2_sina_result.enabled = True - arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed" - else: - arm2_sina_result.enabled = False - arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)" - else: - arm2_sina_result.enabled = False - arm2_sina_result.reason = "Not enough Arm2 markers" - except Exception as e: - arm2_sina_result.reason = f"Error: {str(e)}" - results['Arm2SinADependency'] = arm2_sina_result - - return results - - -# ------------------------------------------------------------------ -# JSON helpers -# ------------------------------------------------------------------ - def load_json(path: str) -> Dict[str, Any]: - with open(path, 'r', encoding='utf-8') as f: + with open(path, "r", encoding="utf-8") as f: return json.load(f) def save_json(data: Dict[str, Any], path: Path) -> None: - with open(path, 'w', encoding='utf-8') as f: + with open(path, "w", encoding="utf-8") as f: json.dump(data, f, indent=2) -# ------------------------------------------------------------------ -# robot.json helpers -# ------------------------------------------------------------------ - def resolve_scalar(value: Any, default: float = 0.0) -> float: if value is None: return default @@ -148,7 +62,7 @@ def resolve_scalar(value: Any, default: float = 0.0) -> float: return float(value) try: return float(str(value).strip()) - except ValueError: + except Exception: return default @@ -166,17 +80,17 @@ def resolve_vector(value: Any, default_len: int = 3) -> Tuple[float, ...]: def parse_metric_scale(robot: Dict[str, Any]) -> float: - rendering_info = robot.get('renderingInfo', {}) or {} - metric = rendering_info.get('metric', 'mm') - return 0.001 if str(metric).strip().lower() == 'mm' else 1.0 + rendering_info = robot.get("renderingInfo", {}) or {} + metric = str(rendering_info.get("metric", "mm")).strip().lower() + return 0.001 if metric == "mm" else 1.0 def normalize_axis(axis: Any) -> np.ndarray: - vec = np.asarray(axis, dtype=np.float64) - if vec.shape != (3,): - vec = vec.reshape(-1)[:3] + vec = np.asarray(axis, dtype=np.float64).reshape(-1)[:3] norm = np.linalg.norm(vec) - return vec / max(norm, 1e-9) + if norm < 1e-12: + return np.array([1.0, 0.0, 0.0], dtype=np.float64) + return vec / norm def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray: @@ -192,31 +106,15 @@ def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray: cz = math.cos(z) sz = math.sin(z) - Rx = np.array([ - [1.0, 0.0, 0.0], - [0.0, cx, -sx], - [0.0, sx, cx] - ], dtype=np.float64) - - Ry = np.array([ - [cy, 0.0, sy], - [0.0, 1.0, 0.0], - [-sy, 0.0, cy] - ], dtype=np.float64) - - Rz = np.array([ - [cz, -sz, 0.0], - [sz, cz, 0.0], - [0.0, 0.0, 1.0] - ], dtype=np.float64) - + Rx = np.array([[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]], dtype=np.float64) + Ry = np.array([[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]], dtype=np.float64) + Rz = np.array([[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64) return Rz @ Ry @ Rx def transform_from_translation_rotation(translation: Any, rotation_deg: Any) -> np.ndarray: T = np.eye(4, dtype=np.float64) - pos = np.asarray(resolve_vector(translation, 3), dtype=np.float64) - T[:3, 3] = pos + T[:3, 3] = np.asarray(resolve_vector(translation, 3), dtype=np.float64) T[:3, :3] = euler_deg_to_matrix(rotation_deg) return T @@ -229,45 +127,174 @@ def axis_angle_matrix(axis: Any, angle_deg: float) -> np.ndarray: s = math.sin(theta) v = 1.0 - c R = np.array([ - [kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s], - [ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s], - [kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c] + [kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s], + [ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s], + [kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c], ], dtype=np.float64) T = np.eye(4, dtype=np.float64) T[:3, :3] = R return T -# ------------------------------------------------------------------ -# Kinematics and marker extraction -# ------------------------------------------------------------------ +# ----------------------------------------------------------------------------- +# Optional quality configuration +# ----------------------------------------------------------------------------- + +@dataclass +class ObservationQualityConfig: + # q indicators are normalized to 0..1 + size_ref_px: float = 50.0 + border_ref_px: float = 120.0 + center_ref_norm: float = 1.0 + sharpness_ref: float = 2500.0 + homography_ref: float = 0.18 + + # factor f in min(1, q + f) + # f = 1 -> factor disabled / neutral + # f = 0 -> factor fully active + size_factor: float = 1.0 + aspect_factor: float = 1.0 + border_factor: float = 1.0 + center_factor: float = 1.0 + sharpness_factor: float = 1.0 + homography_factor: float = 1.0 + + # Placeholders for later phases + normal_visibility_factor: float = 1.0 + spin_factor: float = 1.0 + + # Keep a tiny floor for weights so the optimizer remains numerically stable + weight_floor: float = 0.01 + + +def _load_nested_quality_config(src: Dict[str, Any]) -> Dict[str, Any]: + if not isinstance(src, dict): + return {} + for key in ("multiview_quality", "multiviewQuality", "quality_config", "multiview"): + v = src.get(key) + if isinstance(v, dict): + return v + return {} + + +def load_quality_config(robot: Dict[str, Any]) -> ObservationQualityConfig: + cfg = ObservationQualityConfig() + candidates = [] + candidates.append(_load_nested_quality_config(robot)) + candidates.append(_load_nested_quality_config(robot.get("vision_config", {}) or {})) + + for src in candidates: + if not src: + continue + for field_name in cfg.__dataclass_fields__.keys(): + if field_name in src: + setattr(cfg, field_name, resolve_scalar(src.get(field_name), getattr(cfg, field_name))) + return cfg + + +# ----------------------------------------------------------------------------- +# Marker extraction and kinematics +# ----------------------------------------------------------------------------- + +class ConstraintResult: + def __init__(self, name: str, enabled: bool, reason: str = ""): + self.name = name + self.enabled = enabled + self.reason = reason + self.residuals = [] + + def __str__(self) -> str: + status = "✓ ENABLED" if self.enabled else "✗ DISABLED" + return f"{self.name:40s} {status:12s} {self.reason}" + + +def build_link_chain(robot: Dict[str, Any]) -> List[str]: + links = robot.get("links", {}) or {} + ordered: List[str] = [] + remaining = set(links.keys()) + while remaining: + progress = False + for name in list(remaining): + parent = links[name].get("parent") + if not parent or parent in ordered: + ordered.append(name) + remaining.remove(name) + progress = True + if not progress: + raise RuntimeError("Cycle detected in robot link tree or missing parent link") + return ordered + def extract_markers(robot: Dict[str, Any], scale: float) -> Dict[int, Dict[str, Any]]: - markers = {} - links = robot.get('links', {}) or {} - marker_defaults = (robot.get('renderingInfo', {}) or {}).get('markerDefaults', {}) or {} - default_size_mm = float(marker_defaults.get('size', 25.0)) + markers: Dict[int, Dict[str, Any]] = {} + links = robot.get("links", {}) or {} + marker_defaults = (robot.get("renderingInfo", {}) or {}).get("markerDefaults", {}) or {} + default_size_mm = float(marker_defaults.get("size", 25.0)) for link_name, link_info in links.items(): - for marker in link_info.get('markers', []) or []: - marker_id = int(marker.get('id', -1)) + for marker in link_info.get("markers", []) or []: + marker_id = int(marker.get("id", -1)) if marker_id < 0: continue + if marker_id in markers: + # Duplicate ids exist in the provided robot.json. Keep the first entry and warn. + print(f"[WARN] Duplicate marker id {marker_id} on link '{link_name}'. Ignoring this duplicate entry.") + continue - pos = resolve_vector(marker.get('position', [0, 0, 0]), 3) - size_mm = float(marker.get('size', default_size_mm)) + pos = resolve_vector(marker.get("position", [0, 0, 0]), 3) + size_mm = float(marker.get("size", default_size_mm)) markers[marker_id] = { - 'marker_id': marker_id, - 'link_name': link_name, - 'position_m': np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64), - 'normal': normalize_axis(resolve_vector(marker.get('normal', [0, 0, 1]), 3)), - 'spin_deg': float(marker.get('spin', 0.0)), - 'size_m': size_mm * scale, + "marker_id": marker_id, + "link_name": link_name, + "position_m": np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64), + "normal": normalize_axis(resolve_vector(marker.get("normal", [0, 0, 1]), 3)), + "spin_deg": float(marker.get("spin", 0.0)), + "size_m": size_mm * scale, } return markers +def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]: + links = robot.get("links", {}) or {} + ordered_links = build_link_chain(robot) + transforms: Dict[str, np.ndarray] = {} + + for link_name in ordered_links: + link_info = links[link_name] or {} + parent_name = link_info.get("parent") + parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64) + + mount_translation = np.asarray(resolve_vector(link_info.get("mountPosition", [0, 0, 0]), 3), dtype=np.float64) * scale + mount = transform_from_translation_rotation(mount_translation, link_info.get("mountRotation", [0, 0, 0])) + + joint_info = link_info.get("jointToParent", {}) or {} + joint_origin = np.asarray(resolve_vector(joint_info.get("origin", [0, 0, 0]), 3), dtype=np.float64) * scale + joint = transform_from_translation_rotation(joint_origin, joint_info.get("rotation", [0, 0, 0])) + + motion = np.eye(4, dtype=np.float64) + joint_type = str(joint_info.get("type", "fixed")).strip().lower() + control_var = str(joint_info.get("variable", joint_info.get("control", ""))).strip().lower() + axis = resolve_vector(joint_info.get("axis", [1, 0, 0]), 3) + + if joint_type == "linear": + motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale + elif joint_type == "revolute": + motion = axis_angle_matrix(axis, state.get(control_var, 0.0)) + + transforms[link_name] = parent_transform @ mount @ joint @ motion + + return transforms + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker["link_name"]] + local = np.ones(4, dtype=np.float64) + local[:3] = marker["position_m"] + world = link_transform @ local + return world[:3] + + def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, np.ndarray]: n = normalize_axis(normal) candidate = np.array((0.0, 0.0, 1.0), dtype=np.float64) @@ -278,7 +305,7 @@ def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, x_dir /= max(np.linalg.norm(x_dir), 1e-9) y_dir = np.cross(n, x_dir) - if abs(spin_deg) > 1e-6: + if abs(spin_deg) > 1e-9: theta = math.radians(spin_deg) cos_t = math.cos(theta) sin_t = math.sin(theta) @@ -290,95 +317,264 @@ def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, def marker_object_corners(marker: Dict[str, Any]) -> np.ndarray: - half = marker['size_m'] * 0.5 - x_dir, y_dir = marker_plane_axes(marker['normal'], marker['spin_deg']) + half = marker["size_m"] * 0.5 + x_dir, y_dir = marker_plane_axes(marker["normal"], marker["spin_deg"]) corners = np.stack([ -x_dir * half + y_dir * half, x_dir * half + y_dir * half, x_dir * half - y_dir * half, - -x_dir * half - y_dir * half + -x_dir * half - y_dir * half, ], axis=0) - return marker['position_m'].reshape(1, 3) + corners + return marker["position_m"].reshape(1, 3) + corners -def build_link_chain(robot: Dict[str, Any]) -> List[str]: - links = robot.get('links', {}) or {} - ordered: List[str] = [] - remaining = set(links.keys()) - - while remaining: - progress = False - for name in list(remaining): - parent = links[name].get('parent') - if not parent or parent in ordered: - ordered.append(name) - remaining.remove(name) - progress = True - if not progress: - raise RuntimeError('Cycle detected in robot link tree or missing parent link') - return ordered +def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker["link_name"]] + local = marker_object_corners(marker) + homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1) + world = (link_transform @ homogeneous.T).T + return world[:, :3] -def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]: - links = robot.get('links', {}) or {} - ordered_links = build_link_chain(robot) - transforms: Dict[str, np.ndarray] = {} +# ----------------------------------------------------------------------------- +# Quality model +# ----------------------------------------------------------------------------- - for link_name in ordered_links: - link_info = links[link_name] or {} - parent_name = link_info.get('parent') - parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64) - - mount_translation = np.asarray(resolve_vector(link_info.get('mountPosition', [0, 0, 0]), 3), dtype=np.float64) * scale - mount = transform_from_translation_rotation( - mount_translation, - link_info.get('mountRotation', [0, 0, 0]) - ) - - joint_info = link_info.get('jointToParent', {}) or {} - joint_origin = np.asarray(resolve_vector(joint_info.get('origin', [0, 0, 0]), 3), dtype=np.float64) * scale - joint = transform_from_translation_rotation( - joint_origin, - joint_info.get('rotation', [0, 0, 0]) - ) - - motion = np.eye(4, dtype=np.float64) - joint_type = str(joint_info.get('type', 'fixed')).strip().lower() - control_var = str(joint_info.get('variable', joint_info.get('control', ''))).strip().lower() - axis = resolve_vector(joint_info.get('axis', [1, 0, 0]), 3) - - if joint_type == 'linear': - motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale - elif joint_type == 'revolute': - motion = axis_angle_matrix(axis, state.get(control_var, 0.0)) - - transforms[link_name] = parent_transform @ mount @ joint @ motion - - return transforms +def quality_factor(q: float, f: float) -> float: + # Saturating factor in [0, 1]. + # f = 1.0 -> ignore this q-indicator completely. + return clamp01(q + f) -def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: - link_transform = link_transforms[marker['link_name']] - local = np.ones(4, dtype=np.float64) - local[:3] = marker['position_m'] - world = link_transform @ local - return world[:3] +def projective_homography_quality(image_points_px: np.ndarray, image_shape: Tuple[int, int], ref: float) -> float: + if image_points_px is None or len(image_points_px) != 4: + return 1.0 + + h, w = image_shape + if h <= 0 or w <= 0: + return 1.0 + + src = np.array([[0.0, 0.0], [1.0, 0.0], [1.0, 1.0], [0.0, 1.0]], dtype=np.float32) + dst = np.asarray(image_points_px, dtype=np.float32).copy() + dst[:, 0] /= float(w) + dst[:, 1] /= float(h) + + try: + H = cv2.getPerspectiveTransform(src, dst).astype(np.float64) + if abs(H[2, 2]) > 1e-12: + H = H / H[2, 2] + proj_strength = float(abs(H[2, 0]) + abs(H[2, 1])) + q = 1.0 / (1.0 + proj_strength / max(ref, 1e-6)) + return clamp01(q) + except Exception: + return 1.0 -# ------------------------------------------------------------------ +def compute_observation_quality( + det: Dict[str, Any], + image_shape: Tuple[int, int], + cfg: ObservationQualityConfig, +) -> Dict[str, Any]: + quality = det.get("quality", {}) or {} + geometry = quality.get("geometry", {}) or {} + sharpness = quality.get("sharpness", {}) or {} + + edge_lengths = quality.get("edge_lengths_px", []) or [] + edge_lengths = [float(x) for x in edge_lengths if x is not None] + mean_edge_px = float(np.mean(edge_lengths)) if len(edge_lengths) else math.sqrt(max(float(quality.get("area_px", 0.0)), 0.0)) + + edge_ratio = float(quality.get("edge_ratio", 1.0) or 1.0) + distance_to_border_px = float(geometry.get("distance_to_border_px", 0.0) or 0.0) + distance_to_center_norm = float(geometry.get("distance_to_center_norm", 1.0) or 1.0) + laplacian_var = float(sharpness.get("laplacian_var", 0.0) or 0.0) + + # q in 0..1 + q_size = clamp01(mean_edge_px / max(cfg.size_ref_px, 1e-6)) + q_aspect = clamp01(2.0 / (1.0 + max(edge_ratio, 1e-6))) + q_border = clamp01(distance_to_border_px / max(cfg.border_ref_px, 1e-6)) + q_center = clamp01(1.0 - (distance_to_center_norm / max(cfg.center_ref_norm, 1e-6))) + q_sharpness = clamp01(laplacian_var / max(cfg.sharpness_ref, 1e-6)) + q_homography = projective_homography_quality(np.asarray(det.get("image_points_px", []), dtype=np.float64), image_shape, cfg.homography_ref) + + factor_map = { + "size": quality_factor(q_size, cfg.size_factor), + "aspect": quality_factor(q_aspect, cfg.aspect_factor), + "border": quality_factor(q_border, cfg.border_factor), + "center": quality_factor(q_center, cfg.center_factor), + "sharpness": quality_factor(q_sharpness, cfg.sharpness_factor), + "homography": quality_factor(q_homography, cfg.homography_factor), + } + + # Currently not active in phase 1, but already supported for later phases. + # They default to f=1, which makes them neutral. + q_normal_visibility = 1.0 + q_spin = 1.0 + factor_map["normal_visibility"] = quality_factor(q_normal_visibility, cfg.normal_visibility_factor) + factor_map["spin"] = quality_factor(q_spin, cfg.spin_factor) + + weight_multiplier = 1.0 + for v in factor_map.values(): + weight_multiplier *= float(v) + + # Conservative default: if no factor is activated in robot.json, + # this remains effectively neutral. + + detector_confidence = clamp01(float(det.get("confidence", 1.0) or 1.0)) + weighted_confidence = detector_confidence * weight_multiplier + weighted_confidence = max(cfg.weight_floor, min(1.0, weighted_confidence)) + + return { + "detector_confidence": detector_confidence, + "weighted_confidence": weighted_confidence, + "q": { + "size": q_size, + "aspect": q_aspect, + "border": q_border, + "center": q_center, + "sharpness": q_sharpness, + "homography": q_homography, + "normal_visibility": q_normal_visibility, + "spin": q_spin, + }, + "factor": factor_map, + "weight_multiplier": weight_multiplier, + "raw": { + "mean_edge_px": mean_edge_px, + "edge_ratio": edge_ratio, + "distance_to_border_px": distance_to_border_px, + "distance_to_center_norm": distance_to_center_norm, + "laplacian_var": laplacian_var, + }, + } + + +# ----------------------------------------------------------------------------- +# Constraints (kept from the existing approach) +# ----------------------------------------------------------------------------- + + +def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]: + results = {} + + rigid_body_result = ConstraintResult("RigidBodyDistances", False) + try: + rigid_body_count = 0 + for link_name in ["Arm1", "Ellbow", "Arm2"]: + link_markers = [m for m in robot_markers.values() if m["link_name"] == link_name] + if len(link_markers) >= 2: + rigid_body_count += 1 + if rigid_body_count >= 2: + rigid_body_result.enabled = True + rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each" + else: + rigid_body_result.reason = "Not enough rigid links with multiple markers" + except Exception as e: + rigid_body_result.reason = f"Error: {str(e)}" + results["RigidBodyDistances"] = rigid_body_result + + inter_link_x_result = ConstraintResult("InterLinkXDistances", False) + try: + links_with_markers = set(m["link_name"] for m in robot_markers.values()) + x_rotated_links = [] + for link_name in ["Arm1", "Ellbow"]: + if link_name in links_with_markers: + link_markers = [m for m in robot_markers.values() if m["link_name"] == link_name] + if len(link_markers) >= 1: + x_rotated_links.append(link_name) + if len(x_rotated_links) >= 2: + inter_link_x_result.enabled = True + inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}" + else: + inter_link_x_result.reason = "Not enough X-rotation links" + except Exception as e: + inter_link_x_result.reason = f"Error: {str(e)}" + results["InterLinkXDistances"] = inter_link_x_result + + arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)") + try: + arm2_markers = [m for m in robot_markers.values() if m["link_name"] == "Arm2"] + if len(arm2_markers) >= 2: + z_values = set(float(m["position_m"][2]) for m in arm2_markers) + if len(z_values) > 1: + arm2_sina_result.enabled = True + arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "Not enough Arm2 markers" + except Exception as e: + arm2_sina_result.reason = f"Error: {str(e)}" + results["Arm2SinADependency"] = arm2_sina_result + + return results + + +def compute_soft_constraint_residuals( + robot_state: Dict[str, float], + robot_markers: Dict[int, Dict[str, Any]], + link_transforms: Dict[str, np.ndarray], + robot: Dict[str, Any], + enabled_constraints: Dict[str, ConstraintResult], +) -> List[float]: + residuals = [] + weight_scale = 0.1 + + if enabled_constraints["RigidBodyDistances"].enabled: + for link_name in ["Arm1", "Ellbow", "Arm2"]: + link_markers = [m for m in robot_markers.values() if m["link_name"] == link_name] + if len(link_markers) < 2: + continue + for i in range(len(link_markers)): + for j in range(i + 1, len(link_markers)): + m_i = link_markers[i] + m_j = link_markers[j] + pos_i = compute_marker_world_position(m_i, link_transforms) + pos_j = compute_marker_world_position(m_j, link_transforms) + dist_world = np.linalg.norm(pos_i - pos_j) + dist_local = np.linalg.norm(m_i["position_m"] - m_j["position_m"]) + error = dist_world - dist_local + residuals.append(error * weight_scale * 0.1) + + if enabled_constraints["InterLinkXDistances"].enabled: + arm1_markers = [m for m in robot_markers.values() if m["link_name"] == "Arm1"] + ellbow_markers = [m for m in robot_markers.values() if m["link_name"] == "Ellbow"] + if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: + m_arm1 = arm1_markers[0] + m_ellbow = ellbow_markers[0] + pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) + pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) + x_diff_world = pos_ellbow[0] - pos_arm1[0] + x_diff_ref = m_ellbow["position_m"][0] - m_arm1["position_m"][0] + residuals.append((x_diff_world - x_diff_ref) * weight_scale) + + return residuals + + +# ----------------------------------------------------------------------------- # Camera / observation helpers -# ------------------------------------------------------------------ +# ----------------------------------------------------------------------------- + def load_intrinsics(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: - cam = detection_json['camera'] - K = np.asarray(cam['camera_matrix'], dtype=np.float64) - D = np.asarray(cam.get('distortion_coefficients', [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1) + cam = detection_json["camera"] + K = np.asarray(cam["camera_matrix"], dtype=np.float64) + D = np.asarray(cam.get("distortion_coefficients", [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1) return K, D +def detection_image_shape(detection_json: Dict[str, Any]) -> Tuple[int, int]: + image = detection_json.get("image", {}) or {} + h = int(image.get("height_px", detection_json.get("height_px", 720)) or 720) + w = int(image.get("width_px", detection_json.get("width_px", 1280)) or 1280) + return h, w + + def collect_views_and_observations( detection_files: List[str], - robot_markers: Dict[int, Dict[str, Any]] + robot_markers: Dict[int, Dict[str, Any]], + quality_cfg: ObservationQualityConfig, ) -> Tuple[List[Dict[str, Any]], List[Dict[str, Any]]]: views: List[Dict[str, Any]] = [] observations: List[Dict[str, Any]] = [] @@ -386,73 +582,70 @@ def collect_views_and_observations( for idx, det_path in enumerate(detection_files): detection_json = load_json(det_path) K, D = load_intrinsics(detection_json) + image_shape = detection_image_shape(detection_json) + views.append({ - 'index': idx, - 'source_file': os.path.abspath(det_path), - 'camera_id': detection_json.get('camera', {}).get('camera_id', f'cam{idx+1}'), - 'image_file': detection_json.get('image', {}).get('image_file'), - 'K': K, - 'D': D + "index": idx, + "source_file": os.path.abspath(det_path), + "camera_id": detection_json.get("camera", {}).get("camera_id", f"cam{idx+1}"), + "image_file": detection_json.get("image", {}).get("image_file"), + "image_shape": image_shape, + "K": K, + "D": D, }) - for det in detection_json.get('detections', []) or []: - marker_id = int(det.get('marker_id', -1)) + for det in detection_json.get("detections", []) or []: + if str(det.get("type", "aruco")).lower() != "aruco": + continue + marker_id = int(det.get("marker_id", -1)) if marker_id < 0 or marker_id not in robot_markers: continue - image_points = det.get('image_points_px') - if isinstance(image_points, list) and len(image_points) == 4: - image_points = np.asarray(image_points, dtype=np.float64) - else: - center = resolve_vector(det.get('center_px', [0, 0]), 2) - image_points = np.asarray([center], dtype=np.float64) + image_points = det.get("image_points_px") + if not (isinstance(image_points, list) and len(image_points) == 4): + # Phase 1 uses full marker corners only. + continue - confidence = float(det.get('confidence', 1.0)) + image_points = np.asarray(image_points, dtype=np.float64) marker = robot_markers[marker_id] + obs_quality = compute_observation_quality(det, image_shape, quality_cfg) + observations.append({ - 'view_index': idx, - 'marker_id': marker_id, - 'marker_link_corners': marker_object_corners(marker), - 'image_points_px': image_points, - 'confidence': max(0.01, min(1.0, confidence)) + "view_index": idx, + "marker_id": marker_id, + "marker_link_corners": marker_object_corners(marker), + "image_points_px": image_points, + "confidence_base": obs_quality["detector_confidence"], + "confidence": obs_quality["weighted_confidence"], + "quality": obs_quality, + "raw_detection": det, }) if len(views) == 0: - raise RuntimeError('No valid detection views found') - + raise RuntimeError("No valid detection views found") if len(observations) == 0: - raise RuntimeError('No marker observations matched robot.json markers') - + raise RuntimeError("No marker observations matched robot.json markers") return views, observations -def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: - link_transform = link_transforms[marker['link_name']] - local = marker_object_corners(marker) - homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1) - world = (link_transform @ homogeneous.T).T - return world[:, :3] - - def initial_camera_guess( view: Dict[str, Any], observations: List[Dict[str, Any]], robot_markers: Dict[int, Dict[str, Any]], default_state: Dict[str, float], scale: float, - robot: Dict[str, Any] + robot: Dict[str, Any], ) -> Tuple[np.ndarray, np.ndarray]: object_points = [] image_points = [] - link_transforms = compute_link_transforms(robot, default_state, scale) for obs in observations: - if obs['view_index'] != view['index']: + if obs["view_index"] != view["index"]: continue - marker = robot_markers[obs['marker_id']] + marker = robot_markers[obs["marker_id"]] object_points.append(compute_marker_world_corners(marker, link_transforms)) - image_points.append(obs['image_points_px']) + image_points.append(obs["image_points_px"]) if len(object_points) == 0: return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) @@ -466,9 +659,9 @@ def initial_camera_guess( success, rvec, tvec = cv2.solvePnP( object_points, image_points, - view['K'], - view['D'], - flags=cv2.SOLVEPNP_ITERATIVE + view["K"], + view["D"], + flags=cv2.SOLVEPNP_ITERATIVE, ) if not success: @@ -477,91 +670,15 @@ def initial_camera_guess( return rvec, tvec -def project_points( - points_3d: np.ndarray, - rvec: np.ndarray, - tvec: np.ndarray, - K: np.ndarray, - D: np.ndarray -) -> np.ndarray: +def project_points(points_3d: np.ndarray, rvec: np.ndarray, tvec: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray: projected, _ = cv2.projectPoints(points_3d, rvec, tvec, K, D) return projected.reshape(-1, 2) -def compute_soft_constraint_residuals( - robot_state: Dict[str, float], - robot_markers: Dict[int, Dict[str, Any]], - link_transforms: Dict[str, np.ndarray], - robot: Dict[str, Any], - enabled_constraints: Dict[str, ConstraintResult] -) -> List[float]: - """ - Compute residuals from soft constraints (kinematic consistency, rigid body distances). - Returns a list of constraint residuals to append to the total residual vector. - """ - residuals = [] - weight_scale = 0.1 # Weight for soft constraints relative to reprojection errors - - # Constraint 1: Rigid body distances within each link - if enabled_constraints['RigidBodyDistances'].enabled: - for link_name in ['Arm1', 'Ellbow', 'Arm2']: - link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] - if len(link_markers) < 2: - continue - - # Compute all pairwise distances in world coords - for i in range(len(link_markers)): - for j in range(i + 1, len(link_markers)): - m_i = link_markers[i] - m_j = link_markers[j] - - pos_i = compute_marker_world_position(m_i, link_transforms) - pos_j = compute_marker_world_position(m_j, link_transforms) - - dist_world = np.linalg.norm(pos_i - pos_j) - - # Reference distance in local coords - dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m']) - - # Residual: difference should be zero (rigid body) - error = dist_world - dist_local - residuals.append(error * weight_scale * 0.1) # Very soft weight - - # Constraint 2: Fixed X-distances between links (Arm1 <-> Ellbow) - if enabled_constraints['InterLinkXDistances'].enabled: - arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1'] - ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow'] - - if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: - # Get first marker from each link - m_arm1 = arm1_markers[0] - m_ellbow = ellbow_markers[0] - - pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) - pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) - - # X-distance in world should match reference (relative position) - # Since both rotate around X-axis at different points, we check consistency - x_diff_world = pos_ellbow[0] - pos_arm1[0] - x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0] - - error = x_diff_world - x_diff_ref - residuals.append(error * weight_scale) - - return residuals - - -def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: - """Compute the world position of a marker given current link transforms.""" - link_transform = link_transforms[marker['link_name']] - local_pos = np.concatenate([marker['position_m'], [1.0]]) - world_pos = (link_transform @ local_pos)[:3] - return world_pos - - -# ------------------------------------------------------------------ +# ----------------------------------------------------------------------------- # Optimization -# ------------------------------------------------------------------ +# ----------------------------------------------------------------------------- + def pack_parameters(robot_state: Dict[str, float], camera_params: List[Tuple[np.ndarray, np.ndarray]]) -> np.ndarray: state_vec = np.asarray([robot_state[k] for k in STATE_KEYS], dtype=np.float64) @@ -592,45 +709,35 @@ def residuals_for_parameters( robot: Dict[str, Any], scale: float, default_state: Dict[str, float], - enabled_constraints: Dict[str, ConstraintResult] + enabled_constraints: Dict[str, ConstraintResult], ) -> np.ndarray: robot_state, camera_params = unpack_parameters(params, len(views)) link_transforms = compute_link_transforms(robot, robot_state, scale) residuals = [] - - # Reprojection residuals (primary observation) + for obs in observations: - marker = robot_markers[obs['marker_id']] + marker = robot_markers[obs["marker_id"]] world_corners = compute_marker_world_corners(marker, link_transforms) - rvec, tvec = camera_params[obs['view_index']] - proj = project_points(world_corners, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) - diffs = proj - obs['image_points_px'] - weight = math.sqrt(obs['confidence']) + rvec, tvec = camera_params[obs["view_index"]] + proj = project_points(world_corners, rvec, tvec, views[obs["view_index"]]["K"], views[obs["view_index"]]["D"]) + diffs = proj - obs["image_points_px"] + weight = math.sqrt(max(float(obs["confidence"]), 1e-9)) residuals.extend((diffs * weight).reshape(-1)) - # Weak priors on robot state for key in STATE_KEYS: diff = robot_state[key] - default_state.get(key, 0.0) - if key in ('x', 'y', 'z', 'e'): - w = 0.001 - else: - w = 0.01 + w = 0.001 if key in ("x", "y", "z", "e") else 0.01 residuals.append(diff * w) - # Soft constraints (kinematic consistency, rigid body constraints) - soft_constraint_residuals = compute_soft_constraint_residuals( - robot_state, robot_markers, link_transforms, robot, enabled_constraints - ) - residuals.extend(soft_constraint_residuals) + residuals.extend(compute_soft_constraint_residuals(robot_state, robot_markers, link_transforms, robot, enabled_constraints)) return np.asarray(residuals, dtype=np.float64) - def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray: if result.jac is None: - return np.full(n_params, float('nan'), dtype=np.float64) + return np.full(n_params, float("nan"), dtype=np.float64) J = result.jac m, n = J.shape JTJ = J.T @ J @@ -645,103 +752,16 @@ def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray: return np.sqrt(np.diag(cov)) -def print_constraint_sanity_check( - robot_state: Dict[str, float], - robot_markers: Dict[int, Dict[str, Any]], - link_transforms: Dict[str, np.ndarray], - robot: Dict[str, Any], - enabled_constraints: Dict[str, ConstraintResult] -) -> None: - """ - Print sanity checks for all constraints to verify the optimization result. - """ - print("\n" + "=" * 70) - print("CONSTRAINT SANITY CHECKS (after optimization)") - print("=" * 70) - - # Check 1: Rigid body distances - if enabled_constraints['RigidBodyDistances'].enabled: - print("\n1. RIGID BODY DISTANCES") - for link_name in ['Arm1', 'Ellbow', 'Arm2']: - link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] - if len(link_markers) < 2: - continue - - max_error = 0.0 - for i in range(len(link_markers)): - for j in range(i + 1, len(link_markers)): - m_i = link_markers[i] - m_j = link_markers[j] - - pos_i = compute_marker_world_position(m_i, link_transforms) - pos_j = compute_marker_world_position(m_j, link_transforms) - - dist_world = np.linalg.norm(pos_i - pos_j) - dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m']) - error = abs(dist_world - dist_local) - max_error = max(max_error, error) - - status = "✓" if max_error < 1.0 else "⚠" if max_error < 5.0 else "✗" - print(f" {link_name:10s}: max_error = {max_error:.3f} mm {status}") - - # Check 2: Inter-link X distances - if enabled_constraints['InterLinkXDistances'].enabled: - print("\n2. INTER-LINK X-DISTANCES") - arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1'] - ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow'] - - if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: - m_arm1 = arm1_markers[0] - m_ellbow = ellbow_markers[0] - - pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) - pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) - - x_diff_world = pos_ellbow[0] - pos_arm1[0] - x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0] - error = abs(x_diff_world - x_diff_ref) - - status = "✓" if error < 1.0 else "⚠" if error < 5.0 else "✗" - print(f" Arm1 <-> Ellbow: error = {error:.3f} mm {status}") - - # Check 3: Arm2 sin(a) dependency - if enabled_constraints['Arm2SinADependency'].enabled: - print("\n3. ARM2 sin(a) DEPENDENCY (sanity check)") - arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] - if len(arm2_markers) >= 2: - # Check that markers with different Z values have different X spreads - a_rad = math.radians(robot_state['a']) - sin_a = math.sin(a_rad) - cos_a = math.cos(a_rad) - - z_variations = {} - for m in arm2_markers: - z_local = m['position_m'][2] - x_local = m['position_m'][0] - pos_world = compute_marker_world_position(m, link_transforms) - x_world = pos_world[0] - - # Expected: x_world = 90 + x_local * cos(a) - z_local * sin(a) - x_expected = 90 * (robot.get('renderingInfo', {}).get('metric', 'mm') == 'mm' and 0.09 or 0.09) + x_local * cos_a - z_local * sin_a - x_error = abs(x_world - x_expected) - - if z_local not in z_variations: - z_variations[z_local] = [] - z_variations[z_local].append(x_error) - - max_error = max(max(errors) for errors in z_variations.values()) if z_variations else 0.0 - status = "✓" if max_error < 5.0 else "⚠" if max_error < 10.0 else "⚠" - print(f" X-consistency with sin(a): max_error = {max_error:.3f} mm {status}") - print(f" (Note: this is a consistency check, not a hard constraint)") - - print("=" * 70) - - def camera_position_world(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: R, _ = cv2.Rodrigues(rvec) return (-R.T @ tvec).reshape(3) +# ----------------------------------------------------------------------------- +# Output building +# ----------------------------------------------------------------------------- + + def build_output( robot_state: Dict[str, float], state_uncertainty: np.ndarray, @@ -751,116 +771,260 @@ def build_output( robot_markers: Dict[int, Dict[str, Any]], scale: float, robot: Dict[str, Any], - robot_json_path: str + robot_json_path: str, + quality_cfg: ObservationQualityConfig, + final_cost: Optional[float] = None, + solver_status: Optional[int] = None, + solver_message: Optional[str] = None, ) -> Dict[str, Any]: link_transforms = compute_link_transforms(robot, robot_state, scale) marker_summary: Dict[int, Dict[str, Any]] = {} for marker_id, marker in robot_markers.items(): marker_summary[marker_id] = { - 'marker_id': marker_id, - 'link_name': marker['link_name'], - 'position_world_m': compute_marker_world_position(marker, link_transforms).tolist(), - 'size_m': marker['size_m'], - 'observation_count': 0, - 'mean_confidence': None, - 'mean_reprojection_error_px': None, - 'observations': [] + "marker_id": marker_id, + "link_name": marker["link_name"], + "position_world_m": compute_marker_world_position(marker, link_transforms).tolist(), + "size_m": marker["size_m"], + "observation_count": 0, + "mean_confidence": None, + "mean_detector_confidence": None, + "mean_reprojection_error_px": None, + "observations": [], } per_marker_errors: Dict[int, List[float]] = {mid: [] for mid in marker_summary} per_marker_confidences: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + per_marker_detector_conf: Dict[int, List[float]] = {mid: [] for mid in marker_summary} - link_transforms = compute_link_transforms(robot, robot_state, scale) for obs in observations: - marker_id = obs['marker_id'] + marker_id = obs["marker_id"] marker = robot_markers[marker_id] object_points_m = compute_marker_world_corners(marker, link_transforms) - rvec, tvec = camera_params[obs['view_index']] - proj = project_points(object_points_m, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) - diffs = proj - obs['image_points_px'] + rvec, tvec = camera_params[obs["view_index"]] + proj = project_points(object_points_m, rvec, tvec, views[obs["view_index"]]["K"], views[obs["view_index"]]["D"]) + diffs = proj - obs["image_points_px"] errors = np.linalg.norm(diffs, axis=1) repro_error = float(np.mean(errors)) per_marker_errors[marker_id].extend(errors.tolist()) - per_marker_confidences[marker_id].append(obs['confidence']) - marker_summary[marker_id]['observation_count'] += 1 - marker_summary[marker_id]['observations'].append({ - 'view_index': obs['view_index'], - 'source_file': views[obs['view_index']]['source_file'], - 'image_file': views[obs['view_index']]['image_file'], - 'confidence': obs['confidence'], - 'mean_reprojection_error_px': repro_error, - 'corner_reprojection_errors_px': errors.tolist() + per_marker_confidences[marker_id].append(float(obs["confidence"])) + per_marker_detector_conf[marker_id].append(float(obs["confidence_base"])) + marker_summary[marker_id]["observation_count"] += 1 + marker_summary[marker_id]["observations"].append({ + "view_index": obs["view_index"], + "source_file": views[obs["view_index"]]["source_file"], + "image_file": views[obs["view_index"]]["image_file"], + "confidence_detector": float(obs["confidence_base"]), + "confidence_weighted": float(obs["confidence"]), + "quality": obs["quality"], + "mean_reprojection_error_px": repro_error, + "corner_reprojection_errors_px": errors.tolist(), }) for marker_id, summary in marker_summary.items(): - if summary['observation_count'] > 0: - summary['mean_confidence'] = float(np.mean(per_marker_confidences[marker_id])) - summary['mean_reprojection_error_px'] = float(np.mean(per_marker_errors[marker_id])) + if summary["observation_count"] > 0: + summary["mean_confidence"] = float(np.mean(per_marker_confidences[marker_id])) + summary["mean_detector_confidence"] = float(np.mean(per_marker_detector_conf[marker_id])) + summary["mean_reprojection_error_px"] = float(np.mean(per_marker_errors[marker_id])) camera_outputs = [] for idx, view in enumerate(views): rvec, tvec = camera_params[idx] cam_pos = camera_position_world(rvec, tvec) - observed_count = sum(1 for obs in observations if obs['view_index'] == idx) + observed_count = sum(1 for obs in observations if obs["view_index"] == idx) camera_outputs.append({ - 'view_index': idx, - 'source_file': view['source_file'], - 'camera_id': view['camera_id'], - 'camera_position_world_m': cam_pos.tolist(), - 'rvec': rvec.reshape(-1).tolist(), - 'tvec': tvec.reshape(-1).tolist(), - 'intrinsics': { - 'camera_matrix': view['K'].tolist(), - 'distortion_coefficients': view['D'].reshape(-1).tolist() + "view_index": idx, + "source_file": view["source_file"], + "camera_id": view["camera_id"], + "camera_position_world_m": cam_pos.tolist(), + "rvec": rvec.reshape(-1).tolist(), + "tvec": tvec.reshape(-1).tolist(), + "intrinsics": { + "camera_matrix": view["K"].tolist(), + "distortion_coefficients": view["D"].reshape(-1).tolist(), }, - 'observation_count': observed_count + "observation_count": observed_count, }) robot_pose_output = { - 'state': {k: float(robot_state[k]) for k in STATE_KEYS}, - 'uncertainty': { - 'x_mm': float(state_uncertainty[0]), - 'y_mm': float(state_uncertainty[1]), - 'z_mm': float(state_uncertainty[2]), - 'a_deg': float(state_uncertainty[3]), - 'b_deg': float(state_uncertainty[4]), - 'c_deg': float(state_uncertainty[5]), - 'e_mm': float(state_uncertainty[6]) + "state": {k: float(robot_state[k]) for k in STATE_KEYS}, + "uncertainty": { + "x_mm": float(state_uncertainty[0]), + "y_mm": float(state_uncertainty[1]), + "z_mm": float(state_uncertainty[2]), + "a_deg": float(state_uncertainty[3]), + "b_deg": float(state_uncertainty[4]), + "c_deg": float(state_uncertainty[5]), + "e_mm": float(state_uncertainty[6]), + }, + "confidence": { + "x": float(math.exp(-state_uncertainty[0] / 10.0)) if np.isfinite(state_uncertainty[0]) else 0.0, + "y": float(math.exp(-state_uncertainty[1] / 10.0)) if np.isfinite(state_uncertainty[1]) else 0.0, + "z": float(math.exp(-state_uncertainty[2] / 10.0)) if np.isfinite(state_uncertainty[2]) else 0.0, + "a": float(math.exp(-state_uncertainty[3] / 10.0)) if np.isfinite(state_uncertainty[3]) else 0.0, + "b": float(math.exp(-state_uncertainty[4] / 10.0)) if np.isfinite(state_uncertainty[4]) else 0.0, + "c": float(math.exp(-state_uncertainty[5] / 10.0)) if np.isfinite(state_uncertainty[5]) else 0.0, + "e": float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6]))) if np.isfinite(state_uncertainty[6]) else 0.0, }, - 'confidence': { - 'x': float(math.exp(-state_uncertainty[0] / 10.0)), - 'y': float(math.exp(-state_uncertainty[1] / 10.0)), - 'z': float(math.exp(-state_uncertainty[2] / 10.0)), - 'a': float(math.exp(-state_uncertainty[3] / 10.0)), - 'b': float(math.exp(-state_uncertainty[4] / 10.0)), - 'c': float(math.exp(-state_uncertainty[5] / 10.0)), - 'e': float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6]))) - } } + all_conf = np.asarray([obs["confidence"] for obs in observations], dtype=np.float64) + all_det_conf = np.asarray([obs["confidence_base"] for obs in observations], dtype=np.float64) + all_q_size = np.asarray([obs["quality"]["q"]["size"] for obs in observations], dtype=np.float64) + all_q_aspect = np.asarray([obs["quality"]["q"]["aspect"] for obs in observations], dtype=np.float64) + all_q_border = np.asarray([obs["quality"]["q"]["border"] for obs in observations], dtype=np.float64) + all_q_homography = np.asarray([obs["quality"]["q"]["homography"] for obs in observations], dtype=np.float64) + + all_errors = [] + for marker in marker_summary.values(): + if marker["mean_reprojection_error_px"] is not None: + all_errors.append(marker["mean_reprojection_error_px"]) + + statistics = { + "observation_count": len(observations), + "camera_count": len(views), + "marker_count": len(robot_markers), + "observed_marker_count": int(sum(1 for m in marker_summary.values() if m["observation_count"] > 0)), + "mean_detector_confidence": float(np.mean(all_det_conf)) if len(all_det_conf) else None, + "mean_weighted_confidence": float(np.mean(all_conf)) if len(all_conf) else None, + "mean_reprojection_error_px": float(np.mean(all_errors)) if len(all_errors) else None, + "quality_means": { + "size": float(np.mean(all_q_size)) if len(all_q_size) else None, + "aspect": float(np.mean(all_q_aspect)) if len(all_q_aspect) else None, + "border": float(np.mean(all_q_border)) if len(all_q_border) else None, + "homography": float(np.mean(all_q_homography)) if len(all_q_homography) else None, + }, + "quality_config": { + "size_ref_px": quality_cfg.size_ref_px, + "border_ref_px": quality_cfg.border_ref_px, + "center_ref_norm": quality_cfg.center_ref_norm, + "sharpness_ref": quality_cfg.sharpness_ref, + "homography_ref": quality_cfg.homography_ref, + "size_factor": quality_cfg.size_factor, + "aspect_factor": quality_cfg.aspect_factor, + "border_factor": quality_cfg.border_factor, + "center_factor": quality_cfg.center_factor, + "sharpness_factor": quality_cfg.sharpness_factor, + "homography_factor": quality_cfg.homography_factor, + }, + } + + output = { + "schema_version": "1.0", + "created_utc": _dt.datetime.utcnow().isoformat() + "Z", + "source_robot_json": os.path.abspath(robot_json_path), + "source_detections": [view["source_file"] for view in views], + "robot_pose": robot_pose_output, + "camera_poses": camera_outputs, + "marker_positions": list(marker_summary.values()), + "statistics": statistics, + "solver": { + "final_cost": final_cost, + "status": solver_status, + "message": solver_message, + }, + } + return output + + +def build_summary(output: Dict[str, Any]) -> Dict[str, Any]: return { - 'schema_version': '1.0', - 'created_utc': datetime.datetime.utcnow().isoformat() + 'Z', - 'source_robot_json': os.path.abspath(robot_json_path), - 'source_detections': [view['source_file'] for view in views], - 'robot_pose': robot_pose_output, - 'camera_poses': camera_outputs, - 'marker_positions': list(marker_summary.values()) + "schema_version": output.get("schema_version"), + "created_utc": output.get("created_utc"), + "source_robot_json": output.get("source_robot_json"), + "source_detections": output.get("source_detections"), + "solver": output.get("solver", {}), + "robot_pose": output.get("robot_pose"), + "statistics": output.get("statistics", {}), } -# ------------------------------------------------------------------ +# ----------------------------------------------------------------------------- +# Diagnostics +# ----------------------------------------------------------------------------- + + +def print_constraint_sanity_check( + robot_state: Dict[str, float], + robot_markers: Dict[int, Dict[str, Any]], + link_transforms: Dict[str, np.ndarray], + robot: Dict[str, Any], + enabled_constraints: Dict[str, ConstraintResult], + scale: float, +) -> None: + print("\n" + "=" * 70) + print("CONSTRAINT SANITY CHECKS (after optimization)") + print("=" * 70) + + if enabled_constraints["RigidBodyDistances"].enabled: + print("\n1. RIGID BODY DISTANCES") + for link_name in ["Arm1", "Ellbow", "Arm2"]: + link_markers = [m for m in robot_markers.values() if m["link_name"] == link_name] + if len(link_markers) < 2: + continue + max_error = 0.0 + for i in range(len(link_markers)): + for j in range(i + 1, len(link_markers)): + m_i = link_markers[i] + m_j = link_markers[j] + pos_i = compute_marker_world_position(m_i, link_transforms) + pos_j = compute_marker_world_position(m_j, link_transforms) + dist_world = np.linalg.norm(pos_i - pos_j) + dist_local = np.linalg.norm(m_i["position_m"] - m_j["position_m"]) + max_error = max(max_error, abs(dist_world - dist_local)) + status = "✓" if max_error < 1.0 else "⚠" if max_error < 5.0 else "✗" + print(f" {link_name:10s}: max_error = {max_error:.3f} mm {status}") + + if enabled_constraints["InterLinkXDistances"].enabled: + print("\n2. INTER-LINK X-DISTANCES") + arm1_markers = [m for m in robot_markers.values() if m["link_name"] == "Arm1"] + ellbow_markers = [m for m in robot_markers.values() if m["link_name"] == "Ellbow"] + if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: + m_arm1 = arm1_markers[0] + m_ellbow = ellbow_markers[0] + pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) + pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) + x_diff_world = pos_ellbow[0] - pos_arm1[0] + x_diff_ref = m_ellbow["position_m"][0] - m_arm1["position_m"][0] + error = abs(x_diff_world - x_diff_ref) + status = "✓" if error < 1.0 else "⚠" if error < 5.0 else "✗" + print(f" Arm1 <-> Ellbow: error = {error:.3f} mm {status}") + + if enabled_constraints["Arm2SinADependency"].enabled: + print("\n3. ARM2 sin(a) DEPENDENCY (sanity check)") + arm2_markers = [m for m in robot_markers.values() if m["link_name"] == "Arm2"] + if len(arm2_markers) >= 2: + a_rad = math.radians(robot_state["a"]) + sin_a = math.sin(a_rad) + cos_a = math.cos(a_rad) + max_error = 0.0 + # This remains only a qualitative check. + for m in arm2_markers: + pos_world = compute_marker_world_position(m, link_transforms) + x_world = pos_world[0] + x_local = m["position_m"][0] + z_local = m["position_m"][2] + x_expected = (90.0 * scale) + x_local * cos_a - z_local * sin_a + max_error = max(max_error, abs(x_world - x_expected)) + status = "✓" if max_error < 5.0 else "⚠" + print(f" X-consistency with sin(a): max_error = {max_error:.3f} mm {status}") + print(" (Note: this is a consistency check, not a hard constraint)") + + print("=" * 70) + + +# ----------------------------------------------------------------------------- # Main -# ------------------------------------------------------------------ +# ----------------------------------------------------------------------------- + def main() -> None: - parser = argparse.ArgumentParser(description='Multiview optimization of robot pose and camera extrinsics') - parser.add_argument('--robot', required=True, help='Path to robot.json') - parser.add_argument('--detections', required=True, nargs='+', help='List of detection JSON files') - parser.add_argument('--outDir', required=True, help='Output directory') - parser.add_argument('--write-summary', action='store_true', help='Write summary file') - parser.add_argument('--max-iter', type=int, default=500, help='Maximum optimizer iterations') + parser = argparse.ArgumentParser(description="Multiview optimization of robot pose and camera extrinsics") + parser.add_argument("--robot", required=True, help="Path to robot.json") + parser.add_argument("--detections", required=True, nargs="+", help="List of detection JSON files") + parser.add_argument("--outDir", required=True, help="Output directory") + parser.add_argument("--write-summary", action="store_true", help="Write summary file") + parser.add_argument("--max-iter", type=int, default=500, help="Maximum optimizer iterations") args = parser.parse_args() os.makedirs(args.outDir, exist_ok=True) @@ -868,24 +1032,43 @@ def main() -> None: robot_json_path = os.path.abspath(args.robot) robot = load_json(robot_json_path) scale = parse_metric_scale(robot) + quality_cfg = load_quality_config(robot) - default_state = { - k: float(robot.get('defaultPosition', {}).get(k, 0.0) or 0.0) - for k in STATE_KEYS - } - + default_state = {k: float(robot.get("defaultPosition", {}).get(k, 0.0) or 0.0) for k in STATE_KEYS} robot_markers = extract_markers(robot, scale) - - # Validate constraints + print("\n" + "=" * 70) print("CONSTRAINT VALIDATION") print("=" * 70) enabled_constraints = validate_constraints(robot, robot_markers) - for constraint_name, result in enabled_constraints.items(): + for _, result in enabled_constraints.items(): print(result) print("=" * 70) - - views, observations = collect_views_and_observations(args.detections, robot_markers) + + views, observations = collect_views_and_observations(args.detections, robot_markers, quality_cfg) + + print("\n" + "=" * 70) + print("OBSERVATION QUALITY SUMMARY") + print("=" * 70) + print(f"Total observations: {len(observations)}") + print() + + quality_by_marker: Dict[int, List[Dict[str, Any]]] = {} + for obs in observations: + quality_by_marker.setdefault(obs["marker_id"], []).append(obs["quality"]) + + print(f"{'Marker':>8} {'Link':>12} {'Count':>6} {'Avg Size':>10} {'Avg Aspec':>10} {'Avg Hmg.':>10} {'Avg Conf.':>10}") + print("-" * 74) + for marker_id in sorted(quality_by_marker.keys()): + marker = robot_markers[marker_id] + qlist = quality_by_marker[marker_id] + avg_size = float(np.mean([q["q"]["size"] for q in qlist])) + avg_aspect = float(np.mean([q["q"]["aspect"] for q in qlist])) + avg_homog = float(np.mean([q["q"]["homography"] for q in qlist])) + obs_for_marker = [o for o in observations if o["marker_id"] == marker_id] + avg_conf = float(np.mean([o["confidence"] for o in obs_for_marker])) + print(f"{marker_id:8d} {marker['link_name']:>12} {len(qlist):6d} {avg_size:10.3f} {avg_aspect:10.3f} {avg_homog:10.3f} {avg_conf:10.3f}") + print("=" * 70) camera_guesses = [] for view in views: @@ -895,77 +1078,78 @@ def main() -> None: x0 = pack_parameters(default_state, camera_guesses) progress = { - 'iter': 0, - 'last_cost': None, - 'last_print': time.time(), - 'prev_x': x0.copy() + "iter": 0, + "last_cost": None, + "last_print": time.time(), + "prev_x": x0.copy(), } def progress_callback(xk: np.ndarray) -> None: - progress['iter'] += 1 + progress["iter"] += 1 now = time.time() - if progress['iter'] == 1 or now - progress['last_print'] >= 1.0: + if progress["iter"] == 1 or now - progress["last_print"] >= 1.0: res = residuals_for_parameters(xk, views, observations, robot_markers, robot, scale, default_state, enabled_constraints) cost = 0.5 * float(np.dot(res, res)) delta_cost = None - convergence = '' - if progress['last_cost'] is not None: - delta_cost = cost - progress['last_cost'] + convergence = "" + if progress["last_cost"] is not None: + delta_cost = cost - progress["last_cost"] if abs(delta_cost) < 1e-3: - convergence = ' stable' + convergence = " stable" elif delta_cost < 0: - convergence = ' improving' + convergence = " improving" else: - convergence = ' worsening' - step_norm = float(np.linalg.norm(xk - progress['prev_x'])) - print( - f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}' - + (f' delta={delta_cost:.4g}' if delta_cost is not None else '') - + f' step={step_norm:.4g}' - + convergence - ) - progress['last_cost'] = cost - progress['last_print'] = now - progress['prev_x'] = xk.copy() + convergence = " worsening" + step_norm = float(np.linalg.norm(xk - progress["prev_x"])) + print(f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}' + (f' delta={delta_cost:.4g}' if delta_cost is not None else "") + f' step={step_norm:.4g}' + convergence) + progress["last_cost"] = cost + progress["last_print"] = now + progress["prev_x"] = xk.copy() result = least_squares( residuals_for_parameters, x0, args=(views, observations, robot_markers, robot, scale, default_state, enabled_constraints), - jac='2-point', - method='trf', - loss='soft_l1', + jac="2-point", + method="trf", + loss="soft_l1", f_scale=1.0, max_nfev=args.max_iter, - callback=progress_callback + callback=progress_callback, ) robot_state, camera_params = unpack_parameters(result.x, len(views)) uncertainties = estimate_uncertainty(result, len(result.x)) - - # Print constraint sanity checks + link_transforms = compute_link_transforms(robot, robot_state, scale) - print_constraint_sanity_check(robot_state, robot_markers, link_transforms, robot, enabled_constraints) + print_constraint_sanity_check(robot_state, robot_markers, link_transforms, robot, enabled_constraints, scale) - output = build_output(robot_state, uncertainties[:len(STATE_KEYS)], views, camera_params, observations, robot_markers, scale, robot, robot_json_path) + output = build_output( + robot_state, + uncertainties[:len(STATE_KEYS)], + views, + camera_params, + observations, + robot_markers, + scale, + robot, + robot_json_path, + quality_cfg, + final_cost=float(result.cost), + solver_status=int(result.status), + solver_message=str(result.message), + ) - out_path = Path(args.outDir) / 'multiview_pose.json' + out_path = Path(args.outDir) / "multiview_pose.json" save_json(output, out_path) + print(f"Saved: {out_path}") - print(f'Saved: {out_path}') if args.write_summary: - summary_path = Path(args.outDir) / 'multiview_pose_summary.json' - summary = { - 'final_cost': float(result.cost), - 'status': int(result.status), - 'message': result.message, - 'robot_state': output['robot_pose'], - 'camera_count': len(views), - 'marker_count': len(robot_markers) - } + summary_path = Path(args.outDir) / "multiview_pose_summary.json" + summary = build_summary(output) save_json(summary, summary_path) - print(f'Saved: {summary_path}') + print(f"Saved: {summary_path}") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/pipeline/2_Multiview_Weighted.py b/pipeline/2_Multiview_Weighted.py new file mode 100644 index 0000000..fa8d461 --- /dev/null +++ b/pipeline/2_Multiview_Weighted.py @@ -0,0 +1,1079 @@ +#!/usr/bin/env python3 + +""" +============================================================ +STEP 2b — Simultane Multiview-Optimierung für Roboterpose +============================================================ + +Ziel: + Aus mehreren ArUco-Detektionsdateien die gemeinsame + Roboterpose (x,y,z,a,b,c,e) schätzen und jede Kamera-Pose + sowie Marker-Weltpositionen ausgeben. + +Eingabe: + --robot ../robot.json + --detections render_1a_aruco_detection.json render_1b_aruco_detection.json ... + --outDir . + +Ausgabe: + multiview_pose.json + +Hinweis: + Dieses Skript verwendet die Markerpositionen aus robot.json + als kinematische Constraints und optimiert gleichzeitig: + - Roboterzustand (x,y,z,a,b,c,e) + - Kameraextrinsische Parameter pro Bild + +""" + +import argparse +import datetime +import json +import math +import os +import time +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import cv2 +import numpy as np +from scipy.optimize import least_squares + +STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"] + +# ------------------------------------------------------------------ +# Constraint definitions and validation +# ------------------------------------------------------------------ + +class ConstraintResult: + """Result of validating/applying a single constraint""" + def __init__(self, name: str, enabled: bool, reason: str = ""): + self.name = name + self.enabled = enabled + self.reason = reason + self.residuals = [] + + def __str__(self) -> str: + status = "✓ ENABLED" if self.enabled else "✗ DISABLED" + return f"{self.name:40s} {status:12s} {self.reason}" + + +def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]: + """ + Validate which constraints can be applied based on robot geometry. + Returns a dict of constraint_name -> ConstraintResult + """ + results = {} + + # --- Constraint 1: Rigid body distances within each link --- + rigid_body_result = ConstraintResult("RigidBodyDistances", False) + try: + rigid_body_count = 0 + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) >= 2: + rigid_body_count += 1 + if rigid_body_count >= 2: + rigid_body_result.enabled = True + rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each" + else: + rigid_body_result.reason = "Not enough rigid links with multiple markers" + except Exception as e: + rigid_body_result.reason = f"Error: {str(e)}" + results['RigidBodyDistances'] = rigid_body_result + + # --- Constraint 2: Fixed X-distances between links (rotation around X-axis) --- + inter_link_x_result = ConstraintResult("InterLinkXDistances", False) + try: + links_with_markers = set(m['link_name'] for m in robot_markers.values()) + x_rotated_links = [] + for link_name in ['Arm1', 'Ellbow']: + if link_name in links_with_markers: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) >= 1: + x_rotated_links.append(link_name) + if len(x_rotated_links) >= 2: + inter_link_x_result.enabled = True + inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}" + else: + inter_link_x_result.reason = "Not enough X-rotation links" + except Exception as e: + inter_link_x_result.reason = f"Error: {str(e)}" + results['InterLinkXDistances'] = inter_link_x_result + + # --- Sanity check (not a hard constraint): Arm2 sin(a) dependency --- + arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)") + try: + arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] + if len(arm2_markers) >= 2: + z_values = set(m['position_m'][2] for m in arm2_markers) + if len(z_values) > 1: + arm2_sina_result.enabled = True + arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "Not enough Arm2 markers" + except Exception as e: + arm2_sina_result.reason = f"Error: {str(e)}" + results['Arm2SinADependency'] = arm2_sina_result + + return results + + +# ------------------------------------------------------------------ +# JSON helpers +# ------------------------------------------------------------------ + +def load_json(path: str) -> Dict[str, Any]: + with open(path, 'r', encoding='utf-8') as f: + return json.load(f) + + +def save_json(data: Dict[str, Any], path: Path) -> None: + with open(path, 'w', encoding='utf-8') as f: + json.dump(data, f, indent=2) + + +# ------------------------------------------------------------------ +# robot.json helpers +# ------------------------------------------------------------------ + +def resolve_scalar(value: Any, default: float = 0.0) -> float: + if value is None: + return default + if isinstance(value, (int, float)): + return float(value) + try: + return float(str(value).strip()) + except ValueError: + return default + + +def resolve_vector(value: Any, default_len: int = 3) -> Tuple[float, ...]: + if value is None: + return tuple(0.0 for _ in range(default_len)) + if isinstance(value, (int, float, str)): + return (resolve_scalar(value),) + tuple(0.0 for _ in range(default_len - 1)) + if isinstance(value, (list, tuple)): + resolved = [resolve_scalar(v) for v in value] + if len(resolved) < default_len: + resolved.extend([0.0] * (default_len - len(resolved))) + return tuple(resolved[:default_len]) + return tuple(0.0 for _ in range(default_len)) + + +def parse_metric_scale(robot: Dict[str, Any]) -> float: + rendering_info = robot.get('renderingInfo', {}) or {} + metric = rendering_info.get('metric', 'mm') + return 0.001 if str(metric).strip().lower() == 'mm' else 1.0 + + +def normalize_axis(axis: Any) -> np.ndarray: + vec = np.asarray(axis, dtype=np.float64) + if vec.shape != (3,): + vec = vec.reshape(-1)[:3] + norm = np.linalg.norm(vec) + return vec / max(norm, 1e-9) + + +def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray: + x_deg, y_deg, z_deg = resolve_vector(euler_deg, 3) + x = math.radians(x_deg) + y = math.radians(y_deg) + z = math.radians(z_deg) + + cx = math.cos(x) + sx = math.sin(x) + cy = math.cos(y) + sy = math.sin(y) + cz = math.cos(z) + sz = math.sin(z) + + Rx = np.array([ + [1.0, 0.0, 0.0], + [0.0, cx, -sx], + [0.0, sx, cx] + ], dtype=np.float64) + + Ry = np.array([ + [cy, 0.0, sy], + [0.0, 1.0, 0.0], + [-sy, 0.0, cy] + ], dtype=np.float64) + + Rz = np.array([ + [cz, -sz, 0.0], + [sz, cz, 0.0], + [0.0, 0.0, 1.0] + ], dtype=np.float64) + + return Rz @ Ry @ Rx + + +def transform_from_translation_rotation(translation: Any, rotation_deg: Any) -> np.ndarray: + T = np.eye(4, dtype=np.float64) + pos = np.asarray(resolve_vector(translation, 3), dtype=np.float64) + T[:3, 3] = pos + T[:3, :3] = euler_deg_to_matrix(rotation_deg) + return T + + +def axis_angle_matrix(axis: Any, angle_deg: float) -> np.ndarray: + axis_vec = normalize_axis(axis) + theta = math.radians(angle_deg) + kx, ky, kz = axis_vec + c = math.cos(theta) + s = math.sin(theta) + v = 1.0 - c + R = np.array([ + [kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s], + [ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s], + [kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c] + ], dtype=np.float64) + T = np.eye(4, dtype=np.float64) + T[:3, :3] = R + return T + + +# ------------------------------------------------------------------ +# Kinematics and marker extraction +# ------------------------------------------------------------------ + +def extract_markers(robot: Dict[str, Any], scale: float) -> Dict[int, Dict[str, Any]]: + markers = {} + links = robot.get('links', {}) or {} + marker_defaults = (robot.get('renderingInfo', {}) or {}).get('markerDefaults', {}) or {} + default_size_mm = float(marker_defaults.get('size', 25.0)) + + for link_name, link_info in links.items(): + for marker in link_info.get('markers', []) or []: + marker_id = int(marker.get('id', -1)) + if marker_id < 0: + continue + + pos = resolve_vector(marker.get('position', [0, 0, 0]), 3) + size_mm = float(marker.get('size', default_size_mm)) + markers[marker_id] = { + 'marker_id': marker_id, + 'link_name': link_name, + 'position_m': np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64), + 'normal': normalize_axis(resolve_vector(marker.get('normal', [0, 0, 1]), 3)), + 'spin_deg': float(marker.get('spin', 0.0)), + 'size_m': size_mm * scale, + } + + return markers + + +def compute_marker_observation_quality( + corners_px: np.ndarray, + image_shape: Tuple[int, int] +) -> Dict[str, float]: + """ + Compute quality metrics for a marker observation based on: + - Orientation/distortion in the image + - Size (distance) to camera + + Returns dict with: + - 'orientation_quality': 0..1 (1 = frontal, 0 = very skewed) + - 'size_quality': 0..1 (1 = reasonable size, 0 = too small or too large) + - 'combined_quality': product of both + """ + if corners_px.shape[0] != 4: + return { + 'orientation_quality': 0.5, + 'size_quality': 0.5, + 'combined_quality': 0.25 + } + + # Compute edge lengths + corners = corners_px.astype(np.float64) + edges = [ + np.linalg.norm(corners[(i+1) % 4] - corners[i]) + for i in range(4) + ] + + # Orientation quality: low aspect ratio means frontal (1.0 = square) + aspect_ratio = max(edges) / (min(edges) + 1e-6) + # For aspect ratio 1.0->1, 1.5->0.8, 3.0->0.4, 5.0->0.2 + orientation_quality = max(0.1, 2.0 / (1.0 + aspect_ratio)) + + # Size quality: area-based + area_px = cv2.contourArea(corners.astype(np.int32)) + image_area = image_shape[0] * image_shape[1] + + # Ideal: marker should be 5-20% of image area + # Too small: <1%, too large: >40% + area_ratio = area_px / image_area + + if area_ratio < 0.01: + size_quality = 0.3 + area_ratio * 20 # linear growth 0-0.3 + elif area_ratio > 0.4: + size_quality = 0.3 + (1.0 - area_ratio) * 2.33 # linear decay from 1.0 + else: + # 1% to 40%: bell curve, max at ~5-20% + ideal_ratio = 0.10 + size_quality = 1.0 - (area_ratio - ideal_ratio) ** 2 / (0.15 ** 2) + size_quality = max(0.3, min(1.0, size_quality)) + + combined_quality = orientation_quality * size_quality + + return { + 'orientation_quality': float(orientation_quality), + 'size_quality': float(size_quality), + 'combined_quality': float(combined_quality) + } + + +def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, np.ndarray]: + n = normalize_axis(normal) + candidate = np.array((0.0, 0.0, 1.0), dtype=np.float64) + if abs(np.dot(n, candidate)) > 0.99: + candidate = np.array((1.0, 0.0, 0.0), dtype=np.float64) + + x_dir = np.cross(candidate, n) + x_dir /= max(np.linalg.norm(x_dir), 1e-9) + y_dir = np.cross(n, x_dir) + + if abs(spin_deg) > 1e-6: + theta = math.radians(spin_deg) + cos_t = math.cos(theta) + sin_t = math.sin(theta) + x_rot = x_dir * cos_t + np.cross(n, x_dir) * sin_t + n * np.dot(n, x_dir) * (1.0 - cos_t) + y_rot = y_dir * cos_t + np.cross(n, y_dir) * sin_t + n * np.dot(n, y_dir) * (1.0 - cos_t) + return x_rot, y_rot + + return x_dir, y_dir + + +def marker_object_corners(marker: Dict[str, Any]) -> np.ndarray: + half = marker['size_m'] * 0.5 + x_dir, y_dir = marker_plane_axes(marker['normal'], marker['spin_deg']) + corners = np.stack([ + -x_dir * half + y_dir * half, + x_dir * half + y_dir * half, + x_dir * half - y_dir * half, + -x_dir * half - y_dir * half + ], axis=0) + return marker['position_m'].reshape(1, 3) + corners + + +def build_link_chain(robot: Dict[str, Any]) -> List[str]: + links = robot.get('links', {}) or {} + ordered: List[str] = [] + remaining = set(links.keys()) + + while remaining: + progress = False + for name in list(remaining): + parent = links[name].get('parent') + if not parent or parent in ordered: + ordered.append(name) + remaining.remove(name) + progress = True + if not progress: + raise RuntimeError('Cycle detected in robot link tree or missing parent link') + return ordered + + +def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]: + links = robot.get('links', {}) or {} + ordered_links = build_link_chain(robot) + transforms: Dict[str, np.ndarray] = {} + + for link_name in ordered_links: + link_info = links[link_name] or {} + parent_name = link_info.get('parent') + parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64) + + mount_translation = np.asarray(resolve_vector(link_info.get('mountPosition', [0, 0, 0]), 3), dtype=np.float64) * scale + mount = transform_from_translation_rotation( + mount_translation, + link_info.get('mountRotation', [0, 0, 0]) + ) + + joint_info = link_info.get('jointToParent', {}) or {} + joint_origin = np.asarray(resolve_vector(joint_info.get('origin', [0, 0, 0]), 3), dtype=np.float64) * scale + joint = transform_from_translation_rotation( + joint_origin, + joint_info.get('rotation', [0, 0, 0]) + ) + + motion = np.eye(4, dtype=np.float64) + joint_type = str(joint_info.get('type', 'fixed')).strip().lower() + control_var = str(joint_info.get('variable', joint_info.get('control', ''))).strip().lower() + axis = resolve_vector(joint_info.get('axis', [1, 0, 0]), 3) + + if joint_type == 'linear': + motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale + elif joint_type == 'revolute': + motion = axis_angle_matrix(axis, state.get(control_var, 0.0)) + + transforms[link_name] = parent_transform @ mount @ joint @ motion + + return transforms + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = np.ones(4, dtype=np.float64) + local[:3] = marker['position_m'] + world = link_transform @ local + return world[:3] + + +# ------------------------------------------------------------------ +# Camera / observation helpers +# ------------------------------------------------------------------ + +def load_intrinsics(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: + cam = detection_json['camera'] + K = np.asarray(cam['camera_matrix'], dtype=np.float64) + D = np.asarray(cam.get('distortion_coefficients', [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1) + return K, D + + +def collect_views_and_observations( + detection_files: List[str], + robot_markers: Dict[int, Dict[str, Any]] +) -> Tuple[List[Dict[str, Any]], List[Dict[str, Any]]]: + views: List[Dict[str, Any]] = [] + observations: List[Dict[str, Any]] = [] + + for idx, det_path in enumerate(detection_files): + detection_json = load_json(det_path) + K, D = load_intrinsics(detection_json) + image_shape = detection_json.get('image', {}).get('image_shape') + if not image_shape: + image_shape = (720, 1280) # fallback + + views.append({ + 'index': idx, + 'source_file': os.path.abspath(det_path), + 'camera_id': detection_json.get('camera', {}).get('camera_id', f'cam{idx+1}'), + 'image_file': detection_json.get('image', {}).get('image_file'), + 'image_shape': image_shape, + 'K': K, + 'D': D + }) + + for det in detection_json.get('detections', []) or []: + marker_id = int(det.get('marker_id', -1)) + if marker_id < 0 or marker_id not in robot_markers: + continue + + image_points = det.get('image_points_px') + if isinstance(image_points, list) and len(image_points) == 4: + image_points = np.asarray(image_points, dtype=np.float64) + else: + center = resolve_vector(det.get('center_px', [0, 0]), 2) + image_points = np.asarray([center], dtype=np.float64) + + confidence = float(det.get('confidence', 1.0)) + + # Compute observation quality metrics + quality_metrics = compute_marker_observation_quality(image_points, tuple(image_shape)) + + # Blend base confidence and observation quality instead of multiplying them. + # Reduce the distance/size-based quality influence by roughly 40% relative to the previous blend. + base_confidence = max(0.01, min(1.0, confidence)) + combined_quality = quality_metrics['combined_quality'] + combined_confidence = base_confidence * 0.76 + combined_quality * 0.24 + + marker = robot_markers[marker_id] + observations.append({ + 'view_index': idx, + 'marker_id': marker_id, + 'marker_link_corners': marker_object_corners(marker), + 'image_points_px': image_points, + 'confidence_base': base_confidence, + 'quality_metrics': quality_metrics, + 'confidence': max(0.01, min(1.0, combined_confidence)) + }) + + if len(views) == 0: + raise RuntimeError('No valid detection views found') + + if len(observations) == 0: + raise RuntimeError('No marker observations matched robot.json markers') + + return views, observations + + +def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = marker_object_corners(marker) + homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1) + world = (link_transform @ homogeneous.T).T + return world[:, :3] + + +def initial_camera_guess( + view: Dict[str, Any], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + default_state: Dict[str, float], + scale: float, + robot: Dict[str, Any] +) -> Tuple[np.ndarray, np.ndarray]: + object_points = [] + image_points = [] + + link_transforms = compute_link_transforms(robot, default_state, scale) + + for obs in observations: + if obs['view_index'] != view['index']: + continue + marker = robot_markers[obs['marker_id']] + object_points.append(compute_marker_world_corners(marker, link_transforms)) + image_points.append(obs['image_points_px']) + + if len(object_points) == 0: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + object_points = np.vstack(object_points) + image_points = np.vstack(image_points) + + if object_points.shape[0] < 4: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + success, rvec, tvec = cv2.solvePnP( + object_points, + image_points, + view['K'], + view['D'], + flags=cv2.SOLVEPNP_ITERATIVE + ) + + if not success: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + return rvec, tvec + + +def project_points( + points_3d: np.ndarray, + rvec: np.ndarray, + tvec: np.ndarray, + K: np.ndarray, + D: np.ndarray +) -> np.ndarray: + projected, _ = cv2.projectPoints(points_3d, rvec, tvec, K, D) + return projected.reshape(-1, 2) + + +def compute_soft_constraint_residuals( + robot_state: Dict[str, float], + robot_markers: Dict[int, Dict[str, Any]], + link_transforms: Dict[str, np.ndarray], + robot: Dict[str, Any], + enabled_constraints: Dict[str, ConstraintResult] +) -> List[float]: + """ + Compute residuals from soft constraints (kinematic consistency, rigid body distances). + Returns a list of constraint residuals to append to the total residual vector. + """ + residuals = [] + weight_scale = 0.1 # Weight for soft constraints relative to reprojection errors + + # Constraint 1: Rigid body distances within each link + if enabled_constraints['RigidBodyDistances'].enabled: + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) < 2: + continue + + # Compute all pairwise distances in world coords + for i in range(len(link_markers)): + for j in range(i + 1, len(link_markers)): + m_i = link_markers[i] + m_j = link_markers[j] + + pos_i = compute_marker_world_position(m_i, link_transforms) + pos_j = compute_marker_world_position(m_j, link_transforms) + + dist_world = np.linalg.norm(pos_i - pos_j) + + # Reference distance in local coords + dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m']) + + # Residual: difference should be zero (rigid body) + error = dist_world - dist_local + residuals.append(error * weight_scale * 0.1) # Very soft weight + + # Constraint 2: Fixed X-distances between links (Arm1 <-> Ellbow) + if enabled_constraints['InterLinkXDistances'].enabled: + arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1'] + ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow'] + + if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: + # Get first marker from each link + m_arm1 = arm1_markers[0] + m_ellbow = ellbow_markers[0] + + pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) + pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) + + # X-distance in world should match reference (relative position) + # Since both rotate around X-axis at different points, we check consistency + x_diff_world = pos_ellbow[0] - pos_arm1[0] + x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0] + + error = x_diff_world - x_diff_ref + residuals.append(error * weight_scale) + + return residuals + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + """Compute the world position of a marker given current link transforms.""" + link_transform = link_transforms[marker['link_name']] + local_pos = np.concatenate([marker['position_m'], [1.0]]) + world_pos = (link_transform @ local_pos)[:3] + return world_pos + + +# ------------------------------------------------------------------ +# Optimization +# ------------------------------------------------------------------ + +def pack_parameters(robot_state: Dict[str, float], camera_params: List[Tuple[np.ndarray, np.ndarray]]) -> np.ndarray: + state_vec = np.asarray([robot_state[k] for k in STATE_KEYS], dtype=np.float64) + cams = [] + for rvec, tvec in camera_params: + cams.append(rvec.reshape(3)) + cams.append(tvec.reshape(3)) + return np.concatenate([state_vec] + cams) + + +def unpack_parameters(params: np.ndarray, n_views: int) -> Tuple[Dict[str, float], List[Tuple[np.ndarray, np.ndarray]]]: + robot_state = {STATE_KEYS[i]: float(params[i]) for i in range(len(STATE_KEYS))} + camera_params = [] + offset = len(STATE_KEYS) + for _ in range(n_views): + rvec = params[offset:offset + 3].reshape(3, 1) + tvec = params[offset + 3:offset + 6].reshape(3, 1) + camera_params.append((rvec, tvec)) + offset += 6 + return robot_state, camera_params + + +def residuals_for_parameters( + params: np.ndarray, + views: List[Dict[str, Any]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + robot: Dict[str, Any], + scale: float, + default_state: Dict[str, float], + enabled_constraints: Dict[str, ConstraintResult] +) -> np.ndarray: + robot_state, camera_params = unpack_parameters(params, len(views)) + link_transforms = compute_link_transforms(robot, robot_state, scale) + + residuals = [] + + # Reprojection residuals (primary observation) + for obs in observations: + marker = robot_markers[obs['marker_id']] + world_corners = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(world_corners, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + weight = math.sqrt(obs['confidence']) + residuals.extend((diffs * weight).reshape(-1)) + + # Weak priors on robot state + for key in STATE_KEYS: + diff = robot_state[key] - default_state.get(key, 0.0) + if key in ('x', 'y', 'z', 'e'): + w = 0.001 + else: + w = 0.01 + residuals.append(diff * w) + + # Soft constraints (kinematic consistency, rigid body constraints) + soft_constraint_residuals = compute_soft_constraint_residuals( + robot_state, robot_markers, link_transforms, robot, enabled_constraints + ) + residuals.extend(soft_constraint_residuals) + + return np.asarray(residuals, dtype=np.float64) + + + +def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray: + if result.jac is None: + return np.full(n_params, float('nan'), dtype=np.float64) + J = result.jac + m, n = J.shape + JTJ = J.T @ J + try: + cov = np.linalg.pinv(JTJ) + except np.linalg.LinAlgError: + cov = np.linalg.pinv(JTJ + np.eye(n) * 1e-9) + residuals = result.fun + dof = max(1, m - n) + sigma2 = float(np.sum(residuals ** 2) / dof) + cov *= sigma2 + return np.sqrt(np.diag(cov)) + + +def print_constraint_sanity_check( + robot_state: Dict[str, float], + robot_markers: Dict[int, Dict[str, Any]], + link_transforms: Dict[str, np.ndarray], + robot: Dict[str, Any], + enabled_constraints: Dict[str, ConstraintResult] +) -> None: + """ + Print sanity checks for all constraints to verify the optimization result. + """ + print("\n" + "=" * 70) + print("CONSTRAINT SANITY CHECKS (after optimization)") + print("=" * 70) + + # Check 1: Rigid body distances + if enabled_constraints['RigidBodyDistances'].enabled: + print("\n1. RIGID BODY DISTANCES") + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) < 2: + continue + + max_error = 0.0 + for i in range(len(link_markers)): + for j in range(i + 1, len(link_markers)): + m_i = link_markers[i] + m_j = link_markers[j] + + pos_i = compute_marker_world_position(m_i, link_transforms) + pos_j = compute_marker_world_position(m_j, link_transforms) + + dist_world = np.linalg.norm(pos_i - pos_j) + dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m']) + error = abs(dist_world - dist_local) + max_error = max(max_error, error) + + status = "✓" if max_error < 1.0 else "⚠" if max_error < 5.0 else "✗" + print(f" {link_name:10s}: max_error = {max_error:.3f} mm {status}") + + # Check 2: Inter-link X distances + if enabled_constraints['InterLinkXDistances'].enabled: + print("\n2. INTER-LINK X-DISTANCES") + arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1'] + ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow'] + + if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: + m_arm1 = arm1_markers[0] + m_ellbow = ellbow_markers[0] + + pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) + pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) + + x_diff_world = pos_ellbow[0] - pos_arm1[0] + x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0] + error = abs(x_diff_world - x_diff_ref) + + status = "✓" if error < 1.0 else "⚠" if error < 5.0 else "✗" + print(f" Arm1 <-> Ellbow: error = {error:.3f} mm {status}") + + # Check 3: Arm2 sin(a) dependency + if enabled_constraints['Arm2SinADependency'].enabled: + print("\n3. ARM2 sin(a) DEPENDENCY (sanity check)") + arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] + if len(arm2_markers) >= 2: + # Check that markers with different Z values have different X spreads + a_rad = math.radians(robot_state['a']) + sin_a = math.sin(a_rad) + cos_a = math.cos(a_rad) + + z_variations = {} + for m in arm2_markers: + z_local = m['position_m'][2] + x_local = m['position_m'][0] + pos_world = compute_marker_world_position(m, link_transforms) + x_world = pos_world[0] + + # Expected: x_world = 90 + x_local * cos(a) - z_local * sin(a) + x_expected = 90 * (robot.get('renderingInfo', {}).get('metric', 'mm') == 'mm' and 0.09 or 0.09) + x_local * cos_a - z_local * sin_a + x_error = abs(x_world - x_expected) + + if z_local not in z_variations: + z_variations[z_local] = [] + z_variations[z_local].append(x_error) + + max_error = max(max(errors) for errors in z_variations.values()) if z_variations else 0.0 + status = "✓" if max_error < 5.0 else "⚠" if max_error < 10.0 else "⚠" + print(f" X-consistency with sin(a): max_error = {max_error:.3f} mm {status}") + print(f" (Note: this is a consistency check, not a hard constraint)") + + print("=" * 70) + + +def camera_position_world(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: + R, _ = cv2.Rodrigues(rvec) + return (-R.T @ tvec).reshape(3) + + +def build_output( + robot_state: Dict[str, float], + state_uncertainty: np.ndarray, + views: List[Dict[str, Any]], + camera_params: List[Tuple[np.ndarray, np.ndarray]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + scale: float, + robot: Dict[str, Any], + robot_json_path: str +) -> Dict[str, Any]: + link_transforms = compute_link_transforms(robot, robot_state, scale) + + marker_summary: Dict[int, Dict[str, Any]] = {} + for marker_id, marker in robot_markers.items(): + marker_summary[marker_id] = { + 'marker_id': marker_id, + 'link_name': marker['link_name'], + 'position_world_m': compute_marker_world_position(marker, link_transforms).tolist(), + 'size_m': marker['size_m'], + 'observation_count': 0, + 'mean_confidence': None, + 'mean_reprojection_error_px': None, + 'observations': [] + } + + per_marker_errors: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + per_marker_confidences: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + + link_transforms = compute_link_transforms(robot, robot_state, scale) + for obs in observations: + marker_id = obs['marker_id'] + marker = robot_markers[marker_id] + object_points_m = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(object_points_m, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + errors = np.linalg.norm(diffs, axis=1) + repro_error = float(np.mean(errors)) + per_marker_errors[marker_id].extend(errors.tolist()) + per_marker_confidences[marker_id].append(obs['confidence']) + marker_summary[marker_id]['observation_count'] += 1 + marker_summary[marker_id]['observations'].append({ + 'view_index': obs['view_index'], + 'source_file': views[obs['view_index']]['source_file'], + 'image_file': views[obs['view_index']]['image_file'], + 'confidence': obs['confidence'], + 'mean_reprojection_error_px': repro_error, + 'corner_reprojection_errors_px': errors.tolist() + }) + + for marker_id, summary in marker_summary.items(): + if summary['observation_count'] > 0: + summary['mean_confidence'] = float(np.mean(per_marker_confidences[marker_id])) + summary['mean_reprojection_error_px'] = float(np.mean(per_marker_errors[marker_id])) + + camera_outputs = [] + for idx, view in enumerate(views): + rvec, tvec = camera_params[idx] + cam_pos = camera_position_world(rvec, tvec) + observed_count = sum(1 for obs in observations if obs['view_index'] == idx) + camera_outputs.append({ + 'view_index': idx, + 'source_file': view['source_file'], + 'camera_id': view['camera_id'], + 'camera_position_world_m': cam_pos.tolist(), + 'rvec': rvec.reshape(-1).tolist(), + 'tvec': tvec.reshape(-1).tolist(), + 'intrinsics': { + 'camera_matrix': view['K'].tolist(), + 'distortion_coefficients': view['D'].reshape(-1).tolist() + }, + 'observation_count': observed_count + }) + + robot_pose_output = { + 'state': {k: float(robot_state[k]) for k in STATE_KEYS}, + 'uncertainty': { + 'x_mm': float(state_uncertainty[0]), + 'y_mm': float(state_uncertainty[1]), + 'z_mm': float(state_uncertainty[2]), + 'a_deg': float(state_uncertainty[3]), + 'b_deg': float(state_uncertainty[4]), + 'c_deg': float(state_uncertainty[5]), + 'e_mm': float(state_uncertainty[6]) + }, + 'confidence': { + 'x': float(math.exp(-state_uncertainty[0] / 10.0)), + 'y': float(math.exp(-state_uncertainty[1] / 10.0)), + 'z': float(math.exp(-state_uncertainty[2] / 10.0)), + 'a': float(math.exp(-state_uncertainty[3] / 10.0)), + 'b': float(math.exp(-state_uncertainty[4] / 10.0)), + 'c': float(math.exp(-state_uncertainty[5] / 10.0)), + 'e': float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6]))) + } + } + + return { + 'schema_version': '1.0', + 'created_utc': datetime.datetime.utcnow().isoformat() + 'Z', + 'source_robot_json': os.path.abspath(robot_json_path), + 'source_detections': [view['source_file'] for view in views], + 'robot_pose': robot_pose_output, + 'camera_poses': camera_outputs, + 'marker_positions': list(marker_summary.values()) + } + + +# ------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------ + +def main() -> None: + parser = argparse.ArgumentParser(description='Multiview optimization of robot pose and camera extrinsics') + parser.add_argument('--robot', required=True, help='Path to robot.json') + parser.add_argument('--detections', required=True, nargs='+', help='List of detection JSON files') + parser.add_argument('--outDir', required=True, help='Output directory') + parser.add_argument('--write-summary', action='store_true', help='Write summary file') + parser.add_argument('--max-iter', type=int, default=500, help='Maximum optimizer iterations') + args = parser.parse_args() + + os.makedirs(args.outDir, exist_ok=True) + + robot_json_path = os.path.abspath(args.robot) + robot = load_json(robot_json_path) + scale = parse_metric_scale(robot) + + default_state = { + k: float(robot.get('defaultPosition', {}).get(k, 0.0) or 0.0) + for k in STATE_KEYS + } + + robot_markers = extract_markers(robot, scale) + + # Validate constraints + print("\n" + "=" * 70) + print("CONSTRAINT VALIDATION") + print("=" * 70) + enabled_constraints = validate_constraints(robot, robot_markers) + for constraint_name, result in enabled_constraints.items(): + print(result) + print("=" * 70) + + views, observations = collect_views_and_observations(args.detections, robot_markers) + + # Print observation quality summary + print("\n" + "=" * 70) + print("OBSERVATION QUALITY SUMMARY") + print("=" * 70) + print(f"Total observations: {len(observations)}") + print() + + # Aggregate quality metrics + quality_by_marker = {} + for obs in observations: + mid = obs['marker_id'] + if mid not in quality_by_marker: + quality_by_marker[mid] = [] + quality_by_marker[mid].append(obs['quality_metrics']) + + print(f"{'Marker':>8} {'Link':>12} {'Count':>6} {'Avg Orient.':>12} {'Avg Size':>12} {'Avg Conf.':>12}") + print("-" * 70) + for marker_id in sorted(quality_by_marker.keys()): + marker = robot_markers[marker_id] + quality_list = quality_by_marker[marker_id] + avg_orient = np.mean([q['orientation_quality'] for q in quality_list]) + avg_size = np.mean([q['size_quality'] for q in quality_list]) + + obs_for_marker = [o for o in observations if o['marker_id'] == marker_id] + avg_conf = np.mean([o['confidence'] for o in obs_for_marker]) + + print(f"{marker_id:8d} {marker['link_name']:>12} {len(quality_list):6d} " + f"{avg_orient:12.3f} {avg_size:12.3f} {avg_conf:12.3f}") + print("=" * 70) + + camera_guesses = [] + for view in views: + rvec, tvec = initial_camera_guess(view, observations, robot_markers, default_state, scale, robot) + camera_guesses.append((rvec, tvec)) + + + x0 = pack_parameters(default_state, camera_guesses) + + progress = { + 'iter': 0, + 'last_cost': None, + 'last_print': time.time(), + 'prev_x': x0.copy() + } + + def progress_callback(xk: np.ndarray) -> None: + progress['iter'] += 1 + now = time.time() + if progress['iter'] == 1 or now - progress['last_print'] >= 1.0: + res = residuals_for_parameters(xk, views, observations, robot_markers, robot, scale, default_state, enabled_constraints) + cost = 0.5 * float(np.dot(res, res)) + delta_cost = None + convergence = '' + if progress['last_cost'] is not None: + delta_cost = cost - progress['last_cost'] + if abs(delta_cost) < 1e-3: + convergence = ' stable' + elif delta_cost < 0: + convergence = ' improving' + else: + convergence = ' worsening' + step_norm = float(np.linalg.norm(xk - progress['prev_x'])) + print( + f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}' + + (f' delta={delta_cost:.4g}' if delta_cost is not None else '') + + f' step={step_norm:.4g}' + + convergence + ) + progress['last_cost'] = cost + progress['last_print'] = now + progress['prev_x'] = xk.copy() + + result = least_squares( + residuals_for_parameters, + x0, + args=(views, observations, robot_markers, robot, scale, default_state, enabled_constraints), + jac='2-point', + method='trf', + loss='soft_l1', + f_scale=1.0, + max_nfev=args.max_iter, + callback=progress_callback + ) + + robot_state, camera_params = unpack_parameters(result.x, len(views)) + uncertainties = estimate_uncertainty(result, len(result.x)) + + # Print constraint sanity checks + link_transforms = compute_link_transforms(robot, robot_state, scale) + print_constraint_sanity_check(robot_state, robot_markers, link_transforms, robot, enabled_constraints) + + output = build_output(robot_state, uncertainties[:len(STATE_KEYS)], views, camera_params, observations, robot_markers, scale, robot, robot_json_path) + + out_path = Path(args.outDir) / 'multiview_pose.json' + save_json(output, out_path) + + print(f'Saved: {out_path}') + if args.write_summary: + summary_path = Path(args.outDir) / 'multiview_pose_summary.json' + summary = { + 'final_cost': float(result.cost), + 'status': int(result.status), + 'message': result.message, + 'robot_state': output['robot_pose'], + 'camera_count': len(views), + 'marker_count': len(robot_markers) + } + save_json(summary, summary_path) + print(f'Saved: {summary_path}') + + +if __name__ == '__main__': + main() diff --git a/pipeline/2_Multiview_confidence_fehlt.py b/pipeline/2_Multiview_confidence_fehlt.py new file mode 100644 index 0000000..948fb88 --- /dev/null +++ b/pipeline/2_Multiview_confidence_fehlt.py @@ -0,0 +1,787 @@ +#!/usr/bin/env python3 + +""" +============================================================ +STEP 2b — Simultane Multiview-Optimierung für Roboterpose +============================================================ + +Ziel: + Aus mehreren ArUco-Detektionsdateien die gemeinsame + Roboterpose (x,y,z,a,b,c,e) schätzen und jede Kamera-Pose + sowie Marker-Weltpositionen ausgeben. + +Eingabe: + --robot ../robot.json + --detections render_1a_aruco_detection.json render_1b_aruco_detection.json ... + --outDir . + +Ausgabe: + multiview_pose.json + +Hinweis: + Dieses Skript verwendet die Markerpositionen aus robot.json + als kinematische Constraints und optimiert gleichzeitig: + - Roboterzustand (x,y,z,a,b,c,e) + - Kameraextrinsische Parameter pro Bild + +""" + +import argparse +import datetime +import json +import math +import os +import time +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import cv2 +import numpy as np +from scipy.optimize import least_squares + +STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"] + +# ------------------------------------------------------------------ +# Constraint definitions and validation +# ------------------------------------------------------------------ + +class ConstraintResult: + """Result of validating/applying a single constraint""" + def __init__(self, name: str, enabled: bool, reason: str = ""): + self.name = name + self.enabled = enabled + self.reason = reason + self.residuals = [] + + def __str__(self) -> str: + status = "✓ ENABLED" if self.enabled else "✗ DISABLED" + return f"{self.name:40s} {status:12s} {self.reason}" + + +def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]: + """ + Validate which constraints can be applied based on robot geometry. + Returns a dict of constraint_name -> ConstraintResult + """ + results = {} + + # --- Constraint 1: Rigid body distances within each link --- + rigid_body_result = ConstraintResult("RigidBodyDistances", False) + try: + rigid_body_count = 0 + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) >= 2: + rigid_body_count += 1 + if rigid_body_count >= 2: + rigid_body_result.enabled = True + rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each" + else: + rigid_body_result.reason = "Not enough rigid links with multiple markers" + except Exception as e: + rigid_body_result.reason = f"Error: {str(e)}" + results['RigidBodyDistances'] = rigid_body_result + + # --- Constraint 2: Fixed X-distances between links (rotation around X-axis) --- + inter_link_x_result = ConstraintResult("InterLinkXDistances", False) + try: + links_with_markers = set(m['link_name'] for m in robot_markers.values()) + x_rotated_links = [] + for link_name in ['Arm1', 'Ellbow']: + if link_name in links_with_markers: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) >= 1: + x_rotated_links.append(link_name) + if len(x_rotated_links) >= 2: + inter_link_x_result.enabled = True + inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}" + else: + inter_link_x_result.reason = "Not enough X-rotation links" + except Exception as e: + inter_link_x_result.reason = f"Error: {str(e)}" + results['InterLinkXDistances'] = inter_link_x_result + + # --- Sanity check (not a hard constraint): Arm2 sin(a) dependency --- + arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)") + try: + arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] + if len(arm2_markers) >= 2: + z_values = set(m['position_m'][2] for m in arm2_markers) + if len(z_values) > 1: + arm2_sina_result.enabled = True + arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "Not enough Arm2 markers" + except Exception as e: + arm2_sina_result.reason = f"Error: {str(e)}" + results['Arm2SinADependency'] = arm2_sina_result + + return results + + +# ------------------------------------------------------------------ +# JSON helpers +# ------------------------------------------------------------------ + +def load_json(path: str) -> Dict[str, Any]: + with open(path, 'r', encoding='utf-8') as f: + return json.load(f) + + +def save_json(data: Dict[str, Any], path: Path) -> None: + with open(path, 'w', encoding='utf-8') as f: + json.dump(data, f, indent=2) + + +# ------------------------------------------------------------------ +# robot.json helpers +# ------------------------------------------------------------------ + +def resolve_scalar(value: Any, default: float = 0.0) -> float: + if value is None: + return default + if isinstance(value, (int, float)): + return float(value) + try: + return float(str(value).strip()) + except ValueError: + return default + + +def resolve_vector(value: Any, default_len: int = 3) -> Tuple[float, ...]: + if value is None: + return tuple(0.0 for _ in range(default_len)) + if isinstance(value, (int, float, str)): + return (resolve_scalar(value),) + tuple(0.0 for _ in range(default_len - 1)) + if isinstance(value, (list, tuple)): + resolved = [resolve_scalar(v) for v in value] + if len(resolved) < default_len: + resolved.extend([0.0] * (default_len - len(resolved))) + return tuple(resolved[:default_len]) + return tuple(0.0 for _ in range(default_len)) + + +def parse_metric_scale(robot: Dict[str, Any]) -> float: + rendering_info = robot.get('renderingInfo', {}) or {} + metric = rendering_info.get('metric', 'mm') + return 0.001 if str(metric).strip().lower() == 'mm' else 1.0 + + +def normalize_axis(axis: Any) -> np.ndarray: + vec = np.asarray(axis, dtype=np.float64) + if vec.shape != (3,): + vec = vec.reshape(-1)[:3] + norm = np.linalg.norm(vec) + return vec / max(norm, 1e-9) + + +def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray: + x_deg, y_deg, z_deg = resolve_vector(euler_deg, 3) + x = math.radians(x_deg) + y = math.radians(y_deg) + z = math.radians(z_deg) + + cx = math.cos(x) + sx = math.sin(x) + cy = math.cos(y) + sy = math.sin(y) + cz = math.cos(z) + sz = math.sin(z) + + Rx = np.array([ + [1.0, 0.0, 0.0], + [0.0, cx, -sx], + [0.0, sx, cx] + ], dtype=np.float64) + + Ry = np.array([ + [cy, 0.0, sy], + [0.0, 1.0, 0.0], + [-sy, 0.0, cy] + ], dtype=np.float64) + + Rz = np.array([ + [cz, -sz, 0.0], + [sz, cz, 0.0], + [0.0, 0.0, 1.0] + ], dtype=np.float64) + + return Rz @ Ry @ Rx + + +def transform_from_translation_rotation(translation: Any, rotation_deg: Any) -> np.ndarray: + T = np.eye(4, dtype=np.float64) + pos = np.asarray(resolve_vector(translation, 3), dtype=np.float64) + T[:3, 3] = pos + T[:3, :3] = euler_deg_to_matrix(rotation_deg) + return T + + +def axis_angle_matrix(axis: Any, angle_deg: float) -> np.ndarray: + axis_vec = normalize_axis(axis) + theta = math.radians(angle_deg) + kx, ky, kz = axis_vec + c = math.cos(theta) + s = math.sin(theta) + v = 1.0 - c + R = np.array([ + [kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s], + [ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s], + [kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c] + ], dtype=np.float64) + T = np.eye(4, dtype=np.float64) + T[:3, :3] = R + return T + + +# ------------------------------------------------------------------ +# Kinematics and marker extraction +# ------------------------------------------------------------------ + +def extract_markers(robot: Dict[str, Any], scale: float) -> Dict[int, Dict[str, Any]]: + markers = {} + links = robot.get('links', {}) or {} + marker_defaults = (robot.get('renderingInfo', {}) or {}).get('markerDefaults', {}) or {} + default_size_mm = float(marker_defaults.get('size', 25.0)) + + for link_name, link_info in links.items(): + for marker in link_info.get('markers', []) or []: + marker_id = int(marker.get('id', -1)) + if marker_id < 0: + continue + + pos = resolve_vector(marker.get('position', [0, 0, 0]), 3) + size_mm = float(marker.get('size', default_size_mm)) + markers[marker_id] = { + 'marker_id': marker_id, + 'link_name': link_name, + 'position_m': np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64), + 'normal': normalize_axis(resolve_vector(marker.get('normal', [0, 0, 1]), 3)), + 'spin_deg': float(marker.get('spin', 0.0)), + 'size_m': size_mm * scale, + } + + return markers + + +def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, np.ndarray]: + n = normalize_axis(normal) + candidate = np.array((0.0, 0.0, 1.0), dtype=np.float64) + if abs(np.dot(n, candidate)) > 0.99: + candidate = np.array((1.0, 0.0, 0.0), dtype=np.float64) + + x_dir = np.cross(candidate, n) + x_dir /= max(np.linalg.norm(x_dir), 1e-9) + y_dir = np.cross(n, x_dir) + + if abs(spin_deg) > 1e-6: + theta = math.radians(spin_deg) + cos_t = math.cos(theta) + sin_t = math.sin(theta) + x_rot = x_dir * cos_t + np.cross(n, x_dir) * sin_t + n * np.dot(n, x_dir) * (1.0 - cos_t) + y_rot = y_dir * cos_t + np.cross(n, y_dir) * sin_t + n * np.dot(n, y_dir) * (1.0 - cos_t) + return x_rot, y_rot + + return x_dir, y_dir + + +def marker_object_corners(marker: Dict[str, Any]) -> np.ndarray: + half = marker['size_m'] * 0.5 + x_dir, y_dir = marker_plane_axes(marker['normal'], marker['spin_deg']) + corners = np.stack([ + -x_dir * half + y_dir * half, + x_dir * half + y_dir * half, + x_dir * half - y_dir * half, + -x_dir * half - y_dir * half + ], axis=0) + return marker['position_m'].reshape(1, 3) + corners + + +def build_link_chain(robot: Dict[str, Any]) -> List[str]: + links = robot.get('links', {}) or {} + ordered: List[str] = [] + remaining = set(links.keys()) + + while remaining: + progress = False + for name in list(remaining): + parent = links[name].get('parent') + if not parent or parent in ordered: + ordered.append(name) + remaining.remove(name) + progress = True + if not progress: + raise RuntimeError('Cycle detected in robot link tree or missing parent link') + return ordered + + +def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]: + links = robot.get('links', {}) or {} + ordered_links = build_link_chain(robot) + transforms: Dict[str, np.ndarray] = {} + + for link_name in ordered_links: + link_info = links[link_name] or {} + parent_name = link_info.get('parent') + parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64) + + mount_translation = np.asarray(resolve_vector(link_info.get('mountPosition', [0, 0, 0]), 3), dtype=np.float64) * scale + mount = transform_from_translation_rotation( + mount_translation, + link_info.get('mountRotation', [0, 0, 0]) + ) + + joint_info = link_info.get('jointToParent', {}) or {} + joint_origin = np.asarray(resolve_vector(joint_info.get('origin', [0, 0, 0]), 3), dtype=np.float64) * scale + joint = transform_from_translation_rotation( + joint_origin, + joint_info.get('rotation', [0, 0, 0]) + ) + + motion = np.eye(4, dtype=np.float64) + joint_type = str(joint_info.get('type', 'fixed')).strip().lower() + control_var = str(joint_info.get('variable', joint_info.get('control', ''))).strip().lower() + axis = resolve_vector(joint_info.get('axis', [1, 0, 0]), 3) + + if joint_type == 'linear': + motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale + elif joint_type == 'revolute': + motion = axis_angle_matrix(axis, state.get(control_var, 0.0)) + + transforms[link_name] = parent_transform @ mount @ joint @ motion + + return transforms + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = np.ones(4, dtype=np.float64) + local[:3] = marker['position_m'] + world = link_transform @ local + return world[:3] + + +# ------------------------------------------------------------------ +# Camera / observation helpers +# ------------------------------------------------------------------ + +def load_intrinsics(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: + cam = detection_json['camera'] + K = np.asarray(cam['camera_matrix'], dtype=np.float64) + D = np.asarray(cam.get('distortion_coefficients', [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1) + return K, D + + +def collect_views_and_observations( + detection_files: List[str], + robot_markers: Dict[int, Dict[str, Any]] +) -> Tuple[List[Dict[str, Any]], List[Dict[str, Any]]]: + views: List[Dict[str, Any]] = [] + observations: List[Dict[str, Any]] = [] + + for idx, det_path in enumerate(detection_files): + detection_json = load_json(det_path) + K, D = load_intrinsics(detection_json) + views.append({ + 'index': idx, + 'source_file': os.path.abspath(det_path), + 'camera_id': detection_json.get('camera', {}).get('camera_id', f'cam{idx+1}'), + 'image_file': detection_json.get('image', {}).get('image_file'), + 'K': K, + 'D': D + }) + + for det in detection_json.get('detections', []) or []: + marker_id = int(det.get('marker_id', -1)) + if marker_id < 0 or marker_id not in robot_markers: + continue + + image_points = det.get('image_points_px') + if isinstance(image_points, list) and len(image_points) == 4: + image_points = np.asarray(image_points, dtype=np.float64) + else: + center = resolve_vector(det.get('center_px', [0, 0]), 2) + image_points = np.asarray([center], dtype=np.float64) + + confidence = float(det.get('confidence', 1.0)) + marker = robot_markers[marker_id] + observations.append({ + 'view_index': idx, + 'marker_id': marker_id, + 'marker_link_corners': marker_object_corners(marker), + 'image_points_px': image_points, + 'confidence': max(0.01, min(1.0, confidence)) + }) + + if len(views) == 0: + raise RuntimeError('No valid detection views found') + + if len(observations) == 0: + raise RuntimeError('No marker observations matched robot.json markers') + + return views, observations + + +def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = marker_object_corners(marker) + homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1) + world = (link_transform @ homogeneous.T).T + return world[:, :3] + + +def initial_camera_guess( + view: Dict[str, Any], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + default_state: Dict[str, float], + scale: float, + robot: Dict[str, Any] +) -> Tuple[np.ndarray, np.ndarray]: + object_points = [] + image_points = [] + + link_transforms = compute_link_transforms(robot, default_state, scale) + + for obs in observations: + if obs['view_index'] != view['index']: + continue + marker = robot_markers[obs['marker_id']] + object_points.append(compute_marker_world_corners(marker, link_transforms)) + image_points.append(obs['image_points_px']) + + if len(object_points) == 0: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + object_points = np.vstack(object_points) + image_points = np.vstack(image_points) + + if object_points.shape[0] < 4: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + success, rvec, tvec = cv2.solvePnP( + object_points, + image_points, + view['K'], + view['D'], + flags=cv2.SOLVEPNP_ITERATIVE + ) + + if not success: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + return rvec, tvec + + +def project_points( + points_3d: np.ndarray, + rvec: np.ndarray, + tvec: np.ndarray, + K: np.ndarray, + D: np.ndarray +) -> np.ndarray: + projected, _ = cv2.projectPoints(points_3d, rvec, tvec, K, D) + return projected.reshape(-1, 2) + + +# ------------------------------------------------------------------ +# Optimization +# ------------------------------------------------------------------ + +def pack_parameters(robot_state: Dict[str, float], camera_params: List[Tuple[np.ndarray, np.ndarray]]) -> np.ndarray: + state_vec = np.asarray([robot_state[k] for k in STATE_KEYS], dtype=np.float64) + cams = [] + for rvec, tvec in camera_params: + cams.append(rvec.reshape(3)) + cams.append(tvec.reshape(3)) + return np.concatenate([state_vec] + cams) + + +def unpack_parameters(params: np.ndarray, n_views: int) -> Tuple[Dict[str, float], List[Tuple[np.ndarray, np.ndarray]]]: + robot_state = {STATE_KEYS[i]: float(params[i]) for i in range(len(STATE_KEYS))} + camera_params = [] + offset = len(STATE_KEYS) + for _ in range(n_views): + rvec = params[offset:offset + 3].reshape(3, 1) + tvec = params[offset + 3:offset + 6].reshape(3, 1) + camera_params.append((rvec, tvec)) + offset += 6 + return robot_state, camera_params + + +def residuals_for_parameters( + params: np.ndarray, + views: List[Dict[str, Any]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + robot: Dict[str, Any], + scale: float, + default_state: Dict[str, float] +) -> np.ndarray: + robot_state, camera_params = unpack_parameters(params, len(views)) + link_transforms = compute_link_transforms(robot, robot_state, scale) + + residuals = [] + for obs in observations: + marker = robot_markers[obs['marker_id']] + world_corners = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(world_corners, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + weight = math.sqrt(obs['confidence']) + residuals.extend((diffs * weight).reshape(-1)) + + for key in STATE_KEYS: + diff = robot_state[key] - default_state.get(key, 0.0) + if key in ('x', 'y', 'z', 'e'): + w = 0.001 + else: + w = 0.01 + residuals.append(diff * w) + + return np.asarray(residuals, dtype=np.float64) + + +def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray: + if result.jac is None: + return np.full(n_params, float('nan'), dtype=np.float64) + J = result.jac + m, n = J.shape + JTJ = J.T @ J + try: + cov = np.linalg.pinv(JTJ) + except np.linalg.LinAlgError: + cov = np.linalg.pinv(JTJ + np.eye(n) * 1e-9) + residuals = result.fun + dof = max(1, m - n) + sigma2 = float(np.sum(residuals ** 2) / dof) + cov *= sigma2 + return np.sqrt(np.diag(cov)) + + +# ------------------------------------------------------------------ +# Output assembly +# ------------------------------------------------------------------ + +def camera_position_world(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: + R, _ = cv2.Rodrigues(rvec) + return (-R.T @ tvec).reshape(3) + + +def build_output( + robot_state: Dict[str, float], + state_uncertainty: np.ndarray, + views: List[Dict[str, Any]], + camera_params: List[Tuple[np.ndarray, np.ndarray]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + scale: float, + robot: Dict[str, Any], + robot_json_path: str +) -> Dict[str, Any]: + link_transforms = compute_link_transforms(robot, robot_state, scale) + + marker_summary: Dict[int, Dict[str, Any]] = {} + for marker_id, marker in robot_markers.items(): + marker_summary[marker_id] = { + 'marker_id': marker_id, + 'link_name': marker['link_name'], + 'position_world_m': compute_marker_world_position(marker, link_transforms).tolist(), + 'size_m': marker['size_m'], + 'observation_count': 0, + 'mean_confidence': None, + 'mean_reprojection_error_px': None, + 'observations': [] + } + + per_marker_errors: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + per_marker_confidences: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + + link_transforms = compute_link_transforms(robot, robot_state, scale) + for obs in observations: + marker_id = obs['marker_id'] + marker = robot_markers[marker_id] + object_points_m = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(object_points_m, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + errors = np.linalg.norm(diffs, axis=1) + repro_error = float(np.mean(errors)) + per_marker_errors[marker_id].extend(errors.tolist()) + per_marker_confidences[marker_id].append(obs['confidence']) + marker_summary[marker_id]['observation_count'] += 1 + marker_summary[marker_id]['observations'].append({ + 'view_index': obs['view_index'], + 'source_file': views[obs['view_index']]['source_file'], + 'image_file': views[obs['view_index']]['image_file'], + 'confidence': obs['confidence'], + 'mean_reprojection_error_px': repro_error, + 'corner_reprojection_errors_px': errors.tolist() + }) + + for marker_id, summary in marker_summary.items(): + if summary['observation_count'] > 0: + summary['mean_confidence'] = float(np.mean(per_marker_confidences[marker_id])) + summary['mean_reprojection_error_px'] = float(np.mean(per_marker_errors[marker_id])) + + camera_outputs = [] + for idx, view in enumerate(views): + rvec, tvec = camera_params[idx] + cam_pos = camera_position_world(rvec, tvec) + observed_count = sum(1 for obs in observations if obs['view_index'] == idx) + camera_outputs.append({ + 'view_index': idx, + 'source_file': view['source_file'], + 'camera_id': view['camera_id'], + 'camera_position_world_m': cam_pos.tolist(), + 'rvec': rvec.reshape(-1).tolist(), + 'tvec': tvec.reshape(-1).tolist(), + 'intrinsics': { + 'camera_matrix': view['K'].tolist(), + 'distortion_coefficients': view['D'].reshape(-1).tolist() + }, + 'observation_count': observed_count + }) + + robot_pose_output = { + 'state': {k: float(robot_state[k]) for k in STATE_KEYS}, + 'uncertainty': { + 'x_mm': float(state_uncertainty[0]), + 'y_mm': float(state_uncertainty[1]), + 'z_mm': float(state_uncertainty[2]), + 'a_deg': float(state_uncertainty[3]), + 'b_deg': float(state_uncertainty[4]), + 'c_deg': float(state_uncertainty[5]), + 'e_mm': float(state_uncertainty[6]) + }, + 'confidence': { + 'x': float(math.exp(-state_uncertainty[0] / 10.0)), + 'y': float(math.exp(-state_uncertainty[1] / 10.0)), + 'z': float(math.exp(-state_uncertainty[2] / 10.0)), + 'a': float(math.exp(-state_uncertainty[3] / 10.0)), + 'b': float(math.exp(-state_uncertainty[4] / 10.0)), + 'c': float(math.exp(-state_uncertainty[5] / 10.0)), + 'e': float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6]))) + } + } + + return { + 'schema_version': '1.0', + 'created_utc': datetime.datetime.utcnow().isoformat() + 'Z', + 'source_robot_json': os.path.abspath(robot_json_path), + 'source_detections': [view['source_file'] for view in views], + 'robot_pose': robot_pose_output, + 'camera_poses': camera_outputs, + 'marker_positions': list(marker_summary.values()) + } + + +# ------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------ + +def main() -> None: + parser = argparse.ArgumentParser(description='Multiview optimization of robot pose and camera extrinsics') + parser.add_argument('--robot', required=True, help='Path to robot.json') + parser.add_argument('--detections', required=True, nargs='+', help='List of detection JSON files') + parser.add_argument('--outDir', required=True, help='Output directory') + parser.add_argument('--write-summary', action='store_true', help='Write summary file') + parser.add_argument('--max-iter', type=int, default=500, help='Maximum optimizer iterations') + args = parser.parse_args() + + os.makedirs(args.outDir, exist_ok=True) + + robot_json_path = os.path.abspath(args.robot) + robot = load_json(robot_json_path) + scale = parse_metric_scale(robot) + + default_state = { + k: float(robot.get('defaultPosition', {}).get(k, 0.0) or 0.0) + for k in STATE_KEYS + } + + robot_markers = extract_markers(robot, scale) + views, observations = collect_views_and_observations(args.detections, robot_markers) + + camera_guesses = [] + for view in views: + rvec, tvec = initial_camera_guess(view, observations, robot_markers, default_state, scale, robot) + camera_guesses.append((rvec, tvec)) + + x0 = pack_parameters(default_state, camera_guesses) + + progress = { + 'iter': 0, + 'last_cost': None, + 'last_print': time.time(), + 'prev_x': x0.copy() + } + + def progress_callback(xk: np.ndarray) -> None: + progress['iter'] += 1 + now = time.time() + if progress['iter'] == 1 or now - progress['last_print'] >= 1.0: + res = residuals_for_parameters(xk, views, observations, robot_markers, robot, scale, default_state) + cost = 0.5 * float(np.dot(res, res)) + delta_cost = None + convergence = '' + if progress['last_cost'] is not None: + delta_cost = cost - progress['last_cost'] + if abs(delta_cost) < 1e-3: + convergence = ' stable' + elif delta_cost < 0: + convergence = ' improving' + else: + convergence = ' worsening' + step_norm = float(np.linalg.norm(xk - progress['prev_x'])) + print( + f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}' + + (f' delta={delta_cost:.4g}' if delta_cost is not None else '') + + f' step={step_norm:.4g}' + + convergence + ) + progress['last_cost'] = cost + progress['last_print'] = now + progress['prev_x'] = xk.copy() + + result = least_squares( + residuals_for_parameters, + x0, + args=(views, observations, robot_markers, robot, scale, default_state), + jac='2-point', + method='trf', + loss='soft_l1', + f_scale=1.0, + max_nfev=args.max_iter, + callback=progress_callback + ) + + robot_state, camera_params = unpack_parameters(result.x, len(views)) + uncertainties = estimate_uncertainty(result, len(result.x)) + + output = build_output(robot_state, uncertainties[:len(STATE_KEYS)], views, camera_params, observations, robot_markers, scale, robot, robot_json_path) + + out_path = Path(args.outDir) / 'multiview_pose.json' + save_json(output, out_path) + + print(f'Saved: {out_path}') + if args.write_summary: + summary_path = Path(args.outDir) / 'multiview_pose_summary.json' + summary = { + 'final_cost': float(result.cost), + 'status': int(result.status), + 'message': result.message, + 'robot_state': output['robot_pose'], + 'camera_count': len(views), + 'marker_count': len(robot_markers) + } + save_json(summary, summary_path) + print(f'Saved: {summary_path}') + + +if __name__ == '__main__': + main() diff --git a/pipeline/__pycache__/2_Multiview.cpython-311.pyc b/pipeline/__pycache__/2_Multiview.cpython-311.pyc index 1c70390..141feee 100644 Binary files a/pipeline/__pycache__/2_Multiview.cpython-311.pyc and b/pipeline/__pycache__/2_Multiview.cpython-311.pyc differ diff --git a/pipeline/camera_pose_summary.json b/pipeline/camera_pose_summary.json deleted file mode 100644 index e6d5dad..0000000 --- a/pipeline/camera_pose_summary.json +++ /dev/null @@ -1,47 +0,0 @@ -{ - "schema_version": "1.0", - "algorithm": "board_pnp_camera_pose", - "robot_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json", - "intrinsics_source": "embedded_in_detection_json", - "results": [ - { - "detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", - "output_file": "render_1a_aruco_detection_camera_pose.json", - "rmse_px": 53.69444650385422, - "median_px": 32.08915387366267, - "num_correspondences": 32, - "used_marker_ids": [ - 205, - 206, - 207, - 208, - 210, - 211, - 215, - 217 - ] - }, - { - "detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", - "output_file": "render_1b_aruco_detection_camera_pose.json", - "rmse_px": 0.9028284747812563, - "median_px": 0.87428115526563, - "num_correspondences": 8, - "used_marker_ids": [ - 210, - 215 - ] - }, - { - "detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", - "output_file": "render_1c_aruco_detection_camera_pose.json", - "rmse_px": 1.013497154241286, - "median_px": 0.98147711476875, - "num_correspondences": 8, - "used_marker_ids": [ - 210, - 214 - ] - } - ] -} \ No newline at end of file diff --git a/pipeline/multiview_pose.json b/pipeline/multiview_pose.json index f01dfa0..7942ef2 100644 --- a/pipeline/multiview_pose.json +++ b/pipeline/multiview_pose.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T15:44:40.387206Z", + "created_utc": "2026-05-28T20:33:16.813434Z", "source_robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json", "source_detections": [ "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", @@ -9,28 +9,28 @@ ], "robot_pose": { "state": { - "x": 178.4429017494112, - "y": 58.47337818356634, - "z": -115.44834336922801, - "a": -110.23287555471994, - "b": 21.99999999999879, - "c": 91.00000000000222, - "e": 10.000000000003752 + "x": 216.09132484852927, + "y": 39.690843564316594, + "z": -55.07137540784025, + "a": -131.24664072646647, + "b": 22.00000000000524, + "c": 91.0000000000042, + "e": 9.999999999997005 }, "uncertainty": { - "x_mm": 452.93520846643963, - "y_mm": 2505.6853087014742, - "z_mm": 479.7547548672694, - "a_deg": 419.0184635434338, - "b_deg": 10357.838151169784, - "c_deg": 10357.838358456424, - "e_mm": 103578.27625405855 + "x_mm": 225.298670975141, + "y_mm": 594.8766035835473, + "z_mm": 259.0225242970393, + "a_deg": 1077.9536617200663, + "b_deg": 12207.30048251762, + "c_deg": 12207.300378181952, + "e_mm": 122073.0039182515 }, "confidence": { - "x": 2.1343902596830898e-20, - "y": 1.5117142415373693e-109, - "z": 1.460547647110293e-21, - "a": 6.342483509852416e-19, + "x": 1.6421130086210916e-10, + "y": 1.4616297196124474e-26, + "z": 5.6337127678067286e-12, + "a": 1.531324732033841e-47, "b": 0.0, "c": 0.0, "e": 0.36787944117144233 @@ -42,19 +42,19 @@ "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "camera_id": "cam1", "camera_position_world_m": [ - -152.34091569714903, - -19.013446299323782, - -99.72128351445228 + -38.34617352837666, + -74.24635979425081, + -20.188423219708135 ], "rvec": [ - -5.499131960772023, - -21.91270066095422, - 3.835322497331174 + 111.52327474141508, + -126.11068340719217, + 44.203270370543855 ], "tvec": [ - 1.5121559708775556, - 7.396415796967461, - -182.91147186295072 + 0.46738739070887664, + 3.5663317158792367, + -85.89287645773129 ], "intrinsics": { "camera_matrix": [ @@ -89,19 +89,19 @@ "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "camera_id": "cam1", "camera_position_world_m": [ - 0.08962448933807897, - 0.30899658581804224, - -0.6948843092613544 + -18.749065173061467, + 50.58295606304779, + -8.605729054607524 ], "rvec": [ - -0.523540707058038, - -3.160660993109504, - -0.8426789395152159 + 0.329953779441624, + 1.990640379537416, + 1.5517306914240998 ], "tvec": [ - 0.17310377758414208, - 0.03144977103054412, - -0.7452661514399899 + -1.9764953616377263, + 2.0849863370638175, + -54.55243128918203 ], "intrinsics": { "camera_matrix": [ @@ -136,19 +136,19 @@ "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "camera_id": "cam1", "camera_position_world_m": [ - 0.19930315742175841, - -0.4034279127468583, - 0.6690051405865579 + 0.2728534702741987, + -0.6585880592537502, + 0.4418986748635548 ], "rvec": [ - -3.515407873484386, - 0.6592488811642211, - 0.3554510183912642 + 2.1894889099532713, + -0.20366077493465337, + -0.3361765800667276 ], "tvec": [ - -0.12629764596438484, - 0.0026714931553666757, - 0.7962948418902289 + -0.13776092296131418, + 0.016095933671670594, + 0.8271786602074209 ], "intrinsics": { "camera_matrix": [ @@ -191,45 +191,145 @@ "size_m": 0.025, "observation_count": 3, "mean_confidence": 0.6997700579082148, - "mean_reprojection_error_px": 219.98520262396713, + "mean_detector_confidence": 0.6997700579082148, + "mean_reprojection_error_px": 348.3808535157419, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.5673043275049208, - "mean_reprojection_error_px": 582.6928891559149, + "confidence_detector": 0.5673043275049208, + "confidence_weighted": 0.5673043275049208, + "quality": { + "detector_confidence": 0.5673043275049208, + "weighted_confidence": 0.5673043275049208, + "q": { + "size": 0.8108224868774414, + "aspect": 0.9096486158109359, + "border": 0.2833333333333333, + "center": 0.2385730743408203, + "sharpness": 1.0, + "homography": 0.9999983069718208, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 40.54112434387207, + "edge_ratio": 1.1986511772098227, + "distance_to_border_px": 34.0, + "distance_to_center_norm": 0.7614269256591797, + "laplacian_var": 3797.5312506581813 + } + }, + "mean_reprojection_error_px": 587.0995139995744, "corner_reprojection_errors_px": [ - 608.4008310831335, - 564.1978138064919, - 556.9944685718621, - 601.1784431621722 + 612.7534949573636, + 568.3181566115126, + 561.4573585579957, + 605.8690458714257 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.7813266383036261, - "mean_reprojection_error_px": 39.955946109809304, + "confidence_detector": 0.7813266383036261, + "confidence_weighted": 0.7813266383036261, + "quality": { + "detector_confidence": 0.7813266383036261, + "weighted_confidence": 0.7813266383036261, + "q": { + "size": 0.7824347686767578, + "aspect": 0.8792155830729744, + "border": 1.0, + "center": 0.49675697088241577, + "sharpness": 0.9904164099455028, + "homography": 0.7145588675251666, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 39.12173843383789, + "edge_ratio": 1.274754950327127, + "distance_to_border_px": 252.0, + "distance_to_center_norm": 0.5032430291175842, + "laplacian_var": 2476.0410248637572 + } + }, + "mean_reprojection_error_px": 427.8772796416253, "corner_reprojection_errors_px": [ - 50.05440600297249, - 62.06318313444874, - 46.054117736318254, - 1.6520775654977295 + 446.3587605969117, + 404.1867109578964, + 408.4069136534603, + 452.55673335823275 ] }, { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.7506792079160973, - "mean_reprojection_error_px": 37.30677260617712, + "confidence_detector": 0.7506792079160973, + "confidence_weighted": 0.7506792079160973, + "quality": { + "detector_confidence": 0.7506792079160973, + "weighted_confidence": 0.7506792079160973, + "q": { + "size": 0.8434026336669922, + "aspect": 0.8575862494073492, + "border": 1.0, + "center": 0.6442468464374542, + "sharpness": 1.0, + "homography": 0.8786678750976363, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 42.17013168334961, + "edge_ratio": 1.3321269451115116, + "distance_to_border_px": 298.0, + "distance_to_center_norm": 0.3557531535625458, + "laplacian_var": 2842.0926549741707 + } + }, + "mean_reprojection_error_px": 30.16576690602588, "corner_reprojection_errors_px": [ - 63.966234566185975, - 24.372791532782248, - 11.26324714586789, - 49.62481717987238 + 36.22778096708586, + 21.548187717111936, + 42.84248647061261, + 20.044612469293106 ] } ] @@ -245,19 +345,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.6412317666518965, - "mean_reprojection_error_px": 213.01123894247837, + "mean_detector_confidence": 0.6412317666518965, + "mean_reprojection_error_px": 213.51155158837116, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.6412317666518965, - "mean_reprojection_error_px": 213.01123894247837, + "confidence_detector": 0.6412317666518965, + "confidence_weighted": 0.6412317666518965, + "quality": { + "detector_confidence": 0.6412317666518965, + "weighted_confidence": 0.6412317666518965, + "q": { + "size": 0.6744685745239258, + "aspect": 0.9460729944417767, + "border": 1.0, + "center": 0.7332987189292908, + "sharpness": 1.0, + "homography": 0.7016198391981602, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 33.72342872619629, + "edge_ratio": 1.1140017860673477, + "distance_to_border_px": 276.0, + "distance_to_center_norm": 0.26670128107070923, + "laplacian_var": 3165.7184033540047 + } + }, + "mean_reprojection_error_px": 213.51155158837116, "corner_reprojection_errors_px": [ - 236.1031649719927, - 200.91925610460427, - 190.4144522672059, - 224.6080824261106 + 236.74653013262179, + 201.34252196509405, + 190.76098744554534, + 225.1961668102236 ] } ] @@ -273,32 +407,99 @@ "size_m": 0.025, "observation_count": 2, "mean_confidence": 0.8108527003549935, - "mean_reprojection_error_px": 198.10515794746527, + "mean_detector_confidence": 0.8108527003549935, + "mean_reprojection_error_px": 159.0807109791299, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.7952557920267838, - "mean_reprojection_error_px": 204.57188538155356, + "confidence_detector": 0.7952557920267838, + "confidence_weighted": 0.7952557920267838, + "quality": { + "detector_confidence": 0.7952557920267838, + "weighted_confidence": 0.7952557920267838, + "q": { + "size": 0.7146316909790039, + "aspect": 0.9859016728248396, + "border": 1.0, + "center": 0.786280557513237, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 35.731584548950195, + "edge_ratio": 1.0285998645985972, + "distance_to_border_px": 211.0, + "distance_to_center_norm": 0.213719442486763, + "laplacian_var": 3704.0260869185627 + } + }, + "mean_reprojection_error_px": 202.87090079707735, "corner_reprojection_errors_px": [ - 217.65412604361978, - 184.2181761177656, - 192.91255827135913, - 223.50268109346976 + 216.05481694083824, + 182.3251487108743, + 191.11678207445826, + 221.98685546213872 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.826449608683203, - "mean_reprojection_error_px": 191.63843051337702, + "confidence_detector": 0.826449608683203, + "confidence_weighted": 0.826449608683203, + "quality": { + "detector_confidence": 0.826449608683203, + "weighted_confidence": 0.826449608683203, + "q": { + "size": 0.8409605407714844, + "aspect": 0.9049793706370471, + "border": 1.0, + "center": 0.9083917960524559, + "sharpness": 0.8049029197222222, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 42.04802703857422, + "edge_ratio": 1.2099951279465397, + "distance_to_border_px": 290.0, + "distance_to_center_norm": 0.0916082039475441, + "laplacian_var": 2012.2572993055553 + } + }, + "mean_reprojection_error_px": 115.2905211611824, "corner_reprojection_errors_px": [ - 226.90990661141322, - 176.45008691276055, - 156.32935430702966, - 206.86437422230463 + 102.14384765398431, + 92.14591665858303, + 129.81168442827544, + 137.0606359038868 ] } ] @@ -314,19 +515,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.6041252446922665, - "mean_reprojection_error_px": 443.42359136317395, + "mean_detector_confidence": 0.6041252446922665, + "mean_reprojection_error_px": 338.5476435670504, "observations": [ { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.6041252446922665, - "mean_reprojection_error_px": 443.42359136317395, + "confidence_detector": 0.6041252446922665, + "confidence_weighted": 0.6041252446922665, + "quality": { + "detector_confidence": 0.6041252446922665, + "weighted_confidence": 0.6041252446922665, + "q": { + "size": 0.9836519622802734, + "aspect": 0.7532145593882991, + "border": 1.0, + "center": 0.32276880741119385, + "sharpness": 0.5256355772646303, + "homography": 0.7899472530560614, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 49.18259811401367, + "edge_ratio": 1.655285901037602, + "distance_to_border_px": 137.0, + "distance_to_center_norm": 0.6772311925888062, + "laplacian_var": 1314.0889431615756 + } + }, + "mean_reprojection_error_px": 338.5476435670504, "corner_reprojection_errors_px": [ - 448.5785815165172, - 422.5680699902232, - 440.44289571629525, - 462.10481822966005 + 334.40236466141283, + 319.8057940927665, + 346.37370249262483, + 353.6087130213974 ] } ] @@ -342,19 +577,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.6280424706698967, - "mean_reprojection_error_px": 104.44146786678962, + "mean_detector_confidence": 0.6280424706698967, + "mean_reprojection_error_px": 100.412293994181, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.6280424706698967, - "mean_reprojection_error_px": 104.44146786678962, + "confidence_detector": 0.6280424706698967, + "confidence_weighted": 0.6280424706698967, + "quality": { + "detector_confidence": 0.6280424706698967, + "weighted_confidence": 0.6280424706698967, + "q": { + "size": 0.6625362014770508, + "aspect": 0.9537163525948422, + "border": 1.0, + "center": 0.9434776194393635, + "sharpness": 1.0, + "homography": 0.7625645902124035, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 33.12681007385254, + "edge_ratio": 1.0970595655180506, + "distance_to_border_px": 303.0, + "distance_to_center_norm": 0.05652238056063652, + "laplacian_var": 2848.9886204705635 + } + }, + "mean_reprojection_error_px": 100.412293994181, "corner_reprojection_errors_px": [ - 99.73569678834396, - 84.11026583409073, - 111.81629138739532, - 122.10361745732845 + 95.36403494262059, + 79.9557213035894, + 108.22499615920151, + 118.10442357131254 ] } ] @@ -370,19 +639,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.33820423087105295, - "mean_reprojection_error_px": 269.1974700827843, + "mean_detector_confidence": 0.33820423087105295, + "mean_reprojection_error_px": 276.15167212095713, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.33820423087105295, - "mean_reprojection_error_px": 269.1974700827843, + "confidence_detector": 0.33820423087105295, + "confidence_weighted": 0.33820423087105295, + "quality": { + "detector_confidence": 0.33820423087105295, + "weighted_confidence": 0.33820423087105295, + "q": { + "size": 0.5361141967773437, + "aspect": 0.8936906729306451, + "border": 0.9833333333333333, + "center": 0.5868735313415527, + "sharpness": 1.0, + "homography": 0.6985669093200941, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 26.805709838867188, + "edge_ratio": 1.2379107901411548, + "distance_to_border_px": 118.0, + "distance_to_center_norm": 0.41312646865844727, + "laplacian_var": 4112.338450277431 + } + }, + "mean_reprojection_error_px": 276.15167212095713, "corner_reprojection_errors_px": [ - 250.50345115135508, - 274.79359034013294, - 287.8396527117673, - 263.6531861278821 + 257.9111382626554, + 282.35364104879613, + 294.3934037418535, + 269.9485054305234 ] } ] @@ -398,19 +701,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.28724459014346065, - "mean_reprojection_error_px": 418.6572411272094, + "mean_detector_confidence": 0.28724459014346065, + "mean_reprojection_error_px": 426.6190397265363, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.28724459014346065, - "mean_reprojection_error_px": 418.6572411272094, + "confidence_detector": 0.28724459014346065, + "confidence_weighted": 0.28724459014346065, + "quality": { + "detector_confidence": 0.28724459014346065, + "weighted_confidence": 0.28724459014346065, + "q": { + "size": 0.5248580741882324, + "aspect": 0.839187418386558, + "border": 0.8333333333333334, + "center": 0.4003753066062927, + "sharpness": 1.0, + "homography": 0.9999815099800249, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 26.24290370941162, + "edge_ratio": 1.383257847031654, + "distance_to_border_px": 100.0, + "distance_to_center_norm": 0.5996246933937073, + "laplacian_var": 5067.414824476962 + } + }, + "mean_reprojection_error_px": 426.6190397265363, "corner_reprojection_errors_px": [ - 397.9342521865105, - 419.4901443591502, - 439.2360756416858, - 417.96849232149117 + 406.15079651506966, + 428.05267082059254, + 446.9579519399622, + 425.3147396305208 ] } ] @@ -426,19 +763,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.27228561618555186, - "mean_reprojection_error_px": 365.7887375890425, + "mean_detector_confidence": 0.27228561618555186, + "mean_reprojection_error_px": 374.50255105417807, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.27228561618555186, - "mean_reprojection_error_px": 365.7887375890425, + "confidence_detector": 0.27228561618555186, + "confidence_weighted": 0.27228561618555186, + "quality": { + "detector_confidence": 0.27228561618555186, + "weighted_confidence": 0.27228561618555186, + "q": { + "size": 0.5014523983001709, + "aspect": 0.8554115971298921, + "border": 0.5, + "center": 0.4565395712852478, + "sharpness": 1.0, + "homography": 0.6975670005943979, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 25.072619915008545, + "edge_ratio": 1.3380557461583085, + "distance_to_border_px": 60.0, + "distance_to_center_norm": 0.5434604287147522, + "laplacian_var": 4016.6321593416924 + } + }, + "mean_reprojection_error_px": 374.50255105417807, "corner_reprojection_errors_px": [ - 348.36626075065936, - 369.7628063312705, - 383.5097638627369, - 361.51611941150327 + 357.4436839806056, + 379.04037742345474, + 391.88854109463733, + 369.63760171801465 ] } ] @@ -454,19 +825,53 @@ "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.35596573288593486, - "mean_reprojection_error_px": 324.2590328208219, + "mean_detector_confidence": 0.35596573288593486, + "mean_reprojection_error_px": 330.11805773797414, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.35596573288593486, - "mean_reprojection_error_px": 324.2590328208219, + "confidence_detector": 0.35596573288593486, + "confidence_weighted": 0.35596573288593486, + "quality": { + "detector_confidence": 0.35596573288593486, + "weighted_confidence": 0.35596573288593486, + "q": { + "size": 0.5561128711700439, + "aspect": 0.8721453879265322, + "border": 1.0, + "center": 0.5334544777870178, + "sharpness": 1.0, + "homography": 0.726899391813532, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 27.805643558502197, + "edge_ratio": 1.2931956388084183, + "distance_to_border_px": 161.0, + "distance_to_center_norm": 0.4665455222129822, + "laplacian_var": 3793.153007417641 + } + }, + "mean_reprojection_error_px": 330.11805773797414, "corner_reprojection_errors_px": [ - 302.09103568606685, - 325.56745248682427, - 346.3582043745394, - 323.01943873585714 + 308.2113330466644, + 332.08999978676184, + 351.98742228618335, + 328.1834758322872 ] } ] @@ -475,39 +880,106 @@ "marker_id": 198, "link_name": "Arm1", "position_world_m": [ - 0.1784429017494112, - -0.05382924293825046, - 0.15468488162937843 + 0.21609132484852928, + -0.10076769046005479, + 0.12911573319834646 ], "size_m": 0.025, "observation_count": 2, "mean_confidence": 0.16652042240877096, - "mean_reprojection_error_px": 173.03325228464936, + "mean_detector_confidence": 0.16652042240877096, + "mean_reprojection_error_px": 243.66958450030666, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.20150745250271476, - "mean_reprojection_error_px": 247.8149724280193, + "confidence_detector": 0.20150745250271476, + "confidence_weighted": 0.20150745250271476, + "quality": { + "detector_confidence": 0.20150745250271476, + "weighted_confidence": 0.20150745250271476, + "q": { + "size": 0.6339855194091797, + "aspect": 0.7539930512070505, + "border": 1.0, + "center": 0.6189768612384796, + "sharpness": 1.0, + "homography": 0.6098583763162305, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 31.699275970458984, + "edge_ratio": 1.652544339497884, + "distance_to_border_px": 237.0, + "distance_to_center_norm": 0.3810231387615204, + "laplacian_var": 3972.096222043203 + } + }, + "mean_reprojection_error_px": 248.654861221765, "corner_reprojection_errors_px": [ - 273.59743355359444, - 245.42488018268014, - 222.46143771784855, - 249.77613825795413 + 274.50699319923274, + 246.03321812358865, + 223.2084648682347, + 250.8707686960039 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.13153339231482716, - "mean_reprojection_error_px": 98.25153214127941, + "confidence_detector": 0.13153339231482716, + "confidence_weighted": 0.13153339231482716, + "quality": { + "detector_confidence": 0.13153339231482716, + "weighted_confidence": 0.13153339231482716, + "q": { + "size": 0.6576880264282227, + "aspect": 0.42738020051250675, + "border": 0.5916666666666667, + "center": 0.6157204806804657, + "sharpness": 0.9362022578647182, + "homography": 0.8917140926962271, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 32.88440132141113, + "edge_ratio": 3.6796739708616246, + "distance_to_border_px": 71.0, + "distance_to_center_norm": 0.3842795193195343, + "laplacian_var": 2340.5056446617955 + } + }, + "mean_reprojection_error_px": 238.6843077788483, "corner_reprojection_errors_px": [ - 86.59176111374053, - 97.5474468583387, - 120.03541185467505, - 88.83150873836338 + 255.20670426390527, + 233.9309789087676, + 221.28127646869942, + 244.318271474021 ] } ] @@ -516,39 +988,106 @@ "marker_id": 229, "link_name": "Arm1", "position_world_m": [ - 0.1784429017494112, - -0.100889763924023, - 0.23140063858026605 + 0.21609132484852928, + -0.1700228368607847, + 0.18659376984725667 ], "size_m": 0.025, "observation_count": 2, "mean_confidence": 0.2274747191640703, - "mean_reprojection_error_px": 110.21276026061363, + "mean_detector_confidence": 0.2274747191640703, + "mean_reprojection_error_px": 172.23377000525875, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.26810902547334975, - "mean_reprojection_error_px": 155.73254773328267, + "confidence_detector": 0.26810902547334975, + "confidence_weighted": 0.26810902547334975, + "quality": { + "detector_confidence": 0.26810902547334975, + "weighted_confidence": 0.26810902547334975, + "q": { + "size": 0.6824631023406983, + "aspect": 0.7938768481947832, + "border": 1.0, + "center": 0.733243316411972, + "sharpness": 0.9086003975826067, + "homography": 0.7035912150387432, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 34.12315511703491, + "edge_ratio": 1.5192824359947652, + "distance_to_border_px": 235.0, + "distance_to_center_norm": 0.26675668358802795, + "laplacian_var": 2271.5009939565166 + } + }, + "mean_reprojection_error_px": 155.22297984485186, "corner_reprojection_errors_px": [ - 180.08971330368811, - 157.10232370463987, - 131.63039756460225, - 154.10775636020045 + 179.66512440166122, + 156.3996648998681, + 130.99743974604678, + 153.82969033183136 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.18684041285479083, - "mean_reprojection_error_px": 64.69297278794461, + "confidence_detector": 0.18684041285479083, + "confidence_weighted": 0.18684041285479083, + "quality": { + "detector_confidence": 0.18684041285479083, + "weighted_confidence": 0.18684041285479083, + "q": { + "size": 0.7395829486846924, + "aspect": 0.44796521997625727, + "border": 1.0, + "center": 0.6955729424953461, + "sharpness": 0.5568649165432099, + "homography": 0.5823463065693475, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 36.97914743423462, + "edge_ratio": 3.4646323214690695, + "distance_to_border_px": 130.0, + "distance_to_center_norm": 0.30442705750465393, + "laplacian_var": 1392.1622913580247 + } + }, + "mean_reprojection_error_px": 189.24456016566566, "corner_reprojection_errors_px": [ - 54.16045919019677, - 65.65659031871526, - 89.34107228321712, - 49.613769359649325 + 210.91668755526047, + 180.02751146800094, + 165.080603015969, + 200.95343862343222 ] } ] @@ -557,13 +1096,14 @@ "marker_id": 242, "link_name": "Arm1", "position_world_m": [ - 0.1784429017494112, - -0.1605575748858245, - 0.19479801114688738 + 0.21609132484852928, + -0.21472797647660377, + 0.13272865598002226 ], "size_m": 0.025, "observation_count": 0, "mean_confidence": null, + "mean_detector_confidence": null, "mean_reprojection_error_px": null, "observations": [] }, @@ -571,52 +1111,152 @@ "marker_id": 243, "link_name": "Arm1", "position_world_m": [ - 0.1784429017494112, - -0.14902498312161308, - 0.24293323034447747 + 0.21609132484852928, + -0.21930796360231145, + 0.182013782721549 ], "size_m": 0.025, "observation_count": 3, "mean_confidence": 0.9174550709990235, - "mean_reprojection_error_px": 44.17934855555228, + "mean_detector_confidence": 0.9174550709990235, + "mean_reprojection_error_px": 76.42176737251886, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.8899728634608616, - "mean_reprojection_error_px": 78.81046188174007, + "confidence_detector": 0.8899728634608616, + "confidence_weighted": 0.8899728634608616, + "quality": { + "detector_confidence": 0.8899728634608616, + "weighted_confidence": 0.8899728634608616, + "q": { + "size": 0.8810945701599121, + "aspect": 0.9417837479752699, + "border": 1.0, + "center": 0.8596725612878799, + "sharpness": 1.0, + "homography": 0.8333271723300208, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 44.054728507995605, + "edge_ratio": 1.1236297656439467, + "distance_to_border_px": 275.0, + "distance_to_center_norm": 0.14032743871212006, + "laplacian_var": 2678.6956247003295 + } + }, + "mean_reprojection_error_px": 77.81290659108863, "corner_reprojection_errors_px": [ - 101.13197340765323, - 72.09114541671448, - 50.99718387664307, - 91.0215448259495 + 100.17650566852666, + 71.07480085005695, + 49.77846871424659, + 90.22185113152435 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.9132502955127223, - "mean_reprojection_error_px": 16.837449190455786, + "confidence_detector": 0.9132502955127223, + "confidence_weighted": 0.9132502955127223, + "quality": { + "detector_confidence": 0.9132502955127223, + "weighted_confidence": 0.9132502955127223, + "q": { + "size": 1.0, + "aspect": 0.9546584653922503, + "border": 1.0, + "center": 0.8345897197723389, + "sharpness": 0.5809177986450728, + "homography": 0.9082012101971718, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 55.26974582672119, + "edge_ratio": 1.0949900645130086, + "distance_to_border_px": 219.0, + "distance_to_center_norm": 0.16541028022766113, + "laplacian_var": 1452.2944966126818 + } + }, + "mean_reprojection_error_px": 122.71205765924921, "corner_reprojection_errors_px": [ - 14.377732738720587, - 19.045826627822404, - 19.362443297536196, - 14.563794097743955 + 157.5892876335061, + 109.93561027893956, + 83.96066221414912, + 139.36267051040204 ] }, { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.9491420540234864, - "mean_reprojection_error_px": 36.89013459446098, + "confidence_detector": 0.9491420540234864, + "confidence_weighted": 0.9491420540234864, + "quality": { + "detector_confidence": 0.9491420540234864, + "weighted_confidence": 0.9491420540234864, + "q": { + "size": 1.0, + "aspect": 0.9739075220959238, + "border": 1.0, + "center": 0.8123328536748886, + "sharpness": 0.43477113518953137, + "homography": 0.9224349218355518, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 75.11714553833008, + "edge_ratio": 1.0535830709016873, + "distance_to_border_px": 185.0, + "distance_to_center_norm": 0.1876671463251114, + "laplacian_var": 1086.9278379738284 + } + }, + "mean_reprojection_error_px": 28.740337867218727, "corner_reprojection_errors_px": [ - 44.535439947724775, - 1.0820871934520055, - 48.70513064720212, - 53.23788058946502 + 13.91523395008655, + 28.107202384113876, + 48.80100478346, + 24.137910351214487 ] } ] @@ -625,13 +1265,14 @@ "marker_id": 244, "link_name": "Ellbow", "position_world_m": [ - 0.3034429017494112, + 0.3410913248485293, 0.0, 0.0 ], "size_m": 0.025, "observation_count": 0, "mean_confidence": null, + "mean_detector_confidence": null, "mean_reprojection_error_px": null, "observations": [] }, @@ -639,13 +1280,14 @@ "marker_id": 245, "link_name": "Ellbow", "position_world_m": [ - 0.2684429017494112, - 0.02934513796721045, - -0.019075190108761277 + 0.3060913248485293, + 0.009282998148214212, + -0.03374649530514616 ], "size_m": 0.025, "observation_count": 0, "mean_confidence": null, + "mean_detector_confidence": null, "mean_reprojection_error_px": null, "observations": [] }, @@ -653,52 +1295,152 @@ "marker_id": 246, "link_name": "Ellbow", "position_world_m": [ - 0.2684429017494112, - -0.02934513796721045, - 0.019075190108761277 + 0.3060913248485293, + -0.009282998148214212, + 0.03374649530514616 ], "size_m": 0.025, "observation_count": 3, "mean_confidence": 0.6792447690577089, - "mean_reprojection_error_px": 75.53581325353834, + "mean_detector_confidence": 0.6792447690577089, + "mean_reprojection_error_px": 112.48128522376857, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.8015179238063419, - "mean_reprojection_error_px": 130.96780196095426, + "confidence_detector": 0.8015179238063419, + "confidence_weighted": 0.8015179238063419, + "quality": { + "detector_confidence": 0.8015179238063419, + "weighted_confidence": 0.8015179238063419, + "q": { + "size": 0.7650188636779786, + "aspect": 0.9580747137765034, + "border": 1.0, + "center": 0.7408380806446075, + "sharpness": 1.0, + "homography": 0.7832604752816245, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 38.250943183898926, + "edge_ratio": 1.0875198679615226, + "distance_to_border_px": 150.0, + "distance_to_center_norm": 0.25916191935539246, + "laplacian_var": 3236.7051337367984 + } + }, + "mean_reprojection_error_px": 133.93671094067233, "corner_reprojection_errors_px": [ - 131.52551717948674, - 149.219632082114, - 133.91939902794635, - 109.2066595542699 + 134.92477269026298, + 152.54499570886057, + 136.3819175139715, + 111.89515784959431 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.7581700566520715, - "mean_reprojection_error_px": 40.470485652838775, + "confidence_detector": 0.7581700566520715, + "confidence_weighted": 0.7581700566520715, + "quality": { + "detector_confidence": 0.7581700566520715, + "weighted_confidence": 0.7581700566520715, + "q": { + "size": 1.0, + "aspect": 0.8624536105407097, + "border": 1.0, + "center": 0.6523004770278931, + "sharpness": 0.7134050632712029, + "homography": 0.9089132203508788, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 50.77022838592529, + "edge_ratio": 1.3189653049815784, + "distance_to_border_px": 149.0, + "distance_to_center_norm": 0.34769952297210693, + "laplacian_var": 1783.5126581780073 + } + }, + "mean_reprojection_error_px": 163.5949522441255, "corner_reprojection_errors_px": [ - 54.08557804238714, - 23.05261314724495, - 38.690970054967124, - 46.052781366755866 + 164.9348955776692, + 195.44647967290177, + 166.50970828513942, + 127.48872544079168 ] }, { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.4780463267147134, - "mean_reprojection_error_px": 55.16915214682197, + "confidence_detector": 0.4780463267147134, + "confidence_weighted": 0.4780463267147134, + "quality": { + "detector_confidence": 0.4780463267147134, + "weighted_confidence": 0.4780463267147134, + "q": { + "size": 1.0, + "aspect": 0.6468624400661076, + "border": 1.0, + "center": 0.468610942363739, + "sharpness": 0.6283678308439046, + "homography": 0.7101453224803067, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 59.82627487182617, + "edge_ratio": 2.0918474719224776, + "distance_to_border_px": 163.0, + "distance_to_center_norm": 0.531389057636261, + "laplacian_var": 1570.9195771097613 + } + }, + "mean_reprojection_error_px": 39.91219248650781, "corner_reprojection_errors_px": [ - 4.544326976345962, - 67.214794241062, - 103.3039176555024, - 45.61356971437756 + 41.72658331078573, + 33.04479526975366, + 66.19011920628998, + 18.687272159201875 ] } ] @@ -707,52 +1449,152 @@ "marker_id": 247, "link_name": "Ellbow", "position_world_m": [ - 0.23094290174941118, - -0.02934513796721045, - 0.019075190108761277 + 0.26859132484852927, + -0.009282998148214212, + 0.03374649530514616 ], "size_m": 0.025, "observation_count": 3, "mean_confidence": 0.7117000257529918, - "mean_reprojection_error_px": 71.76825320561402, + "mean_detector_confidence": 0.7117000257529918, + "mean_reprojection_error_px": 102.31904563726128, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.8796777163094818, - "mean_reprojection_error_px": 109.30571474994146, + "confidence_detector": 0.8796777163094818, + "confidence_weighted": 0.8796777163094818, + "quality": { + "detector_confidence": 0.8796777163094818, + "weighted_confidence": 0.8796777163094818, + "q": { + "size": 0.7921941375732422, + "aspect": 0.9657885238796377, + "border": 1.0, + "center": 0.7714440822601318, + "sharpness": 1.0, + "homography": 0.8015391946807702, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 39.60970687866211, + "edge_ratio": 1.0708467232203849, + "distance_to_border_px": 184.0, + "distance_to_center_norm": 0.22855591773986816, + "laplacian_var": 4202.582433286217 + } + }, + "mean_reprojection_error_px": 112.21393223629931, "corner_reprojection_errors_px": [ - 125.10305772921419, - 126.687286672494, - 97.67977416118754, - 87.7527404368701 + 128.20833066079467, + 129.7787908831529, + 100.21730102185268, + 90.65130637939703 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.7448005120854795, - "mean_reprojection_error_px": 41.53123348024193, + "confidence_detector": 0.7448005120854795, + "confidence_weighted": 0.7448005120854795, + "quality": { + "detector_confidence": 0.7448005120854795, + "weighted_confidence": 0.7448005120854795, + "q": { + "size": 1.0, + "aspect": 0.8537371543928008, + "border": 1.0, + "center": 0.7152222096920013, + "sharpness": 0.8698383110697441, + "homography": 0.8325296434913341, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 51.05013561248779, + "edge_ratio": 1.3426413969560103, + "distance_to_border_px": 145.0, + "distance_to_center_norm": 0.28477779030799866, + "laplacian_var": 2174.5957776743603 + } + }, + "mean_reprojection_error_px": 133.70775260569667, "corner_reprojection_errors_px": [ - 62.91946766510497, - 50.39956997542899, - 30.245119355881712, - 22.56077692455205 + 153.58015251204978, + 156.00959760114938, + 114.82745208859265, + 110.41380822099482 ] }, { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.5106218488640142, - "mean_reprojection_error_px": 64.46781138665867, + "confidence_detector": 0.5106218488640142, + "confidence_weighted": 0.5106218488640142, + "quality": { + "detector_confidence": 0.5106218488640142, + "weighted_confidence": 0.5106218488640142, + "q": { + "size": 1.0, + "aspect": 0.6760419217397143, + "border": 1.0, + "center": 0.5872762501239777, + "sharpness": 0.9265392364192184, + "homography": 0.6801291688207284, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 58.36018657684326, + "edge_ratio": 1.958396418454695, + "distance_to_border_px": 143.0, + "distance_to_center_norm": 0.41272374987602234, + "laplacian_var": 2316.348091048046 + } + }, + "mean_reprojection_error_px": 61.03545206978789, "corner_reprojection_errors_px": [ - 53.21697434340821, - 81.71744187395875, - 89.31098122106415, - 33.62584810820357 + 81.11386344556932, + 67.01512262029568, + 55.69321608125353, + 40.319606132033016 ] } ] @@ -761,39 +1603,106 @@ "marker_id": 124, "link_name": "Arm2", "position_world_m": [ - 0.1905471840415046, - -0.14689057815962717, - -0.16571856986506908 + 0.18116461496794556, + -0.10738107374625039, + -0.03191480750403911 ], "size_m": 0.025, "observation_count": 2, "mean_confidence": 0.4106676690676095, - "mean_reprojection_error_px": 110.73873242579455, + "mean_detector_confidence": 0.4106676690676095, + "mean_reprojection_error_px": 260.0804359426223, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.34091855704160245, - "mean_reprojection_error_px": 135.68416308738642, + "confidence_detector": 0.34091855704160245, + "confidence_weighted": 0.34091855704160245, + "quality": { + "detector_confidence": 0.34091855704160245, + "weighted_confidence": 0.34091855704160245, + "q": { + "size": 0.7142756080627441, + "aspect": 0.7379840018220383, + "border": 1.0, + "center": 0.8538652509450912, + "sharpness": 1.0, + "homography": 0.7675736815140904, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 35.71378040313721, + "edge_ratio": 1.7100858488288635, + "distance_to_border_px": 332.0, + "distance_to_center_norm": 0.14613474905490875, + "laplacian_var": 3569.5639450778804 + } + }, + "mean_reprojection_error_px": 133.7951322833782, "corner_reprojection_errors_px": [ - 122.09592215917837, - 104.54432626694273, - 149.5516106827851, - 166.54479324063954 + 119.77684898897931, + 102.66323049904321, + 148.05883888894743, + 164.6816107565428 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.4804167810936165, - "mean_reprojection_error_px": 85.79330176420268, + "confidence_detector": 0.4804167810936165, + "confidence_weighted": 0.4804167810936165, + "quality": { + "detector_confidence": 0.4804167810936165, + "weighted_confidence": 0.4804167810936165, + "q": { + "size": 1.0, + "aspect": 0.9604332717578841, + "border": 0.21666666666666667, + "center": 0.49520277976989746, + "sharpness": 0.5040158791602892, + "homography": 0.7617971982706998, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 61.16414737701416, + "edge_ratio": 1.0823934976132112, + "distance_to_border_px": 26.0, + "distance_to_center_norm": 0.5047972202301025, + "laplacian_var": 1260.039697900723 + } + }, + "mean_reprojection_error_px": 386.36573960186644, "corner_reprojection_errors_px": [ - 80.25443845468878, - 44.769514846082515, - 101.1257798229199, - 117.02347393311956 + 336.07046868011287, + 377.8751921124279, + 434.5865464648627, + 396.9307511500621 ] } ] @@ -802,52 +1711,152 @@ "marker_id": 122, "link_name": "Arm2", "position_world_m": [ - 0.2065917439726603, - -0.09049216814460652, - -0.1582492027972934 + 0.2391668855143965, + -0.11496847136439253, + -0.004332331022756229 ], "size_m": 0.025, "observation_count": 3, "mean_confidence": 0.5690911450460799, - "mean_reprojection_error_px": 112.17658015146829, + "mean_detector_confidence": 0.5690911450460799, + "mean_reprojection_error_px": 156.68910851305085, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.18665602922528673, - "mean_reprojection_error_px": 193.21424613028685, + "confidence_detector": 0.18665602922528673, + "confidence_weighted": 0.18665602922528673, + "quality": { + "detector_confidence": 0.18665602922528673, + "weighted_confidence": 0.18665602922528673, + "q": { + "size": 0.7027967643737792, + "aspect": 0.654945658590568, + "border": 1.0, + "center": 0.720594972372055, + "sharpness": 0.9331720780648362, + "homography": 0.539630944261658, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 35.139838218688965, + "edge_ratio": 2.0536884606639982, + "distance_to_border_px": 242.0, + "distance_to_center_norm": 0.27940502762794495, + "laplacian_var": 2332.9301951620905 + } + }, + "mean_reprojection_error_px": 193.48950896029461, "corner_reprojection_errors_px": [ - 163.5154782260392, - 189.8197178899714, - 221.16013279196892, - 198.36165561316793 + 163.89399315841172, + 190.3342215815896, + 221.39744874765296, + 198.33237235352422 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.9456847621099602, - "mean_reprojection_error_px": 59.619753741849934, + "confidence_detector": 0.9456847621099602, + "confidence_weighted": 0.9456847621099602, + "quality": { + "detector_confidence": 0.9456847621099602, + "weighted_confidence": 0.9456847621099602, + "q": { + "size": 1.0, + "aspect": 0.9720842559145406, + "border": 1.0, + "center": 0.6961156129837036, + "sharpness": 0.4361118058809464, + "homography": 0.7295816020733066, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 53.96408939361572, + "edge_ratio": 1.0574348240198506, + "distance_to_border_px": 283.0, + "distance_to_center_norm": 0.3038843870162964, + "laplacian_var": 1090.279514702366 + } + }, + "mean_reprojection_error_px": 181.76476175391187, "corner_reprojection_errors_px": [ - 62.57446736991388, - 94.35842345190635, - 52.23720146118015, - 29.308922684399352 + 138.7329595472684, + 193.13356440282328, + 223.10719220107293, + 172.0853308644829 ] }, { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.5749326438029929, - "mean_reprojection_error_px": 83.69574058226807, + "confidence_detector": 0.5749326438029929, + "confidence_weighted": 0.5749326438029929, + "quality": { + "detector_confidence": 0.5749326438029929, + "weighted_confidence": 0.5749326438029929, + "q": { + "size": 1.0, + "aspect": 0.7301044220084257, + "border": 1.0, + "center": 0.5485925674438477, + "sharpness": 0.34034870646107274, + "homography": 0.7735552097414035, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 75.16977119445801, + "edge_ratio": 1.7393341824971433, + "distance_to_border_px": 251.0, + "distance_to_center_norm": 0.45140743255615234, + "laplacian_var": 850.8717661526819 + } + }, + "mean_reprojection_error_px": 94.81305482494598, "corner_reprojection_errors_px": [ - 95.82678742449872, - 125.24546180132528, - 68.46920157539058, - 45.24151152785769 + 53.41451564830506, + 104.51039185970349, + 144.44374195328376, + 76.8835698384916 ] } ] @@ -856,13 +1865,14 @@ "marker_id": 218, "link_name": "Arm2", "position_world_m": [ - 0.16633861945731776, - -0.03350621972608657, - -0.1118025920534069 + 0.19301576418266206, + -0.10100909858854287, + -0.05507885712581472 ], "size_m": 0.025, "observation_count": 0, "mean_confidence": null, + "mean_detector_confidence": null, "mean_reprojection_error_px": null, "observations": [] }, @@ -870,26 +1880,60 @@ "marker_id": 101, "link_name": "Arm2", "position_world_m": [ - 0.146660650151536, - -0.08689676917303694, - -0.16058631632445308 + 0.18116461496794556, + -0.1748740643565427, + -0.05048080380046753 ], "size_m": 0.025, "observation_count": 1, "mean_confidence": 0.3325004465415643, - "mean_reprojection_error_px": 267.5418618263792, + "mean_detector_confidence": 0.3325004465415643, + "mean_reprojection_error_px": 264.8653586916646, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.3325004465415643, - "mean_reprojection_error_px": 267.5418618263792, + "confidence_detector": 0.3325004465415643, + "confidence_weighted": 0.3325004465415643, + "quality": { + "detector_confidence": 0.3325004465415643, + "weighted_confidence": 0.3325004465415643, + "q": { + "size": 0.7345538902282714, + "aspect": 0.6782259971718826, + "border": 1.0, + "center": 0.6848434209823608, + "sharpness": 1.0, + "homography": 0.7627185514162481, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 36.727694511413574, + "edge_ratio": 1.9488695631540953, + "distance_to_border_px": 265.0, + "distance_to_center_norm": 0.31515657901763916, + "laplacian_var": 3563.870814189421 + } + }, + "mean_reprojection_error_px": 264.8653586916646, "corner_reprojection_errors_px": [ - 251.71309778665415, - 235.71974142034514, - 284.44360570880553, - 298.291002389712 + 248.76482349823732, + 232.76864494991185, + 282.02433999470924, + 295.9036263237998 ] } ] @@ -898,52 +1942,152 @@ "marker_id": 102, "link_name": "Arm2", "position_world_m": [ - 0.1637795636789253, - -0.12583826165265122, - -0.13527321767766717 + 0.2138000507668149, + -0.18474533510517926, + -0.014595760341876172 ], "size_m": 0.025, "observation_count": 3, "mean_confidence": 0.8473641170835915, - "mean_reprojection_error_px": 135.5185983259116, + "mean_detector_confidence": 0.8473641170835915, + "mean_reprojection_error_px": 203.00720872867376, "observations": [ { "view_index": 0, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "confidence": 0.9494437061622057, - "mean_reprojection_error_px": 275.69603657656097, + "confidence_detector": 0.9494437061622057, + "confidence_weighted": 0.9494437061622057, + "quality": { + "detector_confidence": 0.9494437061622057, + "weighted_confidence": 0.9494437061622057, + "q": { + "size": 0.9537287521362304, + "aspect": 0.9740662971297988, + "border": 1.0, + "center": 0.6450249254703522, + "sharpness": 1.0, + "homography": 0.8234129943538817, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 47.68643760681152, + "edge_ratio": 1.0532483321651058, + "distance_to_border_px": 309.0, + "distance_to_center_norm": 0.3549750745296478, + "laplacian_var": 3935.778766550373 + } + }, + "mean_reprojection_error_px": 274.81864950073583, "corner_reprojection_errors_px": [ - 239.57198324805327, - 266.35716033620514, - 310.0207932359541, - 286.8342094860313 + 238.5526540769381, + 266.0117341060898, + 309.2297757901905, + 285.48043402972485 ] }, { "view_index": 1, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "confidence": 0.8913851020933449, - "mean_reprojection_error_px": 62.952236808026484, + "confidence_detector": 0.8913851020933449, + "confidence_weighted": 0.8913851020933449, + "quality": { + "detector_confidence": 0.8913851020933449, + "weighted_confidence": 0.8913851020933449, + "q": { + "size": 1.0, + "aspect": 0.9425738852513735, + "border": 1.0, + "center": 0.6652068793773651, + "sharpness": 0.7931141622847128, + "homography": 0.9999959590215692, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 62.59456253051758, + "edge_ratio": 1.1218495773056807, + "distance_to_border_px": 134.0, + "distance_to_center_norm": 0.3347931206226349, + "laplacian_var": 1982.7854057117818 + } + }, + "mean_reprojection_error_px": 267.48197698933274, "corner_reprojection_errors_px": [ - 32.059770721250835, - 38.09936127795048, - 90.48302392022927, - 91.16679131267536 + 230.94675676068127, + 248.4628484358831, + 301.9047245920965, + 288.61357816867 ] }, { "view_index": 2, "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "confidence": 0.7012635429952238, - "mean_reprojection_error_px": 67.90752159314737, + "confidence_detector": 0.7012635429952238, + "confidence_weighted": 0.7012635429952238, + "quality": { + "detector_confidence": 0.7012635429952238, + "weighted_confidence": 0.7012635429952238, + "q": { + "size": 1.0, + "aspect": 0.8244031865405023, + "border": 1.0, + "center": 0.6730169951915741, + "sharpness": 0.6987050522983833, + "homography": 0.6814939272403986, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "factor": { + "size": 1.0, + "aspect": 1.0, + "border": 1.0, + "center": 1.0, + "sharpness": 1.0, + "homography": 1.0, + "normal_visibility": 1.0, + "spin": 1.0 + }, + "weight_multiplier": 1.0, + "raw": { + "mean_edge_px": 84.7633228302002, + "edge_ratio": 1.4259974156489281, + "distance_to_border_px": 140.0, + "distance_to_center_norm": 0.3269830048084259, + "laplacian_var": 1746.7626307459584 + } + }, + "mean_reprojection_error_px": 66.72099969595267, "corner_reprojection_errors_px": [ - 102.72093538416912, - 41.99761561398122, - 26.866949386310086, - 100.04458598812904 + 110.45363721468823, + 54.785346050607366, + 11.647409112989692, + 89.99760640552542 ] } ] @@ -952,15 +2096,49 @@ "marker_id": 219, "link_name": "Arm2", "position_world_m": [ - 0.16633861945731776, - -0.09182180091572817, - -0.201514870981736 + 0.19301576418266206, + -0.20417695566427538, + -0.08345830860749817 ], "size_m": 0.025, "observation_count": 0, "mean_confidence": null, + "mean_detector_confidence": null, "mean_reprojection_error_px": null, "observations": [] } - ] + ], + "statistics": { + "observation_count": 34, + "camera_count": 3, + "marker_count": 23, + "observed_marker_count": 18, + "mean_detector_confidence": 0.5871913728875101, + "mean_weighted_confidence": 0.5871913728875101, + "mean_reprojection_error_px": 231.06066327218028, + "quality_means": { + "size": 0.8143773112577551, + "aspect": 0.834768084426124, + "border": 0.9237745098039215, + "homography": 0.7934741744632792 + }, + "quality_config": { + "size_ref_px": 50.0, + "border_ref_px": 120.0, + "center_ref_norm": 1.0, + "sharpness_ref": 2500.0, + "homography_ref": 0.18, + "size_factor": 1.0, + "aspect_factor": 1.0, + "border_factor": 1.0, + "center_factor": 1.0, + "sharpness_factor": 1.0, + "homography_factor": 1.0 + } + }, + "solver": { + "final_cost": 24956.498214769963, + "status": 0, + "message": "The maximum number of function evaluations is exceeded." + } } \ No newline at end of file diff --git a/pipeline/multiview_pose_summary.json b/pipeline/multiview_pose_summary.json deleted file mode 100644 index 776a5f9..0000000 --- a/pipeline/multiview_pose_summary.json +++ /dev/null @@ -1,36 +0,0 @@ -{ - "final_cost": 24986.21700952237, - "status": 0, - "message": "The maximum number of function evaluations is exceeded.", - "robot_state": { - "state": { - "x": 100.65052085564044, - "y": 29.661249073864987, - "z": -35.948789524024555, - "a": -119.85307495241203, - "b": 21.99999999999959, - "c": 90.99999999999991, - "e": 10.000000000000025 - }, - "uncertainty": { - "x_mm": 1490.1455410993876, - "y_mm": 429.51650769405427, - "z_mm": 477.27281993275335, - "a_deg": 3533.703361743133, - "b_deg": 13152.589314453906, - "c_deg": 13152.589314528996, - "e_mm": 131525.9153573403 - }, - "confidence": { - "x": 1.922212631331247e-65, - "y": 2.219908541429891e-19, - "z": 1.8719954726562647e-21, - "a": 3.4136023686230135e-154, - "b": 0.0, - "c": 0.0, - "e": 0.36787944117144233 - } - }, - "camera_count": 3, - "marker_count": 23 -} \ No newline at end of file diff --git a/pipeline/render_1a_aruco_detection.json b/pipeline/render_1a_aruco_detection.json index a242210..4fc9276 100644 --- a/pipeline/render_1a_aruco_detection.json +++ b/pipeline/render_1a_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T15:43:43Z", + "created_utc": "2026-05-28T20:31:58Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "713817fc-5f5b-486d-850a-2b1915ab7eac", + "observation_id": "11b87d79-31e1-4293-abe2-4327e2021077", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.9494437061622057 }, { - "observation_id": "cc667670-1b39-4632-8f1c-1a5fdf0e0c7e", + "observation_id": "3f6119be-7ed5-4ba2-b0b8-f2eb85c0f4ac", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.8899728634608616 }, { - "observation_id": "0cd34a97-e5d7-4bbf-aac9-a4e203b31583", + "observation_id": "a963cd6a-dea8-47d2-ba29-7410d1add84e", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.5673043275049208 }, { - "observation_id": "5632f4b1-0db1-4a98-b8d3-9ebbbb4b126f", + "observation_id": "5da8c3b3-2b28-47b4-9132-c5225c5ba0bb", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.8796777163094818 }, { - "observation_id": "75ae283a-9ba7-43bc-bb54-ba44e24e235f", + "observation_id": "b765e797-5dd9-4123-ac41-85b5606132a0", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.8015179238063419 }, { - "observation_id": "7c280cd2-7b94-476a-be3e-7d89e6e35ed3", + "observation_id": "aca8152b-525f-45e7-9849-c46d2123a21c", "type": "aruco", "marker_id": 101, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.3325004465415643 }, { - "observation_id": "2e554970-1749-4924-ac98-3ac4debec51b", + "observation_id": "f0a37d05-7187-4ae1-a53a-2b0260b751b4", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.7952557920267838 }, { - "observation_id": "cc766bc2-4f75-4520-baf3-e10c62f6c25d", + "observation_id": "38cfa10c-246e-439d-ba19-ab664d030ccc", "type": "aruco", "marker_id": 124, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.34091855704160245 }, { - "observation_id": "c7cc54fc-982b-46e8-b176-82eefba18404", + "observation_id": "95edc7e5-0883-4ab5-8b6a-1f35d392d98a", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.26810902547334975 }, { - "observation_id": "7cd2ae3d-740e-4005-a8b8-b23b0ea0082a", + "observation_id": "602d2bac-2427-4211-86be-9a2b5905a4e4", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -586,7 +586,7 @@ "confidence": 0.18665602922528673 }, { - "observation_id": "95fcb10e-a4e9-4d43-8950-8d0523fe95aa", + "observation_id": "308e1529-24a2-49e0-a7ad-6ac4fdead845", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, @@ -640,7 +640,7 @@ "confidence": 0.20150745250271476 }, { - "observation_id": "e14fdb7d-e877-4a87-9e58-430ac0f56f18", + "observation_id": "0159ed42-fec0-4b1c-a383-9ca83f5a320f", "type": "aruco", "marker_id": 211, "marker_size_m": 0.025, @@ -694,7 +694,7 @@ "confidence": 0.6412317666518965 }, { - "observation_id": "7a8a865f-1251-4024-9255-278cb4cb527f", + "observation_id": "4fa2ceb4-7915-4dba-b0f8-9700378eec0b", "type": "aruco", "marker_id": 208, "marker_size_m": 0.025, @@ -748,7 +748,7 @@ "confidence": 0.6280424706698967 }, { - "observation_id": "d920e29d-6dd3-41f5-affb-0d25e5bdb1f7", + "observation_id": "d3bc0898-720e-4180-b70a-57a9adc4a30e", "type": "aruco", "marker_id": 217, "marker_size_m": 0.025, @@ -802,7 +802,7 @@ "confidence": 0.35596573288593486 }, { - "observation_id": "4024cff0-440d-4688-add4-aaef71fd8566", + "observation_id": "d0d5506c-abf1-495f-bce9-33d4b374e599", "type": "aruco", "marker_id": 206, "marker_size_m": 0.025, @@ -856,7 +856,7 @@ "confidence": 0.33820423087105295 }, { - "observation_id": "7019c1d8-7fcf-44ef-8ea5-407577eff29c", + "observation_id": "7b5443c9-fb76-4270-8c88-32c5a20953c1", "type": "aruco", "marker_id": 205, "marker_size_m": 0.025, @@ -910,7 +910,7 @@ "confidence": 0.28724459014346065 }, { - "observation_id": "17746037-a944-46b0-a817-efcd1d0fddb5", + "observation_id": "ac8f8ef1-039a-4762-a4f6-8bdea9e3a4e4", "type": "aruco", "marker_id": 207, "marker_size_m": 0.025, diff --git a/pipeline/render_1b_aruco_detection.json b/pipeline/render_1b_aruco_detection.json index 5380d2c..f2e2679 100644 --- a/pipeline/render_1b_aruco_detection.json +++ b/pipeline/render_1b_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T15:43:43Z", + "created_utc": "2026-05-28T20:31:58Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "b47ae404-2374-4364-bddc-c3e6282f9754", + "observation_id": "e2619c8d-750d-4334-9b42-cb95426ed979", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.8913851020933449 }, { - "observation_id": "5643f91d-1fc0-43db-b40b-78ef116132a8", + "observation_id": "d1d011f0-a579-448e-ace9-ea148ecbc5e7", "type": "aruco", "marker_id": 124, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.4804167810936165 }, { - "observation_id": "eb068542-bc32-4241-9f94-ae93cba29146", + "observation_id": "1b5435e6-7883-434c-a9c7-63f4b8d750d1", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.9132502955127223 }, { - "observation_id": "98da437d-8a22-4a73-8075-137b743bca43", + "observation_id": "a673d83e-73ff-4d2d-a412-470b6da12d56", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.9456847621099602 }, { - "observation_id": "f0ec764f-dbdf-4170-8cff-c3f5c940e82b", + "observation_id": "a755c678-9ad1-4af2-a744-a3c1a8ae0596", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.7448005120854795 }, { - "observation_id": "bddd6015-4815-41b2-bd83-dd9acce3a41b", + "observation_id": "38397299-767f-490d-a47f-a3f09635e995", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.7581700566520715 }, { - "observation_id": "6bb1f556-4a03-4bf1-90ce-59aedab7b2ee", + "observation_id": "273cc5e9-e6e4-4359-8fb1-02a33a935f0d", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.826449608683203 }, { - "observation_id": "dd82dba6-3cea-4e60-b64a-6ad94127dd37", + "observation_id": "8da984fe-3152-43d6-8a39-b4e9266c2184", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.7813266383036261 }, { - "observation_id": "ed953a44-f516-4275-bc07-b6857fe8f8ee", + "observation_id": "92d1be16-65a4-4855-a63c-4d21357d0a9d", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.18684041285479083 }, { - "observation_id": "27bccaa2-ec68-4f8b-991b-585a270ca491", + "observation_id": "5cbcbaf2-7581-4f25-81e3-85b4ce5a03b2", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, diff --git a/pipeline/render_1c_aruco_detection.json b/pipeline/render_1c_aruco_detection.json index 4fe7797..a5b9b3d 100644 --- a/pipeline/render_1c_aruco_detection.json +++ b/pipeline/render_1c_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T15:43:44Z", + "created_utc": "2026-05-28T20:31:59Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "981a123d-7560-4fbe-93f1-d47980472b76", + "observation_id": "6df4525f-58c7-4757-8a5e-130486fc123b", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.7012635429952238 }, { - "observation_id": "7676c338-2b2b-4bdb-864d-b6839291742b", + "observation_id": "c4dd28db-6041-4464-867a-96b25d671918", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.5749326438029929 }, { - "observation_id": "1476f720-1d72-4d94-b24b-b02e5e2d1a76", + "observation_id": "5bdf65ad-9a27-4a28-be1e-6e925df61c72", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.9491420540234864 }, { - "observation_id": "f1bcb96e-0cf8-4c69-a4c1-fbeb03650fa2", + "observation_id": "37b9dec9-2578-4765-a40a-516fbe0424f5", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.4780463267147134 }, { - "observation_id": "a8bf2494-ddd8-4bf6-b87d-42182112e4e3", + "observation_id": "a49c96d9-7884-4ddb-8c07-8f86130b5220", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.5106218488640142 }, { - "observation_id": "642b867d-25d1-4407-9011-f7a62feed435", + "observation_id": "4da1fc9d-a7a9-4dd8-a39b-a667b8d5d165", "type": "aruco", "marker_id": 214, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.6041252446922665 }, { - "observation_id": "623e960d-2058-4672-b06c-5316a5861158", + "observation_id": "a88abb43-d181-4584-b59b-28c65a2fff45", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, diff --git a/robot.json b/robot.json index f23da2b..cbbde2f 100644 --- a/robot.json +++ b/robot.json @@ -95,6 +95,23 @@ "c": null, "e": null }, + "multiview_quality": { + "combine_mode": "mean", + "size_ref_px": 50.0, + "border_ref_px": 120.0, + "center_ref_norm": 1.0, + "sharpness_ref": 2500.0, + "homography_ref": 0.18, + "size_factor": 1.0, + "aspect_factor": 1.0, + "border_factor": 1.0, + "center_factor": 1.0, + "sharpness_factor": 1.0, + "homography_factor": 1.0, + "normal_visibility_factor": 1.0, + "spin_factor": 1.0, + "weight_floor": 0.01 + }, "movements": { "x": null, "y": null, diff --git a/vibeCoding/29.5.txt b/vibeCoding/29.5.txt new file mode 100644 index 0000000..6a464c2 --- /dev/null +++ b/vibeCoding/29.5.txt @@ -0,0 +1,2351 @@ +== MultiView == + +Aus mehreren Bildern von einem Roboter solle die Pose (die Stellung der Gelenke) erkannt werden. + +Dazu existiert schon jede Menge Code +* roboter.json > Info Datei über alle Joings des Roboters. Hier sind auch Render-Informationen für andere Programme hinterlegt. Das läuft stabil und soll nicht ohne Grund verändert werden. Wenn du werte nicht brauchst, ignoriere sie. Wenn du zusätzliche Infos dort speichern willst, sag das. +* 1_detect_aruco_observations.py > Macht openCV Bilderkennung. Liefert pro Datei alle gefundenen Arucos und ArucoCandidates mit Confidence-Level. +* 1_detect_aruco_observations.py erstellt pro png eine Datei render_1a_aruco_detection.json mit allen Markern und Confidence-Leveln. Hier das Beispiel einer solchen Datei. +* 2_Multiview.py > Hier arbeite ich dran, und hier bin ich noch nicht zufrieden. + +Ziel des nächsten Schritts: Baue eine besser Version von 2_Multiview. Es sollen nur die ..aruco_detection.json und das robot.json genommen werden, und daraus die Position des Roboters ermittelt werden. Der Ansatz Simultane Multiview-Optimierung gefällt mir gut. Was mir noch nicht gut gefällt. + +(1). Es ist mindestens ein Fehler drinnen. multiview_pose.json und multiview_pose_summary.json zeigen unterschiedliche werte an. Ich hätte in der _summary die zusammengefassten werte erwartet. + +(2) Die Gewichtung der Arucos ist noch nicht perfekt. Ich will eigentlich den Abstand und die optische-Schärfe, Grösse der Arucos mit berücksichtigen. Grössere Arucos sollen mehr Gewicht erhalten. Schlecht ausgerichtete Arucos sollen weniger Gewicht erhalten. + +(3) Siehst du eine Möglichkeit, die Orientierung (normal und spin) der Arucos mit in die Gewichtung einfliessen zu lassnen? Hintergrund: auf einem gedrehten Bauteil sind die Arucos gedreht. Hier bin ich unsicher, ob das überhaupt möglich ist. + + +Egal was du machst: Lösche nicht einfach code, wenn du nicht genau weisst wofür er ist. Es ist einiges an Arbeit in diesen Code gegangen, behandel ihn entsprechend. + +=== roboter.json === +{ + "coordinateSystem": { + "handedness": "right", + "x": "right", + "y": "backward", + "z": "up" + }, + "units": { + "length": "mm", + "rotation": "degree" + }, + "vision_config": { + "MarkerType": "DICT_4X4_250", + "MarkerSize": 0.025 + }, + "renderingInfo": { + "width": 1280, + "height": 720, + "cameraPosition": [370, -600, 500], + "cameraTarget": [210, -180, 180], + "cameraUpVector": [0, 0, 1], + "lightPosition": [-500, -500, 500], + "lightTarget": [0, 0, 0], + "lightUpVector": [0, 0, 1], + "metric": "mm", + "showSkeleton": true, + "showMarkers": true, + "backgroundColor": [0.70, 0.85, 1.0], + "backgroundStrength": 0.20, + "sunEnergy": 0.35, + "areaEnergy": 120, + "exposure": -1.5, + "materials": { + "wood": { + "baseColor": [0.72, 0.52, 0.33], + "roughness": 0.85, + "metallic": 0.0 + }, + "plaWhite": { + "baseColor": [0.95, 0.95, 0.95], + "roughness": 0.45, + "metallic": 0.0 + }, + "steel": { + "baseColor": [0.72, 0.72, 0.75], + "roughness": 0.25, + "metallic": 1.0 + }, + "powderCoatBlue": { + "baseColor": [0.15, 0.25, 0.7], + "roughness": 0.55, + "metallic": 0.0 + }, + "defaultPlastic": { + "baseColor": [0.95, 0.95, 0.95], + "roughness": 0.4, + "metallic": 0.0 + }, + "skeletonRed": { + "baseColor": [0.85, 0.20, 0.20], + "roughness": 0.35, + "metallic": 0.0 + }, + "markerBlack": { + "baseColor": [0.04, 0.04, 0.04], + "roughness": 0.80, + "metallic": 0.0 + } + }, + "skeletonDefaults": { + "radius": 4, + "color": [0.85, 0.20, 0.20] + }, + "markerDefaults": { + "size": 25, + "thickness": 1, + "color": [0.04, 0.04, 0.04] + } + }, + "defaultPosition": { + "x": 100, + "y": 30, + "z": -30, + "a": -120, + "b": 22, + "c": 91, + "e": 10 + }, + "recognized": { + "x": null, + "y": null, + "z": null, + "a": null, + "b": null, + "c": null, + "e": null + }, + "movements": { + "x": null, + "y": null, + "z": null, + "a": null, + "b": null, + "c": null, + "e": null + }, + "links": { + "Board": { + "parent": null, + "size": [1000, 200, 25], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "skeleton": { + "from": [0, 0, 16], + "to": [1000, 0, 16], + "radius": 4, + "color": [0.85, 0.20, 0.20] + }, + "markers":[ + {"id":210,"position":[20, -20, 0.3], "normal":[0,0,1]}, + {"id":211,"position":[250, -10, 0.3], "normal":[0,0,1]}, + {"id":215,"position":[250, -90, 0.3], "normal":[0,0,1]}, + {"id":214,"position":[350, -10, 0.3], "normal":[0,0,1]}, + {"id":208,"position":[350, -90, 0.3], "normal":[0,0,1]}, + {"id":206,"position":[650, -10, 0.3], "normal":[0,0,1]}, + {"id":205,"position":[750, -90, 0.3], "normal":[0,0,1]}, + {"id":207,"position":[750, -10, 0.3], "normal":[0,0,1]}, + {"id":217,"position":[650, -90, 0.3], "normal":[0,0,1]} + ], + "model": [ + { + "stlFile": "surfaces/Board.stl", + "originOfModel": [0, 0, 0], + "rotationOfModelDegree": [0, 0, -90], + "material": "wood" + }, + { + "stlFile": "surfaces/BoardRail.stl", + "originOfModel": [0, 0, 0], + "rotationOfModelDegree": [0, 0, -90], + "material": "steel" + } + ] + }, + "Base": { + "parent": "Board", + "size": [150, 200, 150], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Slider", + "type": "linear", + "axis": [1, 0, 0], + "origin": [0, 0, 16], + "rotation": [0, 0, 0], + "variable": "x" + }, + "skeleton": { + "from": [0, 108, 45], + "to": [110, 108, 45], + "radius": 4, + "color": [0.20, 0.80, 0.20] + }, + "markers": [ + ], + "model": [ + { + "stlFile": "surfaces/Base.stl", + "originOfModel": [-30, 0, -35], + "rotationOfModelDegree": [0, 0, 0], + "material": "plaWhite" + } + ] + }, + "Arm1": { + "parent": "Base", + "size": [70, 250, 70], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Joint1", + "type": "revolute", + "axis": [-1, 0, 0], + "origin": [110, 108, 45], + "rotation": [0, 0, 0], + "variable": "y" + }, + "skeleton": { + "from": [0, 0, 0], + "to": [0, -250, 0], + "radius": 4, + "color": [0.20, 0.20, 0.90] + }, + "markers": [ + { + "id": 198, + "name": "aruco_198", + "position": [0, -160, 35], + "normal": [0, 0, 1], + "size": 25, + "spin": 0 + }, + { + "id": 229, + "name": "aruco_229", + "position": [0, -250, 35], + "normal": [0, 0, 1], + "size": 25, + "spin": 0 + }, + { + "id": 242, + "name": "aruco_242", + "position": [0, -250, -35], + "normal": [0, 0, -1], + "size": 25, + "spin": 0 + }, + { + "id": 243, + "name": "aruco_243", + "position": [0, -285, 0], + "normal": [0, -1, 0], + "size": 25, + "spin": 0 + } + ], + "model": [ + { + "stlFile": "surfaces/Holm.stl", + "originOfModel__": [-25,29,-28.5], + "originOfModel": [-29,25,28.5], + "rotationOfModelDegree__": [0, 0, 0], + "rotationOfModelDegree": [180, 0, -90], + "material": "powderCoatBlue" + } + ] + }, + "Ellbow": { + "parent": "Arm1", + + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + + "jointToParent": { + "name": "Joint2", + "type": "revolute", + "axis": [-1, 0, 0], + "origin": [0, -250, 0], + "rotation": [0, 0, 0], + "variable": "z" + }, + + "skeleton": { + "from": [0, 0, 0], + "to": [90, 0, 0], + "radius": 4, + "color": [0.90, 0.20, 0.20] + }, + "model": [ + { + "stlFile": "surfaces/Ellebogen.stl", + "originOfModel": [90,0,0], + "rotationOfModelDegree": [0,-90,-90], + "material": "defaultPlastic" + } + ], + "markers": [ + { + "id": 244, + "name": "aruco_244", + "position": [125, 0, 0], + "normal": [1, 0, 0], + "size": 25, + "spin": 0 + }, + { + "id": 245, + "name": "aruco_245", + "position": [90, 0, -35], + "normal": [0, 0, -1], + "size": 25, + "spin": 0 + }, + { + "id": 246, + "name": "aruco_246", + "position": [90, 0, 35], + "normal": [0, 0, 1], + "size": 25 + }, + { + "id": 247, + "name": "aruco_247", + "position": [52.5, 0, 35], + "normal": [0, 0, 1], + "size": 25 + } + ] + + }, + "Arm2": { + "parent": "Ellbow", + + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + + "jointToParent": { + "name": "Joint3", + "type": "revolute", + "axis": [0, -1, 0], + "origin": [90, 0, 0], + "rotation": [0, 0, 0], + "variable": "a" + }, + + "skeleton": { + "from": [0, 0, 0], + "to": [0, -250, 0], + "radius": 4, + "color": [0.95, 0.85, 0.20] + }, + "model": [ + { + "stlFile": "surfaces/Unterarm.stl", + "originOfModel": [0,-250,0], + "rotationOfModelDegree": [180, 0, -90], + "material": "defaultPlastic" + } + ], + "markers":[ + + {"id":124, "position":[24.75, -112, -24.75],"normal":[1,0,-1]}, + {"id":122, "name": "aruco_122", "position":[-35,-112,0], "normal":[-1,0,0]}, + {"id":218, "name": "aruco_218", "position":[35,-112,0], "normal":[1,0,0]}, + {"id":122, "name": "aruco_122", "position":[0, -182, 30],"normal":[0,0,1]}, + {"id":101, "name": "aruco_122", "position":[ 24.75, -182, -24.75],"normal":[ 1,0,-1]}, + {"id":102, "name": "aruco_122", "position":[-24.75, -182, -24.75],"normal":[-1,0,-1]}, + {"id":124, "name": "aruco_124", "position":[-35,-219,0], "normal":[-1,0,0]}, + {"id":219, "name": "aruco_219", "position":[35,-219,0], "normal":[1,0,0]} + + ] + }, + "Hand": { + "parent": "Arm2", + + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + + "jointToParent": { + "name": "Joint4", + "type": "revolute", + "axis": [1, 0, 0], + "origin": [0, -250, 0], + "rotation": [0, 0, 0], + "variable": "b" + }, + + "skeleton": { + "from": [0, 0, 0], + "to": [0, -35, 0], + "radius": 4, + "color": [0.95, 0.55, 0.15] + } + }, + "Palm": { + "parent": "Hand", + + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + + "jointToParent": { + "name": "Joint3", + "type": "revolute", + "axis": [0, -1, 0], + "origin": [0, 0, 0], + "rotation": [0, 0, 0], + "variable": "c" + }, + + "skeleton": { + "from": [-50, -35, 0], + "to": [50, -35, 0], + "radius": 7, + "color": [0.95, 0.20, 0.20] + } + }, + "FingerA": { + "parent": "Palm", + "size": [80, 60, 20], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Slider", + "type": "linear", + "axis": [1, 0, 0], + "origin": [4, -35,0], + "rotation": [0, 0, 0], + "variable": "e" + }, + "skeleton": { + "from": [0, 0,0], + "to": [0, -60, 0], + "radius": 4, + "color": [0.20, 0.80, 0.20] + }, + "model": [ + { + "stlFile": "surfaces/Finger.stl", + "originOfModel": [24,0,-9.1], + "rotationOfModelDegree": [90, -90,0], + "material": "defaultPlastic" + } + ] + }, + "FingerB": { + "parent": "Palm", + "size": [80, 60, 20], + "mountPosition": [0, 0, 0], + "mountRotation": [0, 0, 0], + "jointToParent": { + "name": "Slider", + "type": "linear", + "axis": [-1, 0, 0], + "origin": [-4, -35,0], + "rotation": [0, 0, 0], + "variable": "e" + }, + "skeleton": { + "from": [0, 0,0], + "to": [0, -60, 0], + "radius": 4, + "color": [0.20, 0.80, 0.20] + }, + "model": [ + { + "stlFile": "surfaces/Finger.stl", + "originOfModel": [-24,0,9.1], + "rotationOfModelDegree": [90, 90,0], + "material": "defaultPlastic" + } + ] + } + } +} + + +=== 1_detect_aruco_observations.py === + +#!/usr/bin/env python3 + +import argparse +import json +import os +import hashlib +import time +import uuid +from typing import Dict, Any + +import cv2 +import numpy as np + + +# ------------------------------------------------------------ +# Utilities +# ------------------------------------------------------------ + +def load_intrinsics_npz(npz_path: str): + data = np.load(npz_path) + + for k in ('camera_matrix', 'mtx', 'K'): + if k in data: + K = data[k].astype(np.float32) + break + else: + raise KeyError('Camera matrix not found in npz') + + for k in ('dist_coeffs', 'dist', 'D'): + if k in data: + D = data[k].astype(np.float32).reshape(-1, 1) + break + else: + D = np.zeros((5, 1), dtype=np.float32) + + return K, D + + +# ------------------------------------------------------------ + +def load_robot_vision_config(robot_json_path: str): + + with open(robot_json_path, 'r', encoding='utf-8') as f: + robot = json.load(f) + + vision_config = robot.get('vision_config', {}) + + marker_type = vision_config.get('MarkerType', 'DICT_4X4_250') + marker_size = float(vision_config.get('MarkerSize', 0.025)) + + return { + 'MarkerType': marker_type, + 'MarkerSize': marker_size + } + + +# ------------------------------------------------------------ + +def get_aruco_detector(dict_name: str): + + mapping = { + 'DICT_4X4_250': cv2.aruco.DICT_4X4_250, + 'DICT_5X5_100': cv2.aruco.DICT_5X5_100, + 'DICT_6X6_250': cv2.aruco.DICT_6X6_250, + 'DICT_ARUCO_ORIGINAL': cv2.aruco.DICT_ARUCO_ORIGINAL, + } + + dict_id = mapping.get(dict_name, cv2.aruco.DICT_4X4_250) + + dictionary = cv2.aruco.getPredefinedDictionary(dict_id) + + try: + params = cv2.aruco.DetectorParameters() + except Exception: + params = cv2.aruco.DetectorParameters_create() + + try: + detector = cv2.aruco.ArucoDetector(dictionary, params) + return detector, None + + except Exception: + return None, (dictionary, params) + + +# ------------------------------------------------------------ + +def detect_markers(image, detector_tuple): + + detector, fallback = detector_tuple + + if detector is not None: + + corners, ids, rejected = detector.detectMarkers(image) + + else: + + dictionary, params = fallback + + corners, ids, rejected = cv2.aruco.detectMarkers( + image, + dictionary, + parameters=params + ) + + return corners, ids, rejected + + +# ------------------------------------------------------------ + +def hash_file(path): + + sha = hashlib.sha256() + + with open(path, 'rb') as f: + + while True: + + chunk = f.read(1024 * 1024) + + if not chunk: + break + + sha.update(chunk) + + return sha.hexdigest() + + +# ------------------------------------------------------------ + +def polygon_mask(shape, polygon): + + mask = np.zeros(shape, dtype=np.uint8) + + cv2.fillConvexPoly( + mask, + polygon.astype(np.int32), + 255 + ) + + return mask + + +# ------------------------------------------------------------ + +def shrink_polygon(points, scale=0.80): + + center = np.mean(points, axis=0) + + shrunk = center + (points - center) * scale + + return shrunk.astype(np.float32) + + +# ------------------------------------------------------------ + +def compute_sharpness(gray_image, polygon): + + shrunk = shrink_polygon(polygon, scale=0.80) + + mask = polygon_mask(gray_image.shape, shrunk) + + pixels = gray_image[mask == 255] + + if pixels.size == 0: + return 0.0 + + temp = np.zeros_like(gray_image) + temp[mask == 255] = gray_image[mask == 255] + + lap = cv2.Laplacian(temp, cv2.CV_64F) + + values = lap[mask == 255] + + if values.size == 0: + return 0.0 + + return float(values.var()) + + +# ------------------------------------------------------------ + +def compute_contrast(gray_image, polygon): + + shrunk = shrink_polygon(polygon, scale=0.80) + + mask = polygon_mask(gray_image.shape, shrunk) + + pixels = gray_image[mask == 255] + + if pixels.size == 0: + + return { + 'p05': 0.0, + 'p95': 0.0, + 'dynamic_range': 0.0, + 'mean_gray': 0.0, + 'std_gray': 0.0 + } + + p05 = float(np.percentile(pixels, 5)) + p95 = float(np.percentile(pixels, 95)) + + return { + 'p05': p05, + 'p95': p95, + 'dynamic_range': float(p95 - p05), + 'mean_gray': float(np.mean(pixels)), + 'std_gray': float(np.std(pixels)) + } + + +# ------------------------------------------------------------ + +def compute_edge_ratio(corners): + + edge_lengths = [] + + for k in range(4): + + p1 = corners[k] + p2 = corners[(k + 1) % 4] + + edge_lengths.append( + float(np.linalg.norm(p1 - p2)) + ) + + edge_ratio = ( + max(edge_lengths) / + max(1e-6, min(edge_lengths)) + ) + + return edge_ratio, edge_lengths + + +# ------------------------------------------------------------ + +def compute_geometry_metrics(center, corners, width, height): + + image_center = np.array( + [width / 2.0, height / 2.0], + dtype=np.float32 + ) + + dist_center = np.linalg.norm(center - image_center) + + max_dist = np.linalg.norm(image_center) + + distance_center_norm = float( + dist_center / max(1e-6, max_dist) + ) + + min_x = np.min(corners[:, 0]) + max_x = np.max(corners[:, 0]) + + min_y = np.min(corners[:, 1]) + max_y = np.max(corners[:, 1]) + + border_distance_px = float(min( + min_x, + min_y, + width - max_x, + height - max_y + )) + + return { + 'distance_to_center_norm': distance_center_norm, + 'distance_to_border_px': border_distance_px + } + + +# ------------------------------------------------------------ + +def compute_confidence( + area_px, + sharpness, + edge_ratio, + dynamic_range, + border_distance_px +): + + score = 1.0 + + # area + score *= min(1.0, area_px / 1500.0) + + # sharpness + score *= min(1.0, sharpness / 120.0) + + # edge distortion + score *= 1.0 / max(1.0, edge_ratio) + + # contrast + score *= min(1.0, dynamic_range / 80.0) + + # border distance + score *= min(1.0, max(0.0, border_distance_px) / 50.0) + + score = max(0.0, min(1.0, score)) + + return float(score) + + +# ------------------------------------------------------------ + +def main(): + + parser = argparse.ArgumentParser() + + parser.add_argument( + '-i', + '--image', + required=True + ) + + parser.add_argument( + '-npz', + '--intrinsics', + required=True + ) + + parser.add_argument( + '-robot', + '--robot', + required=True + ) + + parser.add_argument( + '-cameraId', + '--cameraId', + required=True, + type=str + ) + + parser.add_argument( + '-outDir', + '--outDir', + required=True + ) + + args = parser.parse_args() + + os.makedirs(args.outDir, exist_ok=True) + + # -------------------------------------------------------- + # Load robot vision config + # -------------------------------------------------------- + + vision_config = load_robot_vision_config(args.robot) + + marker_type = vision_config['MarkerType'] + marker_size = vision_config['MarkerSize'] + + # -------------------------------------------------------- + # Load image + # -------------------------------------------------------- + + image = cv2.imread(args.image) + + if image is None: + raise RuntimeError(f'Cannot read image: {args.image}') + + gray = cv2.cvtColor( + image, + cv2.COLOR_BGR2GRAY + ) + + height, width = gray.shape[:2] + + # -------------------------------------------------------- + # Intrinsics + # -------------------------------------------------------- + + K, D = load_intrinsics_npz(args.intrinsics) + + # -------------------------------------------------------- + # Detection + # -------------------------------------------------------- + + detector_tuple = get_aruco_detector(marker_type) + + corners_list, ids, rejected = detect_markers( + gray, + detector_tuple + ) + + detections = [] + + # -------------------------------------------------------- + # Valid detections + # -------------------------------------------------------- + + if ids is not None: + + ids = ids.flatten().tolist() + + for i, marker_id in enumerate(ids): + + corners = corners_list[i].reshape((4, 2)).astype(np.float32) + + center = corners.mean(axis=0) + + area_px = float( + cv2.contourArea(corners) + ) + + perimeter_px = float( + cv2.arcLength(corners, True) + ) + + edge_ratio, edge_lengths = compute_edge_ratio(corners) + + sharpness = compute_sharpness( + gray, + corners + ) + + contrast = compute_contrast( + gray, + corners + ) + + geometry = compute_geometry_metrics( + center, + corners, + width, + height + ) + + confidence = compute_confidence( + area_px=area_px, + sharpness=sharpness, + edge_ratio=edge_ratio, + dynamic_range=contrast['dynamic_range'], + border_distance_px=geometry['distance_to_border_px'] + ) + + detection = { + + 'observation_id': str(uuid.uuid4()), + + 'type': 'aruco', + + 'marker_id': int(marker_id), + + 'marker_size_m': marker_size, + + 'image_points_px': corners.tolist(), + + 'center_px': center.tolist(), + + 'quality': { + + 'area_px': area_px, + + 'perimeter_px': perimeter_px, + + 'sharpness': { + 'laplacian_var': sharpness + }, + + 'contrast': contrast, + + 'geometry': geometry, + + 'edge_ratio': edge_ratio, + + 'edge_lengths_px': edge_lengths + }, + + 'confidence': confidence + } + + detections.append(detection) + + # -------------------------------------------------------- + # Rejected candidates + # -------------------------------------------------------- + + rejected_candidates = [] + + if rejected is not None: + + for candidate in rejected: + + pts = candidate.reshape((-1, 2)).astype(np.float32) + + center = pts.mean(axis=0) + + area_px = float( + cv2.contourArea(pts) + ) + + rejected_candidates.append({ + + 'image_points_px': pts.tolist(), + + 'center_px': center.tolist(), + + 'area_px': area_px + }) + + # -------------------------------------------------------- + # Final output + # -------------------------------------------------------- + + output = { + + 'schema_version': '1.0', + + 'created_utc': time.strftime( + '%Y-%m-%dT%H:%M:%SZ', + time.gmtime() + ), + + 'vision_config': { + 'MarkerType': marker_type, + 'MarkerSize': marker_size + }, + + 'camera': { + + 'camera_id': args.cameraId, + + 'intrinsics_file': os.path.abspath(args.intrinsics), + + 'camera_matrix': K.tolist(), + + 'distortion_coefficients': D.reshape(-1).tolist() + }, + + 'image': { + + 'image_file': os.path.abspath(args.image), + + 'image_sha256': hash_file(args.image), + + 'width_px': int(width), + + 'height_px': int(height) + }, + + 'aruco': { + + 'dictionary': marker_type, + + 'num_detected_markers': len(detections), + + 'num_rejected_candidates': len(rejected_candidates) + }, + + 'detections': detections, + + 'rejected_candidates': rejected_candidates + } + + # -------------------------------------------------------- + # Output path + # -------------------------------------------------------- + + input_filename = os.path.basename(args.image) + + input_base = os.path.splitext(input_filename)[0] + + out_json = os.path.join( + args.outDir, + f'{input_base}_aruco_detection.json' + ) + + # -------------------------------------------------------- + # Save JSON + # -------------------------------------------------------- + + with open(out_json, 'w', encoding='utf-8') as f: + + json.dump( + output, + f, + indent=2 + ) + + print(f'Saved: {out_json}') + + +# ------------------------------------------------------------ + +if __name__ == '__main__': + main() + + +=== render_1a_aruco_detection.json === + +{ + "schema_version": "1.0", + "created_utc": "2026-05-28T19:21:53Z", + "vision_config": { + "MarkerType": "DICT_4X4_250", + "MarkerSize": 0.025 + }, + "camera": { + "camera_id": "cam1", + "intrinsics_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render.npz", + "camera_matrix": [ + [ + 1777.77783203125, + 0.0, + 640.0 + ], + [ + 0.0, + 1500.0, + 360.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "distortion_coefficients": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "image": { + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "image_sha256": "693cee15555027b57d05defcf84517eafed047372b0effa201fc0c3fbc9f664f", + "width_px": 1280, + "height_px": 720 + }, + "aruco": { + "dictionary": "DICT_4X4_250", + "num_detected_markers": 17, + "num_rejected_candidates": 411 + }, + "detections": [ + { + "observation_id": "00b95fef-f4ef-4c40-9fba-19831b071579", + "type": "aruco", + "marker_id": 102, + "marker_size_m": 0.025, + "image_points_px": [ + [ + 863.0, + 344.0 + ], + [ + 894.0, + 309.0 + ], + [ + 936.0, + 332.0 + ], + [ + 906.0, + 368.0 + ] + ], + "center_px": [ + 899.75, + 338.25 + ], + "quality": { + "area_px": 2225.5, + "perimeter_px": 190.7457504272461, + "sharpness": { + "laplacian_var": 3935.778766550373 + }, + "contrast": { + "p05": 14.0, + "p95": 185.0, + "dynamic_range": 171.0, + "mean_gray": 101.42780748663101, + "std_gray": 79.4179284235363 + }, + "geometry": { + "distance_to_center_norm": 0.3549750745296478, + "distance_to_border_px": 309.0 + }, + "edge_ratio": 1.0532483321651058, + "edge_lengths_px": [ + 46.75468063354492, + 47.88528060913086, + 46.86149978637695, + 49.24428939819336 + ] + }, + "confidence": 0.9494437061622057 + }, + { + "observation_id": "bc663284-6938-4f10-90f4-27d1370da670", + "type": "aruco", + "marker_id": 243, + "marker_size_m": 0.025, + "image_points_px": [ + [ + 524.0, + 300.0 + ], + [ + 558.0, + 275.0 + ], + [ + 577.0, + 317.0 + ], + [ + 544.0, + 342.0 + ] + ], + "center_px": [ + 550.75, + 308.5 + ], + "quality": { + "area_px": 1894.5, + "perimeter_px": 176.21891403198242, + "sharpness": { + "laplacian_var": 2678.6956247003295 + }, + "contrast": { + "p05": 25.0, + "p95": 191.0, + "dynamic_range": 166.0, + "mean_gray": 82.62843676355067, + "std_gray": 73.67721820744883 + }, + "geometry": { + "distance_to_center_norm": 0.14032743871212006, + "distance_to_border_px": 275.0 + }, + "edge_ratio": 1.1236297656439467, + "edge_lengths_px": [ + 42.20189666748047, + 46.097721099853516, + 41.400482177734375, + 46.51881408691406 + ] + }, + "confidence": 0.8899728634608616 + }, + { + "observation_id": "6d4d7de6-2133-49ad-a26d-567f5eed42ad", + "type": "aruco", + "marker_id": 210, + "marker_size_m": 0.025, + "image_points_px": [ + [ + 137.0, + 273.0 + ], + [ + 858.0, + 269.0 + ] + ], + "center_px": [ + 863.25, + 271.0 + ], + "area_px": 24.5 + } + ] +} + +=== 2_Multiview.py === + +#!/usr/bin/env python3 + +""" +============================================================ +STEP 2b — Simultane Multiview-Optimierung für Roboterpose +============================================================ + +Ziel: + Aus mehreren ArUco-Detektionsdateien die gemeinsame + Roboterpose (x,y,z,a,b,c,e) schätzen und jede Kamera-Pose + sowie Marker-Weltpositionen ausgeben. + +Eingabe: + --robot ../robot.json + --detections render_1a_aruco_detection.json render_1b_aruco_detection.json ... + --outDir . + +Ausgabe: + multiview_pose.json + +Hinweis: + Dieses Skript verwendet die Markerpositionen aus robot.json + als kinematische Constraints und optimiert gleichzeitig: + - Roboterzustand (x,y,z,a,b,c,e) + - Kameraextrinsische Parameter pro Bild + +""" + +import argparse +import datetime +import json +import math +import os +import time +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import cv2 +import numpy as np +from scipy.optimize import least_squares + +STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"] + +# ------------------------------------------------------------------ +# Constraint definitions and validation +# ------------------------------------------------------------------ + +class ConstraintResult: + """Result of validating/applying a single constraint""" + def __init__(self, name: str, enabled: bool, reason: str = ""): + self.name = name + self.enabled = enabled + self.reason = reason + self.residuals = [] + + def __str__(self) -> str: + status = "✓ ENABLED" if self.enabled else "✗ DISABLED" + return f"{self.name:40s} {status:12s} {self.reason}" + + +def validate_constraints(robot: Dict[str, Any], robot_markers: Dict[int, Dict[str, Any]]) -> Dict[str, ConstraintResult]: + """ + Validate which constraints can be applied based on robot geometry. + Returns a dict of constraint_name -> ConstraintResult + """ + results = {} + + # --- Constraint 1: Rigid body distances within each link --- + rigid_body_result = ConstraintResult("RigidBodyDistances", False) + try: + rigid_body_count = 0 + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) >= 2: + rigid_body_count += 1 + if rigid_body_count >= 2: + rigid_body_result.enabled = True + rigid_body_result.reason = f"Found {rigid_body_count} links with 2+ markers each" + else: + rigid_body_result.reason = "Not enough rigid links with multiple markers" + except Exception as e: + rigid_body_result.reason = f"Error: {str(e)}" + results['RigidBodyDistances'] = rigid_body_result + + # --- Constraint 2: Fixed X-distances between links (rotation around X-axis) --- + inter_link_x_result = ConstraintResult("InterLinkXDistances", False) + try: + links_with_markers = set(m['link_name'] for m in robot_markers.values()) + x_rotated_links = [] + for link_name in ['Arm1', 'Ellbow']: + if link_name in links_with_markers: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) >= 1: + x_rotated_links.append(link_name) + if len(x_rotated_links) >= 2: + inter_link_x_result.enabled = True + inter_link_x_result.reason = f"Found {len(x_rotated_links)} X-rotation links: {', '.join(x_rotated_links)}" + else: + inter_link_x_result.reason = "Not enough X-rotation links" + except Exception as e: + inter_link_x_result.reason = f"Error: {str(e)}" + results['InterLinkXDistances'] = inter_link_x_result + + # --- Sanity check (not a hard constraint): Arm2 sin(a) dependency --- + arm2_sina_result = ConstraintResult("Arm2SinADependency", True, "Sanity check only (not enforced)") + try: + arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] + if len(arm2_markers) >= 2: + z_values = set(m['position_m'][2] for m in arm2_markers) + if len(z_values) > 1: + arm2_sina_result.enabled = True + arm2_sina_result.reason = "Multiple Z-values detected; sin(a) dependency confirmed" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "No Z-variation in Arm2 markers (cannot use sin(a) constraint)" + else: + arm2_sina_result.enabled = False + arm2_sina_result.reason = "Not enough Arm2 markers" + except Exception as e: + arm2_sina_result.reason = f"Error: {str(e)}" + results['Arm2SinADependency'] = arm2_sina_result + + return results + + +# ------------------------------------------------------------------ +# JSON helpers +# ------------------------------------------------------------------ + +def load_json(path: str) -> Dict[str, Any]: + with open(path, 'r', encoding='utf-8') as f: + return json.load(f) + + +def save_json(data: Dict[str, Any], path: Path) -> None: + with open(path, 'w', encoding='utf-8') as f: + json.dump(data, f, indent=2) + + +# ------------------------------------------------------------------ +# robot.json helpers +# ------------------------------------------------------------------ + +def resolve_scalar(value: Any, default: float = 0.0) -> float: + if value is None: + return default + if isinstance(value, (int, float)): + return float(value) + try: + return float(str(value).strip()) + except ValueError: + return default + + +def resolve_vector(value: Any, default_len: int = 3) -> Tuple[float, ...]: + if value is None: + return tuple(0.0 for _ in range(default_len)) + if isinstance(value, (int, float, str)): + return (resolve_scalar(value),) + tuple(0.0 for _ in range(default_len - 1)) + if isinstance(value, (list, tuple)): + resolved = [resolve_scalar(v) for v in value] + if len(resolved) < default_len: + resolved.extend([0.0] * (default_len - len(resolved))) + return tuple(resolved[:default_len]) + return tuple(0.0 for _ in range(default_len)) + + +def parse_metric_scale(robot: Dict[str, Any]) -> float: + rendering_info = robot.get('renderingInfo', {}) or {} + metric = rendering_info.get('metric', 'mm') + return 0.001 if str(metric).strip().lower() == 'mm' else 1.0 + + +def normalize_axis(axis: Any) -> np.ndarray: + vec = np.asarray(axis, dtype=np.float64) + if vec.shape != (3,): + vec = vec.reshape(-1)[:3] + norm = np.linalg.norm(vec) + return vec / max(norm, 1e-9) + + +def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray: + x_deg, y_deg, z_deg = resolve_vector(euler_deg, 3) + x = math.radians(x_deg) + y = math.radians(y_deg) + z = math.radians(z_deg) + + cx = math.cos(x) + sx = math.sin(x) + cy = math.cos(y) + sy = math.sin(y) + cz = math.cos(z) + sz = math.sin(z) + + Rx = np.array([ + [1.0, 0.0, 0.0], + [0.0, cx, -sx], + [0.0, sx, cx] + ], dtype=np.float64) + + Ry = np.array([ + [cy, 0.0, sy], + [0.0, 1.0, 0.0], + [-sy, 0.0, cy] + ], dtype=np.float64) + + Rz = np.array([ + [cz, -sz, 0.0], + [sz, cz, 0.0], + [0.0, 0.0, 1.0] + ], dtype=np.float64) + + return Rz @ Ry @ Rx + + +def transform_from_translation_rotation(translation: Any, rotation_deg: Any) -> np.ndarray: + T = np.eye(4, dtype=np.float64) + pos = np.asarray(resolve_vector(translation, 3), dtype=np.float64) + T[:3, 3] = pos + T[:3, :3] = euler_deg_to_matrix(rotation_deg) + return T + + +def axis_angle_matrix(axis: Any, angle_deg: float) -> np.ndarray: + axis_vec = normalize_axis(axis) + theta = math.radians(angle_deg) + kx, ky, kz = axis_vec + c = math.cos(theta) + s = math.sin(theta) + v = 1.0 - c + R = np.array([ + [kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s], + [ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s], + [kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c] + ], dtype=np.float64) + T = np.eye(4, dtype=np.float64) + T[:3, :3] = R + return T + + +# ------------------------------------------------------------------ +# Kinematics and marker extraction +# ------------------------------------------------------------------ + +def extract_markers(robot: Dict[str, Any], scale: float) -> Dict[int, Dict[str, Any]]: + markers = {} + links = robot.get('links', {}) or {} + marker_defaults = (robot.get('renderingInfo', {}) or {}).get('markerDefaults', {}) or {} + default_size_mm = float(marker_defaults.get('size', 25.0)) + + for link_name, link_info in links.items(): + for marker in link_info.get('markers', []) or []: + marker_id = int(marker.get('id', -1)) + if marker_id < 0: + continue + + pos = resolve_vector(marker.get('position', [0, 0, 0]), 3) + size_mm = float(marker.get('size', default_size_mm)) + markers[marker_id] = { + 'marker_id': marker_id, + 'link_name': link_name, + 'position_m': np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64), + 'normal': normalize_axis(resolve_vector(marker.get('normal', [0, 0, 1]), 3)), + 'spin_deg': float(marker.get('spin', 0.0)), + 'size_m': size_mm * scale, + } + + return markers + + +def compute_marker_observation_quality( + corners_px: np.ndarray, + image_shape: Tuple[int, int] +) -> Dict[str, float]: + """ + Compute quality metrics for a marker observation based on: + - Orientation/distortion in the image + - Size (distance) to camera + + Returns dict with: + - 'orientation_quality': 0..1 (1 = frontal, 0 = very skewed) + - 'size_quality': 0..1 (1 = reasonable size, 0 = too small or too large) + - 'combined_quality': product of both + """ + if corners_px.shape[0] != 4: + return { + 'orientation_quality': 0.5, + 'size_quality': 0.5, + 'combined_quality': 0.25 + } + + # Compute edge lengths + corners = corners_px.astype(np.float64) + edges = [ + np.linalg.norm(corners[(i+1) % 4] - corners[i]) + for i in range(4) + ] + + # Orientation quality: low aspect ratio means frontal (1.0 = square) + aspect_ratio = max(edges) / (min(edges) + 1e-6) + # For aspect ratio 1.0->1, 1.5->0.8, 3.0->0.4, 5.0->0.2 + orientation_quality = max(0.1, 2.0 / (1.0 + aspect_ratio)) + + # Size quality: area-based + area_px = cv2.contourArea(corners.astype(np.int32)) + image_area = image_shape[0] * image_shape[1] + + # Ideal: marker should be 5-20% of image area + # Too small: <1%, too large: >40% + area_ratio = area_px / image_area + + if area_ratio < 0.01: + size_quality = 0.3 + area_ratio * 20 # linear growth 0-0.3 + elif area_ratio > 0.4: + size_quality = 0.3 + (1.0 - area_ratio) * 2.33 # linear decay from 1.0 + else: + # 1% to 40%: bell curve, max at ~5-20% + ideal_ratio = 0.10 + size_quality = 1.0 - (area_ratio - ideal_ratio) ** 2 / (0.15 ** 2) + size_quality = max(0.3, min(1.0, size_quality)) + + combined_quality = orientation_quality * size_quality + + return { + 'orientation_quality': float(orientation_quality), + 'size_quality': float(size_quality), + 'combined_quality': float(combined_quality) + } + + +def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, np.ndarray]: + n = normalize_axis(normal) + candidate = np.array((0.0, 0.0, 1.0), dtype=np.float64) + if abs(np.dot(n, candidate)) > 0.99: + candidate = np.array((1.0, 0.0, 0.0), dtype=np.float64) + + x_dir = np.cross(candidate, n) + x_dir /= max(np.linalg.norm(x_dir), 1e-9) + y_dir = np.cross(n, x_dir) + + if abs(spin_deg) > 1e-6: + theta = math.radians(spin_deg) + cos_t = math.cos(theta) + sin_t = math.sin(theta) + x_rot = x_dir * cos_t + np.cross(n, x_dir) * sin_t + n * np.dot(n, x_dir) * (1.0 - cos_t) + y_rot = y_dir * cos_t + np.cross(n, y_dir) * sin_t + n * np.dot(n, y_dir) * (1.0 - cos_t) + return x_rot, y_rot + + return x_dir, y_dir + + +def marker_object_corners(marker: Dict[str, Any]) -> np.ndarray: + half = marker['size_m'] * 0.5 + x_dir, y_dir = marker_plane_axes(marker['normal'], marker['spin_deg']) + corners = np.stack([ + -x_dir * half + y_dir * half, + x_dir * half + y_dir * half, + x_dir * half - y_dir * half, + -x_dir * half - y_dir * half + ], axis=0) + return marker['position_m'].reshape(1, 3) + corners + + +def build_link_chain(robot: Dict[str, Any]) -> List[str]: + links = robot.get('links', {}) or {} + ordered: List[str] = [] + remaining = set(links.keys()) + + while remaining: + progress = False + for name in list(remaining): + parent = links[name].get('parent') + if not parent or parent in ordered: + ordered.append(name) + remaining.remove(name) + progress = True + if not progress: + raise RuntimeError('Cycle detected in robot link tree or missing parent link') + return ordered + + +def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]: + links = robot.get('links', {}) or {} + ordered_links = build_link_chain(robot) + transforms: Dict[str, np.ndarray] = {} + + for link_name in ordered_links: + link_info = links[link_name] or {} + parent_name = link_info.get('parent') + parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64) + + mount_translation = np.asarray(resolve_vector(link_info.get('mountPosition', [0, 0, 0]), 3), dtype=np.float64) * scale + mount = transform_from_translation_rotation( + mount_translation, + link_info.get('mountRotation', [0, 0, 0]) + ) + + joint_info = link_info.get('jointToParent', {}) or {} + joint_origin = np.asarray(resolve_vector(joint_info.get('origin', [0, 0, 0]), 3), dtype=np.float64) * scale + joint = transform_from_translation_rotation( + joint_origin, + joint_info.get('rotation', [0, 0, 0]) + ) + + motion = np.eye(4, dtype=np.float64) + joint_type = str(joint_info.get('type', 'fixed')).strip().lower() + control_var = str(joint_info.get('variable', joint_info.get('control', ''))).strip().lower() + axis = resolve_vector(joint_info.get('axis', [1, 0, 0]), 3) + + if joint_type == 'linear': + motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale + elif joint_type == 'revolute': + motion = axis_angle_matrix(axis, state.get(control_var, 0.0)) + + transforms[link_name] = parent_transform @ mount @ joint @ motion + + return transforms + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = np.ones(4, dtype=np.float64) + local[:3] = marker['position_m'] + world = link_transform @ local + return world[:3] + + +# ------------------------------------------------------------------ +# Camera / observation helpers +# ------------------------------------------------------------------ + +def load_intrinsics(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: + cam = detection_json['camera'] + K = np.asarray(cam['camera_matrix'], dtype=np.float64) + D = np.asarray(cam.get('distortion_coefficients', [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1) + return K, D + + +def collect_views_and_observations( + detection_files: List[str], + robot_markers: Dict[int, Dict[str, Any]] +) -> Tuple[List[Dict[str, Any]], List[Dict[str, Any]]]: + views: List[Dict[str, Any]] = [] + observations: List[Dict[str, Any]] = [] + + for idx, det_path in enumerate(detection_files): + detection_json = load_json(det_path) + K, D = load_intrinsics(detection_json) + image_shape = detection_json.get('image', {}).get('image_shape') + if not image_shape: + image_shape = (720, 1280) # fallback + + views.append({ + 'index': idx, + 'source_file': os.path.abspath(det_path), + 'camera_id': detection_json.get('camera', {}).get('camera_id', f'cam{idx+1}'), + 'image_file': detection_json.get('image', {}).get('image_file'), + 'image_shape': image_shape, + 'K': K, + 'D': D + }) + + for det in detection_json.get('detections', []) or []: + marker_id = int(det.get('marker_id', -1)) + if marker_id < 0 or marker_id not in robot_markers: + continue + + image_points = det.get('image_points_px') + if isinstance(image_points, list) and len(image_points) == 4: + image_points = np.asarray(image_points, dtype=np.float64) + else: + center = resolve_vector(det.get('center_px', [0, 0]), 2) + image_points = np.asarray([center], dtype=np.float64) + + confidence = float(det.get('confidence', 1.0)) + + # Compute observation quality metrics + quality_metrics = compute_marker_observation_quality(image_points, tuple(image_shape)) + + # Blend base confidence and observation quality instead of multiplying them. + # Reduce the distance/size-based quality influence by roughly 40% relative to the previous blend. + base_confidence = max(0.01, min(1.0, confidence)) + combined_quality = quality_metrics['combined_quality'] + combined_confidence = base_confidence * 0.76 + combined_quality * 0.24 + + marker = robot_markers[marker_id] + observations.append({ + 'view_index': idx, + 'marker_id': marker_id, + 'marker_link_corners': marker_object_corners(marker), + 'image_points_px': image_points, + 'confidence_base': base_confidence, + 'quality_metrics': quality_metrics, + 'confidence': max(0.01, min(1.0, combined_confidence)) + }) + + if len(views) == 0: + raise RuntimeError('No valid detection views found') + + if len(observations) == 0: + raise RuntimeError('No marker observations matched robot.json markers') + + return views, observations + + +def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = marker_object_corners(marker) + homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1) + world = (link_transform @ homogeneous.T).T + return world[:, :3] + + +def initial_camera_guess( + view: Dict[str, Any], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + default_state: Dict[str, float], + scale: float, + robot: Dict[str, Any] +) -> Tuple[np.ndarray, np.ndarray]: + object_points = [] + image_points = [] + + link_transforms = compute_link_transforms(robot, default_state, scale) + + for obs in observations: + if obs['view_index'] != view['index']: + continue + marker = robot_markers[obs['marker_id']] + object_points.append(compute_marker_world_corners(marker, link_transforms)) + image_points.append(obs['image_points_px']) + + if len(object_points) == 0: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + object_points = np.vstack(object_points) + image_points = np.vstack(image_points) + + if object_points.shape[0] < 4: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + success, rvec, tvec = cv2.solvePnP( + object_points, + image_points, + view['K'], + view['D'], + flags=cv2.SOLVEPNP_ITERATIVE + ) + + if not success: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + return rvec, tvec + + +def project_points( + points_3d: np.ndarray, + rvec: np.ndarray, + tvec: np.ndarray, + K: np.ndarray, + D: np.ndarray +) -> np.ndarray: + projected, _ = cv2.projectPoints(points_3d, rvec, tvec, K, D) + return projected.reshape(-1, 2) + + +def compute_soft_constraint_residuals( + robot_state: Dict[str, float], + robot_markers: Dict[int, Dict[str, Any]], + link_transforms: Dict[str, np.ndarray], + robot: Dict[str, Any], + enabled_constraints: Dict[str, ConstraintResult] +) -> List[float]: + """ + Compute residuals from soft constraints (kinematic consistency, rigid body distances). + Returns a list of constraint residuals to append to the total residual vector. + """ + residuals = [] + weight_scale = 0.1 # Weight for soft constraints relative to reprojection errors + + # Constraint 1: Rigid body distances within each link + if enabled_constraints['RigidBodyDistances'].enabled: + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) < 2: + continue + + # Compute all pairwise distances in world coords + for i in range(len(link_markers)): + for j in range(i + 1, len(link_markers)): + m_i = link_markers[i] + m_j = link_markers[j] + + pos_i = compute_marker_world_position(m_i, link_transforms) + pos_j = compute_marker_world_position(m_j, link_transforms) + + dist_world = np.linalg.norm(pos_i - pos_j) + + # Reference distance in local coords + dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m']) + + # Residual: difference should be zero (rigid body) + error = dist_world - dist_local + residuals.append(error * weight_scale * 0.1) # Very soft weight + + # Constraint 2: Fixed X-distances between links (Arm1 <-> Ellbow) + if enabled_constraints['InterLinkXDistances'].enabled: + arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1'] + ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow'] + + if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: + # Get first marker from each link + m_arm1 = arm1_markers[0] + m_ellbow = ellbow_markers[0] + + pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) + pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) + + # X-distance in world should match reference (relative position) + # Since both rotate around X-axis at different points, we check consistency + x_diff_world = pos_ellbow[0] - pos_arm1[0] + x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0] + + error = x_diff_world - x_diff_ref + residuals.append(error * weight_scale) + + return residuals + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + """Compute the world position of a marker given current link transforms.""" + link_transform = link_transforms[marker['link_name']] + local_pos = np.concatenate([marker['position_m'], [1.0]]) + world_pos = (link_transform @ local_pos)[:3] + return world_pos + + +# ------------------------------------------------------------------ +# Optimization +# ------------------------------------------------------------------ + +def pack_parameters(robot_state: Dict[str, float], camera_params: List[Tuple[np.ndarray, np.ndarray]]) -> np.ndarray: + state_vec = np.asarray([robot_state[k] for k in STATE_KEYS], dtype=np.float64) + cams = [] + for rvec, tvec in camera_params: + cams.append(rvec.reshape(3)) + cams.append(tvec.reshape(3)) + return np.concatenate([state_vec] + cams) + + +def unpack_parameters(params: np.ndarray, n_views: int) -> Tuple[Dict[str, float], List[Tuple[np.ndarray, np.ndarray]]]: + robot_state = {STATE_KEYS[i]: float(params[i]) for i in range(len(STATE_KEYS))} + camera_params = [] + offset = len(STATE_KEYS) + for _ in range(n_views): + rvec = params[offset:offset + 3].reshape(3, 1) + tvec = params[offset + 3:offset + 6].reshape(3, 1) + camera_params.append((rvec, tvec)) + offset += 6 + return robot_state, camera_params + + +def residuals_for_parameters( + params: np.ndarray, + views: List[Dict[str, Any]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + robot: Dict[str, Any], + scale: float, + default_state: Dict[str, float], + enabled_constraints: Dict[str, ConstraintResult] +) -> np.ndarray: + robot_state, camera_params = unpack_parameters(params, len(views)) + link_transforms = compute_link_transforms(robot, robot_state, scale) + + residuals = [] + + # Reprojection residuals (primary observation) + for obs in observations: + marker = robot_markers[obs['marker_id']] + world_corners = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(world_corners, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + weight = math.sqrt(obs['confidence']) + residuals.extend((diffs * weight).reshape(-1)) + + # Weak priors on robot state + for key in STATE_KEYS: + diff = robot_state[key] - default_state.get(key, 0.0) + if key in ('x', 'y', 'z', 'e'): + w = 0.001 + else: + w = 0.01 + residuals.append(diff * w) + + # Soft constraints (kinematic consistency, rigid body constraints) + soft_constraint_residuals = compute_soft_constraint_residuals( + robot_state, robot_markers, link_transforms, robot, enabled_constraints + ) + residuals.extend(soft_constraint_residuals) + + return np.asarray(residuals, dtype=np.float64) + + + +def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray: + if result.jac is None: + return np.full(n_params, float('nan'), dtype=np.float64) + J = result.jac + m, n = J.shape + JTJ = J.T @ J + try: + cov = np.linalg.pinv(JTJ) + except np.linalg.LinAlgError: + cov = np.linalg.pinv(JTJ + np.eye(n) * 1e-9) + residuals = result.fun + dof = max(1, m - n) + sigma2 = float(np.sum(residuals ** 2) / dof) + cov *= sigma2 + return np.sqrt(np.diag(cov)) + + +def print_constraint_sanity_check( + robot_state: Dict[str, float], + robot_markers: Dict[int, Dict[str, Any]], + link_transforms: Dict[str, np.ndarray], + robot: Dict[str, Any], + enabled_constraints: Dict[str, ConstraintResult] +) -> None: + """ + Print sanity checks for all constraints to verify the optimization result. + """ + print("\n" + "=" * 70) + print("CONSTRAINT SANITY CHECKS (after optimization)") + print("=" * 70) + + # Check 1: Rigid body distances + if enabled_constraints['RigidBodyDistances'].enabled: + print("\n1. RIGID BODY DISTANCES") + for link_name in ['Arm1', 'Ellbow', 'Arm2']: + link_markers = [m for m in robot_markers.values() if m['link_name'] == link_name] + if len(link_markers) < 2: + continue + + max_error = 0.0 + for i in range(len(link_markers)): + for j in range(i + 1, len(link_markers)): + m_i = link_markers[i] + m_j = link_markers[j] + + pos_i = compute_marker_world_position(m_i, link_transforms) + pos_j = compute_marker_world_position(m_j, link_transforms) + + dist_world = np.linalg.norm(pos_i - pos_j) + dist_local = np.linalg.norm(m_i['position_m'] - m_j['position_m']) + error = abs(dist_world - dist_local) + max_error = max(max_error, error) + + status = "✓" if max_error < 1.0 else "⚠" if max_error < 5.0 else "✗" + print(f" {link_name:10s}: max_error = {max_error:.3f} mm {status}") + + # Check 2: Inter-link X distances + if enabled_constraints['InterLinkXDistances'].enabled: + print("\n2. INTER-LINK X-DISTANCES") + arm1_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm1'] + ellbow_markers = [m for m in robot_markers.values() if m['link_name'] == 'Ellbow'] + + if len(arm1_markers) >= 1 and len(ellbow_markers) >= 1: + m_arm1 = arm1_markers[0] + m_ellbow = ellbow_markers[0] + + pos_arm1 = compute_marker_world_position(m_arm1, link_transforms) + pos_ellbow = compute_marker_world_position(m_ellbow, link_transforms) + + x_diff_world = pos_ellbow[0] - pos_arm1[0] + x_diff_ref = m_ellbow['position_m'][0] - m_arm1['position_m'][0] + error = abs(x_diff_world - x_diff_ref) + + status = "✓" if error < 1.0 else "⚠" if error < 5.0 else "✗" + print(f" Arm1 <-> Ellbow: error = {error:.3f} mm {status}") + + # Check 3: Arm2 sin(a) dependency + if enabled_constraints['Arm2SinADependency'].enabled: + print("\n3. ARM2 sin(a) DEPENDENCY (sanity check)") + arm2_markers = [m for m in robot_markers.values() if m['link_name'] == 'Arm2'] + if len(arm2_markers) >= 2: + # Check that markers with different Z values have different X spreads + a_rad = math.radians(robot_state['a']) + sin_a = math.sin(a_rad) + cos_a = math.cos(a_rad) + + z_variations = {} + for m in arm2_markers: + z_local = m['position_m'][2] + x_local = m['position_m'][0] + pos_world = compute_marker_world_position(m, link_transforms) + x_world = pos_world[0] + + # Expected: x_world = 90 + x_local * cos(a) - z_local * sin(a) + x_expected = 90 * (robot.get('renderingInfo', {}).get('metric', 'mm') == 'mm' and 0.09 or 0.09) + x_local * cos_a - z_local * sin_a + x_error = abs(x_world - x_expected) + + if z_local not in z_variations: + z_variations[z_local] = [] + z_variations[z_local].append(x_error) + + max_error = max(max(errors) for errors in z_variations.values()) if z_variations else 0.0 + status = "✓" if max_error < 5.0 else "⚠" if max_error < 10.0 else "⚠" + print(f" X-consistency with sin(a): max_error = {max_error:.3f} mm {status}") + print(f" (Note: this is a consistency check, not a hard constraint)") + + print("=" * 70) + + +def camera_position_world(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: + R, _ = cv2.Rodrigues(rvec) + return (-R.T @ tvec).reshape(3) + + +def build_output( + robot_state: Dict[str, float], + state_uncertainty: np.ndarray, + views: List[Dict[str, Any]], + camera_params: List[Tuple[np.ndarray, np.ndarray]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + scale: float, + robot: Dict[str, Any], + robot_json_path: str +) -> Dict[str, Any]: + link_transforms = compute_link_transforms(robot, robot_state, scale) + + marker_summary: Dict[int, Dict[str, Any]] = {} + for marker_id, marker in robot_markers.items(): + marker_summary[marker_id] = { + 'marker_id': marker_id, + 'link_name': marker['link_name'], + 'position_world_m': compute_marker_world_position(marker, link_transforms).tolist(), + 'size_m': marker['size_m'], + 'observation_count': 0, + 'mean_confidence': None, + 'mean_reprojection_error_px': None, + 'observations': [] + } + + per_marker_errors: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + per_marker_confidences: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + + link_transforms = compute_link_transforms(robot, robot_state, scale) + for obs in observations: + marker_id = obs['marker_id'] + marker = robot_markers[marker_id] + object_points_m = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(object_points_m, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + errors = np.linalg.norm(diffs, axis=1) + repro_error = float(np.mean(errors)) + per_marker_errors[marker_id].extend(errors.tolist()) + per_marker_confidences[marker_id].append(obs['confidence']) + marker_summary[marker_id]['observation_count'] += 1 + marker_summary[marker_id]['observations'].append({ + 'view_index': obs['view_index'], + 'source_file': views[obs['view_index']]['source_file'], + 'image_file': views[obs['view_index']]['image_file'], + 'confidence': obs['confidence'], + 'mean_reprojection_error_px': repro_error, + 'corner_reprojection_errors_px': errors.tolist() + }) + + for marker_id, summary in marker_summary.items(): + if summary['observation_count'] > 0: + summary['mean_confidence'] = float(np.mean(per_marker_confidences[marker_id])) + summary['mean_reprojection_error_px'] = float(np.mean(per_marker_errors[marker_id])) + + camera_outputs = [] + for idx, view in enumerate(views): + rvec, tvec = camera_params[idx] + cam_pos = camera_position_world(rvec, tvec) + observed_count = sum(1 for obs in observations if obs['view_index'] == idx) + camera_outputs.append({ + 'view_index': idx, + 'source_file': view['source_file'], + 'camera_id': view['camera_id'], + 'camera_position_world_m': cam_pos.tolist(), + 'rvec': rvec.reshape(-1).tolist(), + 'tvec': tvec.reshape(-1).tolist(), + 'intrinsics': { + 'camera_matrix': view['K'].tolist(), + 'distortion_coefficients': view['D'].reshape(-1).tolist() + }, + 'observation_count': observed_count + }) + + robot_pose_output = { + 'state': {k: float(robot_state[k]) for k in STATE_KEYS}, + 'uncertainty': { + 'x_mm': float(state_uncertainty[0]), + 'y_mm': float(state_uncertainty[1]), + 'z_mm': float(state_uncertainty[2]), + 'a_deg': float(state_uncertainty[3]), + 'b_deg': float(state_uncertainty[4]), + 'c_deg': float(state_uncertainty[5]), + 'e_mm': float(state_uncertainty[6]) + }, + 'confidence': { + 'x': float(math.exp(-state_uncertainty[0] / 10.0)), + 'y': float(math.exp(-state_uncertainty[1] / 10.0)), + 'z': float(math.exp(-state_uncertainty[2] / 10.0)), + 'a': float(math.exp(-state_uncertainty[3] / 10.0)), + 'b': float(math.exp(-state_uncertainty[4] / 10.0)), + 'c': float(math.exp(-state_uncertainty[5] / 10.0)), + 'e': float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6]))) + } + } + + return { + 'schema_version': '1.0', + 'created_utc': datetime.datetime.utcnow().isoformat() + 'Z', + 'source_robot_json': os.path.abspath(robot_json_path), + 'source_detections': [view['source_file'] for view in views], + 'robot_pose': robot_pose_output, + 'camera_poses': camera_outputs, + 'marker_positions': list(marker_summary.values()) + } + + +# ------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------ + +def main() -> None: + parser = argparse.ArgumentParser(description='Multiview optimization of robot pose and camera extrinsics') + parser.add_argument('--robot', required=True, help='Path to robot.json') + parser.add_argument('--detections', required=True, nargs='+', help='List of detection JSON files') + parser.add_argument('--outDir', required=True, help='Output directory') + parser.add_argument('--write-summary', action='store_true', help='Write summary file') + parser.add_argument('--max-iter', type=int, default=500, help='Maximum optimizer iterations') + args = parser.parse_args() + + os.makedirs(args.outDir, exist_ok=True) + + robot_json_path = os.path.abspath(args.robot) + robot = load_json(robot_json_path) + scale = parse_metric_scale(robot) + + default_state = { + k: float(robot.get('defaultPosition', {}).get(k, 0.0) or 0.0) + for k in STATE_KEYS + } + + robot_markers = extract_markers(robot, scale) + + # Validate constraints + print("\n" + "=" * 70) + print("CONSTRAINT VALIDATION") + print("=" * 70) + enabled_constraints = validate_constraints(robot, robot_markers) + for constraint_name, result in enabled_constraints.items(): + print(result) + print("=" * 70) + + views, observations = collect_views_and_observations(args.detections, robot_markers) + + # Print observation quality summary + print("\n" + "=" * 70) + print("OBSERVATION QUALITY SUMMARY") + print("=" * 70) + print(f"Total observations: {len(observations)}") + print() + + # Aggregate quality metrics + quality_by_marker = {} + for obs in observations: + mid = obs['marker_id'] + if mid not in quality_by_marker: + quality_by_marker[mid] = [] + quality_by_marker[mid].append(obs['quality_metrics']) + + print(f"{'Marker':>8} {'Link':>12} {'Count':>6} {'Avg Orient.':>12} {'Avg Size':>12} {'Avg Conf.':>12}") + print("-" * 70) + for marker_id in sorted(quality_by_marker.keys()): + marker = robot_markers[marker_id] + quality_list = quality_by_marker[marker_id] + avg_orient = np.mean([q['orientation_quality'] for q in quality_list]) + avg_size = np.mean([q['size_quality'] for q in quality_list]) + + obs_for_marker = [o for o in observations if o['marker_id'] == marker_id] + avg_conf = np.mean([o['confidence'] for o in obs_for_marker]) + + print(f"{marker_id:8d} {marker['link_name']:>12} {len(quality_list):6d} " + f"{avg_orient:12.3f} {avg_size:12.3f} {avg_conf:12.3f}") + print("=" * 70) + + camera_guesses = [] + for view in views: + rvec, tvec = initial_camera_guess(view, observations, robot_markers, default_state, scale, robot) + camera_guesses.append((rvec, tvec)) + + + x0 = pack_parameters(default_state, camera_guesses) + + progress = { + 'iter': 0, + 'last_cost': None, + 'last_print': time.time(), + 'prev_x': x0.copy() + } + + def progress_callback(xk: np.ndarray) -> None: + progress['iter'] += 1 + now = time.time() + if progress['iter'] == 1 or now - progress['last_print'] >= 1.0: + res = residuals_for_parameters(xk, views, observations, robot_markers, robot, scale, default_state, enabled_constraints) + cost = 0.5 * float(np.dot(res, res)) + delta_cost = None + convergence = '' + if progress['last_cost'] is not None: + delta_cost = cost - progress['last_cost'] + if abs(delta_cost) < 1e-3: + convergence = ' stable' + elif delta_cost < 0: + convergence = ' improving' + else: + convergence = ' worsening' + step_norm = float(np.linalg.norm(xk - progress['prev_x'])) + print( + f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}' + + (f' delta={delta_cost:.4g}' if delta_cost is not None else '') + + f' step={step_norm:.4g}' + + convergence + ) + progress['last_cost'] = cost + progress['last_print'] = now + progress['prev_x'] = xk.copy() + + result = least_squares( + residuals_for_parameters, + x0, + args=(views, observations, robot_markers, robot, scale, default_state, enabled_constraints), + jac='2-point', + method='trf', + loss='soft_l1', + f_scale=1.0, + max_nfev=args.max_iter, + callback=progress_callback + ) + + robot_state, camera_params = unpack_parameters(result.x, len(views)) + uncertainties = estimate_uncertainty(result, len(result.x)) + + # Print constraint sanity checks + link_transforms = compute_link_transforms(robot, robot_state, scale) + print_constraint_sanity_check(robot_state, robot_markers, link_transforms, robot, enabled_constraints) + + output = build_output(robot_state, uncertainties[:len(STATE_KEYS)], views, camera_params, observations, robot_markers, scale, robot, robot_json_path) + + out_path = Path(args.outDir) / 'multiview_pose.json' + save_json(output, out_path) + + print(f'Saved: {out_path}') + if args.write_summary: + summary_path = Path(args.outDir) / 'multiview_pose_summary.json' + summary = { + 'final_cost': float(result.cost), + 'status': int(result.status), + 'message': result.message, + 'robot_state': output['robot_pose'], + 'camera_count': len(views), + 'marker_count': len(robot_markers) + } + save_json(summary, summary_path) + print(f'Saved: {summary_path}') + + +if __name__ == '__main__': + main() + + + +========= + + +Aus mehreren Bildern von einem Roboter solle die Pose (die Stellung der Gelenke) erkannt werden. + +Dazu existiert schon jede Menge Code +* roboter.json > Info Datei über alle Joings des Roboters. Hier sind auch Render-Informationen für andere Programme hinterlegt. Das läuft stabil und soll nicht ohne Grund verändert werden. Wenn du werte nicht brauchst, ignoriere sie. Wenn du zusätzliche Infos dort speichern willst, sag das. +* 1_detect_aruco_observations.py > Macht openCV Bilderkennung. Liefert pro Datei alle gefundenen Arucos und ArucoCandidates mit Confidence-Level. +* 1_detect_aruco_observations.py erstellt pro png eine Datei render_1a_aruco_detection.json mit allen Markern und Confidence-Leveln. Hier das Beispiel einer solchen Datei. +* 2_Multiview.py > Hier arbeite ich dran, und hier bin ich noch nicht zufrieden. + +Ziel des nächsten Schritts: Baue eine besser Version von 2_Multiview. Es sollen nur die ..aruco_detection.json und das robot.json genommen werden, und daraus die Position des Roboters ermittelt werden. Der Ansatz Simultane Multiview-Optimierung gefällt mir gut. Was mir noch nicht gut gefällt. + +(1). Es ist mindestens ein Fehler drinnen. multiview_pose.json und multiview_pose_summary.json zeigen unterschiedliche werte an. Ich hätte in der _summary die zusammengefassten werte erwartet. + +(2) Die Gewichtung der Arucos ist noch nicht perfekt. Ich will eigentlich den Abstand und die optische-Schärfe, Grösse der Arucos mit berücksichtigen. Grössere Arucos sollen mehr Gewicht erhalten. Schlecht ausgerichtete Arucos sollen weniger Gewicht erhalten. + +(3) Siehst du eine Möglichkeit, die Orientierung (normal und spin) der Arucos mit in die Gewichtung einfliessen zu lassnen? Hintergrund: auf einem gedrehten Bauteil sind die Arucos gedreht. Hier bin ich unsicher, ob das überhaupt möglich ist. + + +Egal was du machst: Lösche nicht einfach code, wenn du nicht genau weisst wofür er ist. Es ist einiges an Arbeit in diesen Code gegangen, behandel ihn entsprechend. + + +Der Roboter.json enthält noch nicht alle Marker. Die Finger werden später noch Marker erhlaten. Der Code soll in solchen Fällen confidence-level markieren, so dass die Werte als "sehr sehr ungewiss" erkannt werden können. + +Kannst du diese Aufgabe bearbeiten? Wenn möglich, erstelle eine neue Version der 2_Multiveiw.py. Wenn du noch fragen hast, stell bitte erst die Fragen. Du kannst hart sein, es muss kein freundlicher Text raus kommen, sondern es muss ein korrektes Programm rauskommen, das möglichst viele (andere) Test Bilder (sets von je 2-6 Fotos der selben Pose vom bekannten Roboter) erkennt. \ No newline at end of file