== 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.