diff --git a/markers.json b/markers.json new file mode 100644 index 0000000..bd05b96 --- /dev/null +++ b/markers.json @@ -0,0 +1,652 @@ +[ + { + "name": "Board_marker_210", + "id": 210, + "link": "Board", + "position_m": [ + 0.019999999552965164, + -0.019999999552965164, + 0.000800000037997961 + ], + "position_mm": [ + 19.999999552965164, + -19.999999552965164, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_211", + "id": 211, + "link": "Board", + "position_m": [ + 0.25, + -0.009999999776482582, + 0.000800000037997961 + ], + "position_mm": [ + 250.0, + -9.999999776482582, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_215", + "id": 215, + "link": "Board", + "position_m": [ + 0.25, + -0.09000000357627869, + 0.000800000037997961 + ], + "position_mm": [ + 250.0, + -90.00000357627869, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_214", + "id": 214, + "link": "Board", + "position_m": [ + 0.3499999940395355, + -0.009999999776482582, + 0.000800000037997961 + ], + "position_mm": [ + 349.9999940395355, + -9.999999776482582, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_208", + "id": 208, + "link": "Board", + "position_m": [ + 0.3499999940395355, + -0.09000000357627869, + 0.000800000037997961 + ], + "position_mm": [ + 349.9999940395355, + -90.00000357627869, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_206", + "id": 206, + "link": "Board", + "position_m": [ + 0.6499999761581421, + -0.009999999776482582, + 0.000800000037997961 + ], + "position_mm": [ + 649.9999761581421, + -9.999999776482582, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_205", + "id": 205, + "link": "Board", + "position_m": [ + 0.75, + -0.09000000357627869, + 0.000800000037997961 + ], + "position_mm": [ + 750.0, + -90.00000357627869, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_207", + "id": 207, + "link": "Board", + "position_m": [ + 0.75, + -0.009999999776482582, + 0.000800000037997961 + ], + "position_mm": [ + 750.0, + -9.999999776482582, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "Board_marker_217", + "id": 217, + "link": "Board", + "position_m": [ + 0.6499999761581421, + -0.09000000357627869, + 0.000800000037997961 + ], + "position_mm": [ + 649.9999761581421, + -90.00000357627869, + 0.800000037997961 + ], + "rotation_quaternion": [ + 1.0, + 0.0, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.0, + 1.0 + ] + }, + { + "name": "aruco_198", + "id": 198, + "link": "Arm1", + "position_m": [ + 0.11999999731779099, + -0.04913388192653656, + 0.10757456719875336 + ], + "position_mm": [ + 119.99999731779099, + -49.13388192653656, + 107.57456719875336 + ], + "rotation_quaternion": [ + 0.9993908405303955, + -0.03489953652024269, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.06975655257701874, + 0.9975640773773193 + ] + }, + { + "name": "aruco_229", + "id": 229, + "link": "Arm1", + "position_m": [ + 0.11999999731779099, + -0.13891464471817017, + 0.11385266482830048 + ], + "position_mm": [ + 119.99999731779099, + -138.91464471817017, + 113.85266482830048 + ], + "rotation_quaternion": [ + 0.9993908405303955, + -0.03489953652024269, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.06975655257701874, + 0.9975640773773193 + ] + }, + { + "name": "aruco_242", + "id": 242, + "link": "Arm1", + "position_m": [ + 0.11999999731779099, + -0.1438673585653305, + 0.04302562028169632 + ], + "position_mm": [ + 119.99999731779099, + -143.8673585653305, + 43.02562028169632 + ], + "rotation_quaternion": [ + 0.024677660316228867, + 0.7066760063171387, + 0.7066760659217834, + -0.024677699431777 + ], + "normal": [ + -5.21540641784668e-08, + -0.06975650042295456, + -0.9975640773773193 + ] + }, + { + "name": "aruco_243", + "id": 243, + "link": "Arm1", + "position_m": [ + 0.11999999731779099, + -0.17680451273918152, + 0.08091549575328827 + ], + "position_mm": [ + 119.99999731779099, + -176.80451273918152, + 80.91549575328827 + ], + "rotation_quaternion": [ + 0.7313537001609802, + 0.6819983124732971, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + -0.9975640773773193, + 0.06975657492876053 + ] + }, + { + "name": "aruco_244", + "id": 244, + "link": "Ellbow", + "position_m": [ + 0.24549999833106995, + -0.14139100909233093, + 0.0784391462802887 + ], + "position_mm": [ + 245.49999833106995, + -141.39100909233093, + 78.4391462802887 + ], + "rotation_quaternion": [ + 0.6916548013687134, + -0.1470157951116562, + 0.6916547417640686, + -0.1470157951116562 + ], + "normal": [ + 1.0, + 1.4901162970204496e-08, + 7.264316792543468e-08 + ] + }, + { + "name": "aruco_245", + "id": 245, + "link": "Ellbow", + "position_m": [ + 0.21000000834465027, + -0.15583015978336334, + 0.04600828140974045 + ], + "position_mm": [ + 210.00000834465027, + -155.83015978336334, + 46.00828140974045 + ], + "rotation_quaternion": [ + 0.147015780210495, + 0.6916548013687134, + 0.6916547417640686, + -0.1470157951116562 + ], + "normal": [ + -4.4703490686970326e-08, + -0.4067367613315582, + -0.9135454893112183 + ] + }, + { + "name": "aruco_246", + "id": 246, + "link": "Ellbow", + "position_m": [ + 0.21000000834465027, + -0.12695185840129852, + 0.11087001115083694 + ], + "position_mm": [ + 210.00000834465027, + -126.95185840129852, + 110.87001115083694 + ], + "rotation_quaternion": [ + 0.978147566318512, + -0.20791174471378326, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.4067367911338806, + 0.9135454893112183 + ] + }, + { + "name": "aruco_247", + "id": 247, + "link": "Ellbow", + "position_m": [ + 0.17249999940395355, + -0.12695185840129852, + 0.11087001115083694 + ], + "position_mm": [ + 172.49999940395355, + -126.95185840129852, + 110.87001115083694 + ], + "rotation_quaternion": [ + 0.978147566318512, + -0.20791174471378326, + 0.0, + 0.0 + ], + "normal": [ + 0.0, + 0.4067367911338806, + 0.9135454893112183 + ] + }, + { + "name": "Arm2_marker_120", + "id": 120, + "link": "Arm2", + "position_m": [ + 0.23908136785030365, + -0.25199049711227417, + 0.10539114475250244 + ], + "position_mm": [ + 239.08136785030365, + -251.99049711227417, + 105.39114475250244 + ], + "rotation_quaternion": [ + 0.4516582787036896, + -0.09600294381380081, + 0.8676275014877319, + -0.18441995978355408 + ], + "normal": [ + 0.8191521167755127, + -0.23329465091228485, + -0.5239881277084351 + ] + }, + { + "name": "aruco_122", + "id": 122, + "link": "Arm2", + "position_m": [ + 0.17503933608531952, + -0.24621543288230896, + 0.11836209893226624 + ], + "position_mm": [ + 175.03933608531952, + -246.21543288230896, + 118.36209893226624 + ], + "rotation_quaternion": [ + 0.6287412643432617, + -0.13364310562610626, + -0.749304473400116, + 0.15926963090896606 + ], + "normal": [ + -0.9848078489303589, + -0.07062903046607971, + -0.15863528847694397 + ] + }, + { + "name": "aruco_218", + "id": 218, + "link": "Arm2", + "position_m": [ + 0.24496068060398102, + -0.2412007749080658, + 0.12962521612644196 + ], + "position_mm": [ + 244.96068060398102, + -241.2007749080658, + 129.62521612644196 + ], + "rotation_quaternion": [ + 0.749304473400116, + -0.15926964581012726, + 0.6287412643432617, + -0.13364310562610626 + ], + "normal": [ + 0.9848078489303589, + 0.0706290453672409, + 0.15863528847694397 + ] + }, + { + "name": "aruco_113", + "id": 113, + "link": "Arm2", + "position_m": [ + 0.20470373332500458, + -0.2954392731189728, + 0.17990505695343018 + ], + "position_mm": [ + 204.70373332500458, + -295.4392731189728, + 179.90505695343018 + ], + "rotation_quaternion": [ + 0.9744254350662231, + -0.20712058246135712, + -0.08525115996599197, + 0.018120698630809784 + ], + "normal": [ + -0.1736481487751007, + 0.40055757761001587, + 0.8996666669845581 + ] + }, + { + "name": "aruco_101", + "id": 101, + "link": "Arm2", + "position_m": [ + 0.23908136785030365, + -0.31593865156173706, + 0.13386270403862 + ], + "position_mm": [ + 239.08136785030365, + -315.93865156173706, + 133.86270403862 + ], + "rotation_quaternion": [ + 0.4516582787036896, + -0.09600294381380081, + 0.8676275014877319, + -0.18441995978355408 + ], + "normal": [ + 0.8191521167755127, + -0.23329465091228485, + -0.5239881277084351 + ] + }, + { + "name": "aruco_102", + "id": 102, + "link": "Arm2", + "position_m": [ + 0.18963702023029327, + -0.31948474049568176, + 0.1258980929851532 + ], + "position_mm": [ + 189.63702023029327, + -319.48474049568176, + 125.8980929851532 + ], + "rotation_quaternion": [ + 0.2941346764564514, + -0.06252026557922363, + -0.93287593126297, + 0.1982889324426651 + ], + "normal": [ + -0.5735765099525452, + -0.33317920565605164, + -0.7483325600624084 + ] + }, + { + "name": "aruco_124", + "id": 124, + "link": "Arm2", + "position_m": [ + 0.17503933608531952, + -0.3439647853374481, + 0.1618829369544983 + ], + "position_mm": [ + 175.03933608531952, + -343.9647853374481, + 161.8829369544983 + ], + "rotation_quaternion": [ + 0.6287412643432617, + -0.13364310562610626, + -0.749304473400116, + 0.15926963090896606 + ], + "normal": [ + -0.9848078489303589, + -0.07062903046607971, + -0.15863528847694397 + ] + }, + { + "name": "aruco_219", + "id": 219, + "link": "Arm2", + "position_m": [ + 0.24496068060398102, + -0.33895012736320496, + 0.17314603924751282 + ], + "position_mm": [ + 244.96068060398102, + -338.95012736320496, + 173.14603924751282 + ], + "rotation_quaternion": [ + 0.749304473400116, + -0.15926964581012726, + 0.6287412643432617, + -0.13364310562610626 + ], + "normal": [ + 0.9848078489303589, + 0.0706290453672409, + 0.15863528847694397 + ] + } +] \ No newline at end of file diff --git a/pipeline/3_multiview_bundle_adjustment_v2.py b/pipeline/3_multiview_bundle_adjustment_v2.py new file mode 100644 index 0000000..615714d --- /dev/null +++ b/pipeline/3_multiview_bundle_adjustment_v2.py @@ -0,0 +1,1369 @@ +#!/usr/bin/env python3 +""" +3_multiview_bundle_adjustment_v2.py + +Multi-view ArUco marker position optimization with explicit, programmable +constraint families. + +Main ideas: +1) Triangulate initial marker positions from multi-view detections. +2) Compile constraints from robot.json structure using rule-based recognition: + - rigid link constraints (within a link) + - joint-axis projection constraints (between directly connected links) + - chain-axis projection constraints (ancestor/descendant propagation) +3) Optimize marker positions with bundle adjustment: + reprojection residuals + constraint residuals. + +This version is designed to be debuggable and switchable: +- each constraint family can be enabled/disabled +- robot.json may optionally include constraint_rules / constraint_overrides +- duplicate marker IDs can be checked strictly or only warned about + +Dependencies: + numpy, opencv-python, scipy (optional for optimization) + +Example: + python 3_multiview_bundle_adjustment_v2.py ^ + -det cam1_aruco_detection.json cam2_aruco_detection.json cam3_aruco_detection.json ^ + -pose cam1_camera_pose.json cam2_camera_pose.json cam3_camera_pose.json ^ + -robot robot.json ^ + -lambdaWeight 100.0 +""" + +from __future__ import annotations + +import argparse +import json +import os +import sys +import time +from dataclasses import dataclass +from itertools import combinations +from typing import Any, Dict, List, Optional, Tuple + +import cv2 +import numpy as np + + +# =================================================================== +# Path / JSON helpers +# =================================================================== + +def resolve_path(path: str) -> str: + path = os.path.expanduser(path) + if os.path.isabs(path): + return path + return os.path.abspath(path) + + +def load_json(path: str) -> Dict[str, Any]: + with open(resolve_path(path), "r", encoding="utf-8") as f: + return json.load(f) + + +def save_json(path: str, data: Dict[str, Any]) -> None: + with open(resolve_path(path), "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + + +# =================================================================== +# Units +# =================================================================== + +def get_length_scale(robot_data: Dict[str, Any]) -> float: + units = robot_data.get("units", {}) + length_unit = str(units.get("length", "")).strip().lower() + if length_unit in ("mm", "millimeter", "millimeters"): + return 1.0 / 1000.0 + if length_unit in ("cm", "centimeter", "centimeters"): + return 1.0 / 100.0 + return 1.0 + + +# =================================================================== +# Small geometry helpers +# =================================================================== + +def safe_norm(v: np.ndarray, eps: float = 1e-12) -> float: + return float(np.linalg.norm(v) + eps) + + +def normalize_vector(v: np.ndarray, eps: float = 1e-12) -> np.ndarray: + n = safe_norm(v, eps) + return np.asarray(v, dtype=np.float64) / n + + +def principal_axis_id(axis: np.ndarray, threshold: float = 0.95) -> Optional[int]: + """ + Return 0,1,2 for x,y,z if the axis is close enough to a principal axis. + Return None if it is not sufficiently aligned. + """ + a = normalize_vector(np.asarray(axis, dtype=np.float64)) + idx = int(np.argmax(np.abs(a))) + if abs(a[idx]) >= threshold: + return idx + return None + + +def axis_name(axis_id: int) -> str: + return ["x", "y", "z"][axis_id] + + +def camera_center_from_world_to_cam(R_wc: np.ndarray, t_wc: np.ndarray) -> np.ndarray: + """ + world_to_camera: X_cam = R_wc * X_world + t_wc + Camera center in world is -R^T t. + """ + return -R_wc.T @ t_wc + + +# =================================================================== +# Constraint rules / config +# =================================================================== + +@dataclass +class ConstraintRuleConfig: + # Rigid link constraints + rigid_distance_enabled: bool = True + rigid_distance_mode: str = "mst" # mst | star | full + rigid_distance_weight: float = 1.0 + + # Direct joint constraints (parent <-> child) + joint_axis_enabled: bool = True + joint_axis_max_pairs: int = 2 # number of anchor markers selected per side + joint_axis_weight: float = 0.5 + + # Propagated ancestor-descendant constraints + chain_axis_enabled: bool = True + chain_axis_max_depth: int = 3 + chain_axis_max_pairs: int = 2 + chain_axis_weight: float = 0.3 + + # Axis recognition threshold + axis_alignment_threshold: float = 0.95 + + # If True, link-marker duplicates in robot.json are fatal. + strict_unique_marker_ids: bool = False + + # Print/skipped diagnostics + show_skipped_constraints: bool = True + + +def _bool_or_default(value: Any, default: bool) -> bool: + if value is None: + return default + return bool(value) + + +def _float_or_default(value: Any, default: float) -> float: + if value is None: + return default + return float(value) + + +def _int_or_default(value: Any, default: int) -> int: + if value is None: + return default + return int(value) + + +def load_constraint_rule_config(robot_data: Dict[str, Any], args: argparse.Namespace) -> ConstraintRuleConfig: + """ + Merge built-in defaults, optional robot.json constraint_rules, and CLI flags. + """ + cfg = ConstraintRuleConfig() + + rules = robot_data.get("constraint_rules", {}) or {} + + rigid = rules.get("rigid_distance", {}) or {} + joint = rules.get("joint_axis_projection", {}) or {} + chain = rules.get("chain_axis_projection", {}) or {} + + cfg.rigid_distance_enabled = _bool_or_default(rigid.get("enabled"), cfg.rigid_distance_enabled) + cfg.rigid_distance_mode = str(rigid.get("mode", cfg.rigid_distance_mode)).strip().lower() + cfg.rigid_distance_weight = _float_or_default(rigid.get("weight"), cfg.rigid_distance_weight) + + cfg.joint_axis_enabled = _bool_or_default(joint.get("enabled"), cfg.joint_axis_enabled) + cfg.joint_axis_max_pairs = _int_or_default(joint.get("max_pairs"), cfg.joint_axis_max_pairs) + cfg.joint_axis_weight = _float_or_default(joint.get("weight"), cfg.joint_axis_weight) + + cfg.chain_axis_enabled = _bool_or_default(chain.get("enabled"), cfg.chain_axis_enabled) + cfg.chain_axis_max_depth = _int_or_default(chain.get("max_depth"), cfg.chain_axis_max_depth) + cfg.chain_axis_max_pairs = _int_or_default(chain.get("max_pairs"), cfg.chain_axis_max_pairs) + cfg.chain_axis_weight = _float_or_default(chain.get("weight"), cfg.chain_axis_weight) + + cfg.axis_alignment_threshold = _float_or_default( + rules.get("axis_alignment_threshold"), cfg.axis_alignment_threshold + ) + + if getattr(args, "strictUniqueMarkerIds", False): + cfg.strict_unique_marker_ids = True + + if getattr(args, "showSkippedConstraints", None) is not None: + cfg.show_skipped_constraints = bool(args.showSkippedConstraints) + + return cfg + + +# =================================================================== +# Constraint definitions +# =================================================================== + +@dataclass +class MarkerDistanceConstraint: + marker_id_a: int + marker_id_b: int + link_name: str + target_distance_m: float + weight: float = 1.0 + enabled: bool = True + source: str = "rigid_distance" + + +@dataclass +class JointAxisConstraint: + marker_id_parent: int + marker_id_child: int + parent_link: str + child_link: str + joint_axis: np.ndarray + target_delta_along_axis_m: float + weight: float = 1.0 + enabled: bool = True + source: str = "joint_axis_projection" + + +Constraint = MarkerDistanceConstraint | JointAxisConstraint + + +# =================================================================== +# Robot parsing +# =================================================================== + +def parse_robot_markers( + robot_data: Dict[str, Any], + length_scale: float, + strict_unique_marker_ids: bool = False +) -> Tuple[Dict[int, str], Dict[str, List[Dict[str, Any]]], List[str]]: + """ + Build: + marker_to_link: marker_id -> link_name + link_markers: link_name -> [{id, position_m, position_raw, ...}, ...] + """ + links = robot_data.get("links", {}) or {} + + marker_to_link: Dict[int, str] = {} + link_markers: Dict[str, List[Dict[str, Any]]] = {} + issues: List[str] = [] + + seen_global: Dict[int, str] = {} + + for link_name, link_data in links.items(): + markers = link_data.get("markers", []) or [] + collected: List[Dict[str, Any]] = [] + + seen_local: set[int] = set() + + for idx, marker in enumerate(markers): + marker_id = int(marker.get("id", -1)) + pos = marker.get("position", None) + + if marker_id < 0 or pos is None or len(pos) != 3: + issues.append(f"[WARN] link={link_name}: skipped invalid marker entry at index {idx}") + continue + + if marker_id in seen_local: + msg = f"[WARN] duplicate marker id {marker_id} inside link '{link_name}'" + if strict_unique_marker_ids: + raise ValueError(msg) + issues.append(msg + " -> skipped duplicate inside same link") + continue + + if marker_id in seen_global: + msg = ( + f"[WARN] duplicate marker id {marker_id} appears in link '{link_name}' " + f"and already in link '{seen_global[marker_id]}'" + ) + if strict_unique_marker_ids: + raise ValueError(msg) + issues.append(msg + " -> skipped duplicate occurrence") + continue + + seen_local.add(marker_id) + seen_global[marker_id] = link_name + + pos_raw = np.array(pos, dtype=np.float64) + pos_m = pos_raw * float(length_scale) + + collected.append( + { + "id": marker_id, + "name": marker.get("name", f"marker_{marker_id}"), + "position_raw": pos_raw, + "position_m": pos_m, + "normal": np.array(marker.get("normal", [0, 0, 1]), dtype=np.float64), + "size": marker.get("size", None), + "spin": marker.get("spin", None), + } + ) + + marker_to_link[marker_id] = link_name + + link_markers[link_name] = collected + + return marker_to_link, link_markers, issues + + +def get_link_parent_map(robot_data: Dict[str, Any]) -> Dict[str, Optional[str]]: + links = robot_data.get("links", {}) or {} + parent_map: Dict[str, Optional[str]] = {} + for link_name, link_data in links.items(): + parent_map[link_name] = link_data.get("parent", None) + return parent_map + + +def get_joint_info(robot_data: Dict[str, Any], child_link: str) -> Dict[str, Any]: + links = robot_data.get("links", {}) or {} + return (links.get(child_link, {}) or {}).get("jointToParent", {}) or {} + + +def get_joint_axis(robot_data: Dict[str, Any], child_link: str) -> Optional[np.ndarray]: + joint = get_joint_info(robot_data, child_link) + axis = joint.get("axis", None) + if axis is None: + return None + axis = np.asarray(axis, dtype=np.float64) + if safe_norm(axis) < 1e-12: + return None + return normalize_vector(axis) + + +# =================================================================== +# Constraint compilation helpers +# =================================================================== + +def get_enabled_link_rule( + robot_data: Dict[str, Any], + link_name: str, + rule_name: str, + default_enabled: bool = True +) -> bool: + """ + Optional per-link override structure: + + "constraint_overrides": { + "Arm2": { + "rigid_distance": {"enabled": true}, + "joint_axis_projection": {"enabled": false} + } + } + """ + overrides = robot_data.get("constraint_overrides", {}) or {} + link_override = overrides.get(link_name, {}) or {} + rule_override = link_override.get(rule_name, {}) or {} + if "enabled" in rule_override: + return bool(rule_override["enabled"]) + return default_enabled + + +def select_anchor_marker_ids( + markers: List[Dict[str, Any]], + axis: Optional[np.ndarray] = None, + max_count: int = 2 +) -> List[int]: + """ + Deterministic anchor selection: + - if axis is provided, choose markers with min/max projection on that axis + - otherwise choose marker(s) closest to centroid and farthest from centroid + + Returns up to max_count ids, deduplicated. + """ + if not markers: + return [] + + if len(markers) == 1: + return [int(markers[0]["id"])] + + ids = [int(m["id"]) for m in markers] + pos = np.stack([np.asarray(m["position_m"], dtype=np.float64) for m in markers], axis=0) + + selected: List[int] = [] + + if axis is not None and safe_norm(axis) > 1e-12: + a = normalize_vector(axis) + proj = pos @ a + min_idx = int(np.argmin(proj)) + max_idx = int(np.argmax(proj)) + selected = [ids[min_idx], ids[max_idx]] + else: + centroid = np.mean(pos, axis=0) + d = np.linalg.norm(pos - centroid, axis=1) + min_idx = int(np.argmin(d)) + max_idx = int(np.argmax(d)) + selected = [ids[min_idx], ids[max_idx]] + + # Fill if needed + if len(selected) < max_count: + for mid in ids: + if mid not in selected: + selected.append(mid) + if len(selected) >= max_count: + break + + # Deduplicate while preserving order + out: List[int] = [] + seen: set[int] = set() + for mid in selected: + if mid not in seen: + seen.add(mid) + out.append(mid) + if len(out) >= max_count: + break + + return out + + +def mst_edges_for_link(markers: List[Dict[str, Any]]) -> List[Tuple[int, int]]: + """ + Prim's algorithm on a complete graph of marker positions. + Returns n-1 edges connecting all markers with minimal total distance. + """ + n = len(markers) + if n < 2: + return [] + + ids = [int(m["id"]) for m in markers] + pos = np.stack([np.asarray(m["position_m"], dtype=np.float64) for m in markers], axis=0) + + in_tree = np.zeros(n, dtype=bool) + in_tree[0] = True + edges: List[Tuple[int, int]] = [] + + # pairwise distances + dist = np.linalg.norm(pos[:, None, :] - pos[None, :, :], axis=2) + + for _ in range(n - 1): + best = None + best_d = float("inf") + for i in range(n): + if not in_tree[i]: + continue + for j in range(n): + if in_tree[j]: + continue + d = float(dist[i, j]) + if d < best_d: + best_d = d + best = (i, j) + if best is None: + break + i, j = best + in_tree[j] = True + edges.append((ids[i], ids[j])) + + return edges + + +def compile_rigid_distance_constraints( + robot_data: Dict[str, Any], + link_markers: Dict[str, List[Dict[str, Any]]], + cfg: ConstraintRuleConfig +) -> List[MarkerDistanceConstraint]: + constraints: List[MarkerDistanceConstraint] = [] + links = robot_data.get("links", {}) or {} + + for link_name, markers in link_markers.items(): + if not get_enabled_link_rule(robot_data, link_name, "rigid_distance", cfg.rigid_distance_enabled): + continue + + if len(markers) < 2: + continue + + mode = cfg.rigid_distance_mode + + if mode == "full": + pairs = [(int(a["id"]), int(b["id"])) for a, b in combinations(markers, 2)] + elif mode == "star": + anchor_ids = select_anchor_marker_ids(markers, axis=None, max_count=1) + anchor_id = anchor_ids[0] + pairs = [] + for m in markers: + mid = int(m["id"]) + if mid != anchor_id: + pairs.append((anchor_id, mid)) + elif mode == "mst": + pairs = mst_edges_for_link(markers) + else: + raise ValueError(f"Unknown rigid_distance_mode='{mode}'. Use mst|star|full.") + + pos_map = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in markers} + unique_pairs: List[Tuple[int, int]] = [] + seen_pairs: set[Tuple[int, int]] = set() + + for mid_a, mid_b in pairs: + if mid_a == mid_b: + continue + key = tuple(sorted((int(mid_a), int(mid_b)))) + if key in seen_pairs: + continue + seen_pairs.add(key) + unique_pairs.append((int(mid_a), int(mid_b))) + + for mid_a, mid_b in unique_pairs: + pos_a = pos_map[mid_a] + pos_b = pos_map[mid_b] + target = float(np.linalg.norm(pos_b - pos_a)) + + constraints.append( + MarkerDistanceConstraint( + marker_id_a=mid_a, + marker_id_b=mid_b, + link_name=link_name, + target_distance_m=target, + weight=cfg.rigid_distance_weight, + enabled=True, + source=f"rigid_distance:{mode}", + ) + ) + + return constraints + + +def compile_direct_joint_axis_constraints( + robot_data: Dict[str, Any], + link_markers: Dict[str, List[Dict[str, Any]]], + cfg: ConstraintRuleConfig +) -> List[JointAxisConstraint]: + """ + Direct parent-child axis projection constraints. + We keep them intentionally sparse: + - choose up to 2 anchors per side (min/max along axis) + - create cross-product pairs between anchors + """ + constraints: List[JointAxisConstraint] = [] + links = robot_data.get("links", {}) or {} + + for child_link, child_data in links.items(): + parent_link = child_data.get("parent", None) + if not parent_link: + continue + + if not get_enabled_link_rule(robot_data, child_link, "joint_axis_projection", cfg.joint_axis_enabled): + continue + + joint_axis = get_joint_axis(robot_data, child_link) + if joint_axis is None: + continue + + axis_id = principal_axis_id(joint_axis, threshold=cfg.axis_alignment_threshold) + if axis_id is None: + # Still allow non-principal axes, but keep them with lower confidence: + axis_id = int(np.argmax(np.abs(joint_axis))) + axis_vec = np.zeros(3, dtype=np.float64) + axis_vec[axis_id] = float(np.sign(joint_axis[axis_id]) if abs(joint_axis[axis_id]) > 1e-12 else 1.0) + axis_vec = normalize_vector(axis_vec) + + parent_markers = link_markers.get(parent_link, []) + child_markers = link_markers.get(child_link, []) + if len(parent_markers) == 0 or len(child_markers) == 0: + continue + + max_pairs = max(1, int(cfg.joint_axis_max_pairs)) + parent_anchor_ids = select_anchor_marker_ids(parent_markers, axis=axis_vec, max_count=max_pairs) + child_anchor_ids = select_anchor_marker_ids(child_markers, axis=axis_vec, max_count=max_pairs) + + parent_pos = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in parent_markers} + child_pos = {int(m["id"]): np.asarray(m["position_m"], dtype=np.float64) for m in child_markers} + + seen: set[Tuple[int, int]] = set() + + for mid_p in parent_anchor_ids: + for mid_c in child_anchor_ids: + if mid_p == mid_c: + continue + key = (mid_p, mid_c) + if key in seen: + continue + seen.add(key) + + delta = child_pos[mid_c] - parent_pos[mid_p] + target = float(np.dot(delta, axis_vec)) + + constraints.append( + JointAxisConstraint( + marker_id_parent=mid_p, + marker_id_child=mid_c, + parent_link=parent_link, + child_link=child_link, + joint_axis=axis_vec, + target_delta_along_axis_m=target, + weight=cfg.joint_axis_weight, + enabled=True, + source="joint_axis_projection", + ) + ) + + return constraints + + +def ancestors_of(link_name: str, parent_map: Dict[str, Optional[str]], max_depth: int) -> List[str]: + out = [] + cur = parent_map.get(link_name, None) + depth = 0 + while cur is not None and depth < max_depth: + out.append(cur) + cur = parent_map.get(cur, None) + depth += 1 + return out + + +def path_axes_consistent( + robot_data: Dict[str, Any], + ancestor: str, + descendant: str, + parent_map: Dict[str, Optional[str]], + threshold: float +) -> Optional[np.ndarray]: + """ + Check whether the chain from ancestor to descendant is consistent with a single + principal axis. Returns the axis vector (unit, sign-preserving) if yes. + """ + # Build path descendant -> ancestor + chain: List[str] = [] + cur = descendant + while cur is not None and cur != ancestor: + chain.append(cur) + cur = parent_map.get(cur, None) + + if cur != ancestor: + return None # not an ancestor + + chain.reverse() # from ancestor child-step to descendant + + # Collect axes on each joint in the path + axes: List[np.ndarray] = [] + for link in chain: + ax = get_joint_axis(robot_data, link) + if ax is None: + return None + axes.append(ax) + + if not axes: + return None + + axis_ids: List[int] = [] + signs: List[float] = [] + for ax in axes: + aid = principal_axis_id(ax, threshold=threshold) + if aid is None: + return None + axis_ids.append(aid) + signs.append(float(np.sign(ax[aid])) if abs(ax[aid]) > 1e-12 else 1.0) + + if len(set(axis_ids)) != 1: + return None + + axis_id = axis_ids[0] + sign = 1.0 if np.sum(signs) >= 0 else -1.0 + axis_vec = np.zeros(3, dtype=np.float64) + axis_vec[axis_id] = sign + return normalize_vector(axis_vec) + + +def compile_chain_axis_constraints( + robot_data: Dict[str, Any], + link_markers: Dict[str, List[Dict[str, Any]]], + cfg: ConstraintRuleConfig +) -> List[JointAxisConstraint]: + """ + Ancestor-descendant projection constraints for chains that preserve a stable + principal axis over multiple joints. + + Example: + Board -> Base -> Arm1 -> Ellbow + with all joints x-aligned: + add Board<->Arm1, Board<->Ellbow, Base<->Ellbow, ... + """ + constraints: List[JointAxisConstraint] = [] + parent_map = get_link_parent_map(robot_data) + + links_with_markers = [ln for ln, mk in link_markers.items() if len(mk) > 0] + max_depth = max(1, int(cfg.chain_axis_max_depth)) + max_pairs = max(1, int(cfg.chain_axis_max_pairs)) + + # Cache positions for quick lookup + pos_cache: Dict[int, np.ndarray] = {} + for ln, mk in link_markers.items(): + for m in mk: + pos_cache[int(m["id"])] = np.asarray(m["position_m"], dtype=np.float64) + + for descendant in links_with_markers: + if not get_enabled_link_rule(robot_data, descendant, "chain_axis_projection", cfg.chain_axis_enabled): + continue + + for ancestor in ancestors_of(descendant, parent_map, max_depth=max_depth): + if ancestor == descendant: + continue + + axis_vec = path_axes_consistent( + robot_data=robot_data, + ancestor=ancestor, + descendant=descendant, + parent_map=parent_map, + threshold=cfg.axis_alignment_threshold, + ) + if axis_vec is None: + continue + + ancestor_markers = link_markers.get(ancestor, []) + descendant_markers = link_markers.get(descendant, []) + if len(ancestor_markers) == 0 or len(descendant_markers) == 0: + continue + + anc_anchors = select_anchor_marker_ids(ancestor_markers, axis=axis_vec, max_count=max_pairs) + des_anchors = select_anchor_marker_ids(descendant_markers, axis=axis_vec, max_count=max_pairs) + + seen: set[Tuple[int, int]] = set() + for mid_a in anc_anchors: + for mid_b in des_anchors: + if mid_a == mid_b: + continue + key = (mid_a, mid_b) + if key in seen: + continue + seen.add(key) + + target = float(np.dot(pos_cache[mid_b] - pos_cache[mid_a], axis_vec)) + + constraints.append( + JointAxisConstraint( + marker_id_parent=mid_a, + marker_id_child=mid_b, + parent_link=ancestor, + child_link=descendant, + joint_axis=axis_vec, + target_delta_along_axis_m=target, + weight=cfg.chain_axis_weight, + enabled=True, + source=f"chain_axis_projection:depth{len(ancestors_of(descendant, parent_map, max_depth))}", + ) + ) + + return constraints + + +def compile_constraints( + robot_data: Dict[str, Any], + length_scale: float, + cfg: ConstraintRuleConfig +) -> Tuple[Dict[int, str], Dict[str, List[Dict[str, Any]]], List[Constraint], List[str]]: + """ + Full rule-based constraint compiler. + Returns: + marker_to_link + link_markers + constraints + issues + """ + marker_to_link, link_markers, issues = parse_robot_markers( + robot_data, + length_scale=length_scale, + strict_unique_marker_ids=cfg.strict_unique_marker_ids, + ) + + constraints: List[Constraint] = [] + constraints.extend(compile_rigid_distance_constraints(robot_data, link_markers, cfg)) + constraints.extend(compile_direct_joint_axis_constraints(robot_data, link_markers, cfg)) + constraints.extend(compile_chain_axis_constraints(robot_data, link_markers, cfg)) + + # De-duplicate identical constraints + unique_constraints: List[Constraint] = [] + seen_keys: set[Tuple[Any, ...]] = set() + + for c in constraints: + if isinstance(c, MarkerDistanceConstraint): + key = ("d", min(c.marker_id_a, c.marker_id_b), max(c.marker_id_a, c.marker_id_b), c.link_name, round(c.target_distance_m, 9)) + else: + key = ( + "j", + c.parent_link, + c.child_link, + c.marker_id_parent, + c.marker_id_child, + tuple(np.round(c.joint_axis, 9).tolist()), + round(c.target_delta_along_axis_m, 9), + ) + if key in seen_keys: + continue + seen_keys.add(key) + unique_constraints.append(c) + + return marker_to_link, link_markers, unique_constraints, issues + + +# =================================================================== +# Constraint reporting +# =================================================================== + +def print_constraint_summary(constraints: List[Constraint]) -> None: + num_dist = sum(isinstance(c, MarkerDistanceConstraint) for c in constraints) + num_joint = sum(isinstance(c, JointAxisConstraint) for c in constraints) + print(f"[INFO] Constraint summary: total={len(constraints)} distance={num_dist} joint/chain={num_joint}") + + +def print_constraint_list(constraints: List[Constraint]) -> None: + print("\n[INFO] Constraint list:") + for idx, constraint in enumerate(constraints): + if isinstance(constraint, MarkerDistanceConstraint): + print( + f" [{idx:03d}] DISTANCE | " + f"Link='{constraint.link_name}' | " + f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | " + f"Target={constraint.target_distance_m:.6f} m | " + f"Weight={constraint.weight} | " + f"Source={constraint.source}" + ) + else: + axis_str = np.array2string(constraint.joint_axis, precision=3, suppress_small=True) + print( + f" [{idx:03d}] JOINT_AXIS | " + f"{constraint.parent_link}(M{constraint.marker_id_parent}) -> " + f"{constraint.child_link}(M{constraint.marker_id_child}) | " + f"Axis={axis_str} | " + f"TargetDelta={constraint.target_delta_along_axis_m:.6f} m | " + f"Weight={constraint.weight} | " + f"Source={constraint.source}" + ) + + +def print_constraints_with_errors( + title: str, + constraints: List[Constraint], + positions: Dict[int, np.ndarray], + show_skipped: bool = True +) -> None: + print(f"\n[INFO] {title}") + + active = 0 + skipped = 0 + + for idx, constraint in enumerate(constraints): + if isinstance(constraint, MarkerDistanceConstraint): + if constraint.marker_id_a not in positions or constraint.marker_id_b not in positions: + skipped += 1 + if show_skipped: + print( + f" [{idx:03d}] DISTANCE | " + f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | SKIPPED (missing marker)" + ) + continue + + pos_a = positions[constraint.marker_id_a] + pos_b = positions[constraint.marker_id_b] + actual = float(np.linalg.norm(pos_b - pos_a)) + error = actual - constraint.target_distance_m + active += 1 + + print( + f" [{idx:03d}] DISTANCE | " + f"Link='{constraint.link_name}' | " + f"M{constraint.marker_id_a} <-> M{constraint.marker_id_b} | " + f"target={constraint.target_distance_m*1000:.2f} mm | " + f"actual={actual*1000:.2f} mm | " + f"error={error*1000:+.2f} mm" + ) + + elif isinstance(constraint, JointAxisConstraint): + if constraint.marker_id_parent not in positions or constraint.marker_id_child not in positions: + skipped += 1 + if show_skipped: + print( + f" [{idx:03d}] JOINT_AXIS | " + f"M{constraint.marker_id_parent} -> M{constraint.marker_id_child} | SKIPPED (missing marker)" + ) + continue + + pos_parent = positions[constraint.marker_id_parent] + pos_child = positions[constraint.marker_id_child] + delta = pos_child - pos_parent + actual = float(np.dot(delta, constraint.joint_axis)) + error = actual - constraint.target_delta_along_axis_m + active += 1 + + axis_str = np.array2string(constraint.joint_axis, precision=2, suppress_small=True) + print( + f" [{idx:03d}] JOINT_AXIS | " + f"{constraint.parent_link}(M{constraint.marker_id_parent}) -> " + f"{constraint.child_link}(M{constraint.marker_id_child}) | " + f"axis={axis_str} | " + f"target={constraint.target_delta_along_axis_m*1000:.2f} mm | " + f"actual={actual*1000:.2f} mm | " + f"error={error*1000:+.2f} mm" + ) + + print(f"[INFO] Active constraints: {active} | Skipped: {skipped}") + + +# =================================================================== +# Multi-view loading +# =================================================================== + +def load_observations_and_poses( + detection_files: List[str], + pose_files: List[str] +) -> Tuple[Dict[int, List[Tuple[int, np.ndarray]]], List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], List[Dict[str, Any]]]: + """Load detections and camera poses.""" + if len(detection_files) != len(pose_files): + raise ValueError(f"Mismatch: {len(detection_files)} detections vs {len(pose_files)} poses") + + marker_observations: Dict[int, List[Tuple[int, np.ndarray]]] = {} + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = [] + obs_metadata: List[Dict[str, Any]] = [] + + for cam_idx, (det_file, pose_file) in enumerate(zip(detection_files, pose_files)): + det = load_json(det_file) + pose_data = load_json(pose_file) + + cam_section = det.get("camera", {}) or {} + K = np.array(cam_section.get("camera_matrix", []), dtype=np.float64).reshape(3, 3) + D = np.array(cam_section.get("distortion_coefficients", []), dtype=np.float64).reshape(-1, 1) + + pose_section = pose_data.get("camera_pose", {}) or {} + world_to_cam = pose_section.get("world_to_camera", {}) or {} + R_wc = np.array(world_to_cam.get("rotation_matrix", []), dtype=np.float64).reshape(3, 3) + t_wc = np.array(world_to_cam.get("translation_m", []), dtype=np.float64).reshape(3) + + cameras.append((K, D, R_wc, t_wc)) + + detections = det.get("detections", []) or [] + for det_obj in detections: + marker_id = int(det_obj.get("marker_id", -1)) + if marker_id < 0: + continue + + center_px = np.array(det_obj.get("center_px", []), dtype=np.float64) + if center_px.shape != (2,): + continue + + pts = center_px.reshape(1, 1, 2).astype(np.float32) + und = cv2.undistortPoints(pts, K.astype(np.float32), D.astype(np.float32), P=None) + norm_coords = und.reshape(2).astype(np.float64) + + marker_observations.setdefault(marker_id, []).append((cam_idx, norm_coords)) + + obs_metadata.append( + { + "detection_file": det_file, + "pose_file": pose_file, + "num_detections": len(detections), + } + ) + + return marker_observations, cameras, obs_metadata + + +# =================================================================== +# Initial triangulation +# =================================================================== + +def triangulate_marker_initial( + marker_id: int, + observations: List[Tuple[int, np.ndarray]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] +) -> Optional[np.ndarray]: + """ + Triangulate using the best baseline pair among all available observations. + This is more stable than using just the first two views. + """ + if len(observations) < 2: + return None + + best_pair = None + best_baseline = -1.0 + + for (cam_i, _), (cam_j, _) in combinations(observations, 2): + K1, D1, R1, t1 = cameras[cam_i] + K2, D2, R2, t2 = cameras[cam_j] + c1 = camera_center_from_world_to_cam(R1, t1) + c2 = camera_center_from_world_to_cam(R2, t2) + baseline = float(np.linalg.norm(c2 - c1)) + if baseline > best_baseline: + best_baseline = baseline + best_pair = (cam_i, cam_j) + + if best_pair is None: + return None + + cam_i, cam_j = best_pair + norm_coords_i = None + norm_coords_j = None + for ci, coords in observations: + if ci == cam_i: + norm_coords_i = coords + elif ci == cam_j: + norm_coords_j = coords + + if norm_coords_i is None or norm_coords_j is None: + return None + + K1, D1, R1, t1 = cameras[cam_i] + K2, D2, R2, t2 = cameras[cam_j] + + x1_px = K1[0, 0] * norm_coords_i[0] + K1[0, 2] + y1_px = K1[1, 1] * norm_coords_i[1] + K1[1, 2] + x2_px = K2[0, 0] * norm_coords_j[0] + K2[0, 2] + y2_px = K2[1, 1] * norm_coords_j[1] + K2[1, 2] + + P1 = K1 @ np.hstack([R1, t1.reshape(3, 1)]) + P2 = K2 @ np.hstack([R2, t2.reshape(3, 1)]) + + try: + X_h = cv2.triangulatePoints( + P1, + P2, + np.array([[x1_px], [y1_px]], dtype=np.float64), + np.array([[x2_px], [y2_px]], dtype=np.float64), + ) + X = (X_h[:3] / X_h[3]).reshape(3).astype(np.float64) + if not np.all(np.isfinite(X)): + return None + return X + except Exception: + return None + + +def initial_triangulation( + marker_observations: Dict[int, List[Tuple[int, np.ndarray]]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] +) -> Dict[int, np.ndarray]: + triangulated: Dict[int, np.ndarray] = {} + for marker_id, observations in marker_observations.items(): + X = triangulate_marker_initial(marker_id, observations, cameras) + if X is not None: + triangulated[marker_id] = X + return triangulated + + +# =================================================================== +# Residuals / optimization +# =================================================================== + +def bundle_adjustment_residuals( + marker_positions_flat: np.ndarray, + marker_ids: List[int], + marker_observations: Dict[int, List[Tuple[int, np.ndarray]]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], + constraints: List[Constraint], + lambda_constraint: float = 100.0 +) -> np.ndarray: + marker_dict: Dict[int, np.ndarray] = {} + for i, marker_id in enumerate(marker_ids): + marker_dict[marker_id] = marker_positions_flat[i * 3:(i + 1) * 3] + + residuals: List[float] = [] + + # Reprojection residuals in normalized coordinates + for marker_id, observations in marker_observations.items(): + if marker_id not in marker_dict: + continue + + X_world = marker_dict[marker_id] + for cam_idx, norm_coords_obs in observations: + K, D, R_wc, t_wc = cameras[cam_idx] + X_cam = R_wc @ X_world + t_wc + if X_cam[2] > 1e-6: + proj_norm = X_cam[:2] / X_cam[2] + r = proj_norm - norm_coords_obs + residuals.append(float(r[0])) + residuals.append(float(r[1])) + + # Constraint residuals + for constraint in constraints: + if isinstance(constraint, MarkerDistanceConstraint): + if constraint.marker_id_a in marker_dict and constraint.marker_id_b in marker_dict: + pos_a = marker_dict[constraint.marker_id_a] + pos_b = marker_dict[constraint.marker_id_b] + actual_dist = float(np.linalg.norm(pos_b - pos_a)) + residuals.append( + (actual_dist - constraint.target_distance_m) * constraint.weight * lambda_constraint + ) + + elif isinstance(constraint, JointAxisConstraint): + if constraint.marker_id_parent in marker_dict and constraint.marker_id_child in marker_dict: + pos_parent = marker_dict[constraint.marker_id_parent] + pos_child = marker_dict[constraint.marker_id_child] + delta = pos_child - pos_parent + actual_delta = float(np.dot(delta, constraint.joint_axis)) + residuals.append( + (actual_delta - constraint.target_delta_along_axis_m) * constraint.weight * lambda_constraint + ) + + return np.asarray(residuals, dtype=np.float64) + + +def optimize_with_constraints( + initial_positions: Dict[int, np.ndarray], + marker_observations: Dict[int, List[Tuple[int, np.ndarray]]], + cameras: List[Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]], + constraints: List[Constraint], + lambda_constraint: float = 100.0, + max_iterations: int = 50 +) -> Dict[int, np.ndarray]: + try: + from scipy.optimize import least_squares + except ImportError: + print("[WARN] scipy not available, skipping optimization.") + return initial_positions + + marker_ids = sorted(initial_positions.keys()) + if not marker_ids: + return {} + + x0 = np.concatenate([initial_positions[mid] for mid in marker_ids]) + + def residuals_fn(x: np.ndarray) -> np.ndarray: + return bundle_adjustment_residuals( + x, marker_ids, marker_observations, cameras, constraints, lambda_constraint + ) + + print(f"[INFO] Starting optimization with {len(x0)} variables and {len(constraints)} constraints...") + + result = least_squares( + residuals_fn, + x0, + max_nfev=max_iterations * max(1, len(marker_ids)), + verbose=1, + ) + + optimized = {} + for i, marker_id in enumerate(marker_ids): + optimized[marker_id] = result.x[i * 3:(i + 1) * 3] + + print(f"[INFO] Optimization complete. Final cost: {float(np.sum(result.fun ** 2)):.6f}") + return optimized + + +# =================================================================== +# Main +# =================================================================== + +def main() -> None: + parser = argparse.ArgumentParser( + description="Multi-view bundle adjustment with rule-based geometric constraints" + ) + parser.add_argument( + "-det", "--detections", + action="append", + required=True, + help="*_aruco_detection.json files" + ) + + parser.add_argument( + "-pose", "--poses", + action="append", + required=True, + help="*_camera_pose.json files" + ) + parser.add_argument( + "-robot", "--robot", + required=True, + help="robot.json" + ) + parser.add_argument( + "-outDir", "--outDir", + default=None, + help="Output directory" + ) + parser.add_argument( + "-lambdaWeight", "--lambdaWeight", + type=float, + default=100.0, + help="Constraint weight multiplier" + ) + parser.add_argument( + "--strictUniqueMarkerIds", + action="store_true", + help="Fail if a marker ID appears more than once in robot.json" + ) + parser.add_argument( + "--showSkippedConstraints", + action="store_true", + help="Print skipped constraints in the report" + ) + parser.add_argument( + "--noShowSkippedConstraints", + action="store_true", + help="Hide skipped constraints in the report" + ) + parser.add_argument( + "--saveConstraintReport", + action="store_true", + help="Save constraint report JSON files" + ) + + args = parser.parse_args() + + if args.showSkippedConstraints and args.noShowSkippedConstraints: + print("[ERROR] Choose only one of --showSkippedConstraints or --noShowSkippedConstraints") + sys.exit(1) + + if len(args.detections) != len(args.poses): + print(f"[ERROR] Mismatch: {len(args.detections)} detection files vs {len(args.poses)} pose files") + sys.exit(1) + + robot_data = load_json(args.robot) + length_scale = get_length_scale(robot_data) + cfg = load_constraint_rule_config(robot_data, args) + + if args.noShowSkippedConstraints: + cfg.show_skipped_constraints = False + elif args.showSkippedConstraints: + cfg.show_skipped_constraints = True + + print("[STEP 1] Compile constraints from robot.json structure...") + marker_to_link, link_markers, constraints, issues = compile_constraints(robot_data, length_scale, cfg) + + for issue in issues: + print(issue) + + print(f"[INFO] Links with markers: {sum(1 for v in link_markers.values() if len(v) > 0)}") + print(f"[INFO] Unique marker IDs: {len(marker_to_link)}") + print_constraint_summary(constraints) + print_constraint_list(constraints) + + print("\n[STEP 2] Load observations and camera poses...") + marker_observations, cameras, obs_metadata = load_observations_and_poses(args.detections, args.poses) + print(f"[INFO] {len(cameras)} cameras, {len(marker_observations)} observed markers") + print(f"[INFO] Detection files loaded: {len(obs_metadata)}") + + print("\n[STEP 3] Initial triangulation...") + initial_pos = initial_triangulation(marker_observations, cameras) + print(f"[INFO] Triangulated {len(initial_pos)} markers") + + out_dir = args.outDir or os.path.dirname(args.detections[0]) or "." + os.makedirs(resolve_path(out_dir), exist_ok=True) + + # Save initial solution + initial_output_markers = [] + for marker_id, position in sorted(initial_pos.items()): + initial_output_markers.append( + { + "marker_id": int(marker_id), + "position_m": [float(x) for x in position], + "position_mm": [float(x * 1000.0) for x in position], + "link": marker_to_link.get(marker_id, "unknown"), + } + ) + + initial_output = { + "schema_version": "1.1", + "stage": "initial_triangulation", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_cameras": len(cameras), + "num_markers": len(initial_pos), + "num_constraints": len(constraints), + }, + "markers": initial_output_markers, + } + + initial_out_file = os.path.join(out_dir, "aruco_positions_initial.json") + save_json(initial_out_file, initial_output) + print(f"[INFO] Initial triangulation saved to {initial_out_file}") + + print_constraints_with_errors( + "Constraint list BEFORE optimization", + constraints, + initial_pos, + show_skipped=cfg.show_skipped_constraints, + ) + + print("\n[STEP 4] Bundle adjustment with constraints...") + optimized_pos = optimize_with_constraints( + initial_pos, + marker_observations, + cameras, + constraints, + lambda_constraint=args.lambdaWeight, + ) + + print_constraints_with_errors( + "Constraint list AFTER optimization", + constraints, + optimized_pos, + show_skipped=cfg.show_skipped_constraints, + ) + + output_markers = [] + for marker_id, position in sorted(optimized_pos.items()): + output_markers.append( + { + "marker_id": int(marker_id), + "position_m": [float(x) for x in position], + "position_mm": [float(x * 1000.0) for x in position], + "link": marker_to_link.get(marker_id, "unknown"), + } + ) + + output = { + "schema_version": "1.1", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_cameras": len(cameras), + "num_markers": len(optimized_pos), + "num_constraints": len(constraints), + }, + "markers": output_markers, + } + + out_file = os.path.join(out_dir, "aruco_positions_optimized.json") + save_json(out_file, output) + print(f"\n[INFO] Saved to {out_file}") + + if args.saveConstraintReport: + report = { + "schema_version": "1.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "num_constraints": len(constraints), + "num_links_with_markers": sum(1 for v in link_markers.values() if len(v) > 0), + "num_observed_markers": len(marker_observations), + "num_triangulated_markers": len(initial_pos), + "num_optimized_markers": len(optimized_pos), + }, + "constraints": [], + } + for c in constraints: + if isinstance(c, MarkerDistanceConstraint): + report["constraints"].append( + { + "kind": "distance", + "link_name": c.link_name, + "marker_id_a": c.marker_id_a, + "marker_id_b": c.marker_id_b, + "target_distance_m": c.target_distance_m, + "weight": c.weight, + "source": c.source, + } + ) + else: + report["constraints"].append( + { + "kind": "joint_axis", + "parent_link": c.parent_link, + "child_link": c.child_link, + "marker_id_parent": c.marker_id_parent, + "marker_id_child": c.marker_id_child, + "joint_axis": [float(x) for x in c.joint_axis], + "target_delta_along_axis_m": c.target_delta_along_axis_m, + "weight": c.weight, + "source": c.source, + } + ) + report_file = os.path.join(out_dir, "constraint_report.json") + save_json(report_file, report) + print(f"[INFO] Constraint report saved to {report_file}") + + +if __name__ == "__main__": + main() diff --git a/pipeline/aruco_positions_initial.json b/pipeline/aruco_positions_initial.json index fad9d3a..aec87b2 100644 --- a/pipeline/aruco_positions_initial.json +++ b/pipeline/aruco_positions_initial.json @@ -1,121 +1,122 @@ { - "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:17Z", + "schema_version": "1.1", + "stage": "initial_triangulation", + "created_utc": "2026-05-29T11:11:53Z", "summary": { - "stage": "initial_triangulation", "num_cameras": 3, - "num_markers": 8 + "num_markers": 8, + "num_constraints": 31 }, "markers": [ { "marker_id": 122, "position_m": [ - 0.18520598113536835, - -0.21684008836746216, - 0.13996362686157227 + 0.14895405123790784, + -0.2643844341363741, + 0.16564713440852427 ], "position_mm": [ - 185.2059783935547, - -216.840087890625, - 139.963623046875 + 148.95405123790783, + -264.38443413637407, + 165.64713440852427 ], "link": "Arm2" }, { "marker_id": 198, "position_m": [ - 0.12119491398334503, - -0.046654392033815384, - 0.13240119814872742 + 0.11667067734641628, + -0.05481782829770797, + 0.13564627196635054 ], "position_mm": [ - 121.19491577148438, - -46.65439224243164, - 132.4011993408203 + 116.67067734641628, + -54.81782829770797, + 135.64627196635055 ], "link": "Arm1" }, { "marker_id": 210, "position_m": [ - 0.020141778513789177, - -0.01963355578482151, - 7.823568921594415e-06 + 0.020141782836696105, + -0.019633567005699154, + 7.847443788934222e-06 ], "position_mm": [ - 20.14177894592285, - -19.633556365966797, - 0.007823568768799305 + 20.141782836696105, + -19.633567005699152, + 0.007847443788934223 ], "link": "Board" }, { "marker_id": 211, "position_m": [ - 0.24960945546627045, - -0.009326362982392311, - -0.0005378762143664062 + 0.2496094175197415, + -0.009326316290609264, + -0.0005379225707190097 ], "position_mm": [ - 249.6094512939453, - -9.326362609863281, - -0.5378761887550354 + 249.6094175197415, + -9.326316290609263, + -0.5379225707190097 ], "link": "Board" }, { "marker_id": 214, "position_m": [ - 0.3498840034008026, - -0.009714338928461075, - -2.6824214728549123e-05 + 0.3498840022857963, + -0.009714324911855806, + -2.6845703214407957e-05 ], "position_mm": [ - 349.8840026855469, - -9.714339256286621, - -0.026824215427041054 + 349.8840022857963, + -9.714324911855806, + -0.026845703214407955 ], "link": "Board" }, { "marker_id": 215, "position_m": [ - 0.2497633397579193, - -0.0896778479218483, - -0.00011857203207910061 + 0.24976330898445626, + -0.08967780487999989, + -0.00011862459122980862 ], "position_mm": [ - 249.76333618164062, - -89.67784881591797, - -0.11857203394174576 + 249.76330898445624, + -89.6778048799999, + -0.11862459122980862 ], "link": "Board" }, { "marker_id": 229, "position_m": [ - 0.12001997977495193, - -0.13270698487758636, - 0.14000186324119568 + 0.11592480843458029, + -0.1392249195451953, + 0.14252312456392438 ], "position_mm": [ - 120.01998138427734, - -132.7069854736328, - 140.00186157226562 + 115.9248084345803, + -139.22491954519532, + 142.52312456392437 ], "link": "Arm1" }, { "marker_id": 243, "position_m": [ - 0.11871032416820526, - -0.17057909071445465, - 0.09964759647846222 + 0.11871032400722982, + -0.17057910506578056, + 0.0996476225849942 ], "position_mm": [ - 118.7103271484375, - -170.57908630371094, - 99.64759826660156 + 118.71032400722981, + -170.57910506578057, + 99.6476225849942 ], "link": "Arm1" } diff --git a/pipeline/aruco_positions_optimized.json b/pipeline/aruco_positions_optimized.json index f9941b9..db48fd3 100644 --- a/pipeline/aruco_positions_optimized.json +++ b/pipeline/aruco_positions_optimized.json @@ -1,121 +1,121 @@ { - "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:18Z", + "schema_version": "1.1", + "created_utc": "2026-05-29T11:11:54Z", "summary": { "num_cameras": 3, "num_markers": 8, - "num_constraints": 124 + "num_constraints": 31 }, "markers": [ { "marker_id": 122, "position_m": [ - 0.17089422820752076, - -0.25275415714542, - 0.1764152549525782 + 0.17089422820567965, + -0.25275415672150703, + 0.17641525453920115 ], "position_mm": [ - 170.89422820752077, - -252.75415714541998, - 176.4152549525782 + 170.89422820567964, + -252.75415672150703, + 176.41525453920116 ], "link": "Arm2" }, { "marker_id": 198, "position_m": [ - 0.11820089367579532, - -0.047715698531904785, - 0.13484286564203027 + 0.0782893264155328, + -0.04392881888831051, + 0.12370444588754521 ], "position_mm": [ - 118.20089367579533, - -47.715698531904785, - 134.84286564203026 + 78.2893264155328, + -43.92881888831051, + 123.70444588754522 ], "link": "Arm1" }, { "marker_id": 210, "position_m": [ - 0.019901746801827637, - -0.019634665689316648, - 9.617254460632054e-06 + 0.09810874978803365, + 0.04840357531258377, + -0.061619957949719154 ], "position_mm": [ - 19.901746801827638, - -19.63466568931665, - 0.009617254460632054 + 98.10874978803365, + 48.40357531258377, + -61.61995794971915 ], "link": "Board" }, { "marker_id": 211, "position_m": [ - 0.2499096083030422, - -0.00981801346837422, - 7.989879950350478e-05 + 0.26424764035228315, + -0.0563864907759364, + 0.05844872874629282 ], "position_mm": [ - 249.9096083030422, - -9.81801346837422, - 0.07989879950350479 + 264.24764035228316, + -56.3864907759364, + 58.448728746292815 ], "link": "Board" }, { "marker_id": 214, "position_m": [ - 0.3499095633806716, - -0.009897723015074432, - 0.00018350870717776096 + 0.34990989719464827, + -0.024224263154301, + 0.01810457272024633 ], "position_mm": [ - 349.90956338067156, - -9.897723015074432, - 0.18350870717776097 + 349.9098971946483, + -24.224263154301, + 18.10457272024633 ], "link": "Board" }, { "marker_id": 215, "position_m": [ - 0.24984585113464844, - -0.08981799168050893, - 8.39071085193236e-05 + 0.24658646632630482, + -0.07931906684450687, + -0.016131019201613698 ], "position_mm": [ - 249.84585113464843, - -89.81799168050892, - 0.08390710851932359 + 246.58646632630482, + -79.31906684450688, + -16.131019201613697 ], "link": "Board" }, { "marker_id": 229, "position_m": [ - 0.11749065839484758, - -0.137499932562534, - 0.14103531580163794 + 0.1138713279648291, + -0.12617149104964914, + 0.1320972625673932 ], "position_mm": [ - 117.49065839484759, - -137.499932562534, - 141.03531580163795 + 113.8713279648291, + -126.17149104964915, + 132.09726256739322 ], "link": "Arm1" }, { "marker_id": 243, "position_m": [ - 0.11810035461635551, - -0.17482765346898027, - 0.10853212349724542 + 0.1181663829394805, + -0.16401370137352922, + 0.1004795056960036 ], "position_mm": [ - 118.10035461635552, - -174.82765346898026, - 108.53212349724542 + 118.1663829394805, + -164.01370137352922, + 100.4795056960036 ], "link": "Arm1" } diff --git a/pipeline/check_robot_marker_uniqueness.py b/pipeline/check_robot_marker_uniqueness.py new file mode 100644 index 0000000..4b0e674 --- /dev/null +++ b/pipeline/check_robot_marker_uniqueness.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +""" +check_robot_marker_uniqueness.py + +Standalone checker for duplicate / invalid marker IDs in robot.json. + +Use this before running bundle adjustment: + python check_robot_marker_uniqueness.py -robot robot.json + +Exit codes: + 0 = no duplicates found + 1 = duplicates or structural problems found +""" + +from __future__ import annotations + +import argparse +import json +import os +import sys +from collections import defaultdict +from typing import Any, Dict, List, Tuple + + +def resolve_path(path: str) -> str: + path = os.path.expanduser(path) + if os.path.isabs(path): + return path + return os.path.abspath(path) + + +def load_json(path: str) -> Dict[str, Any]: + with open(resolve_path(path), "r", encoding="utf-8") as f: + return json.load(f) + + +def main() -> None: + parser = argparse.ArgumentParser(description="Check marker ID uniqueness in robot.json") + parser.add_argument("-robot", "--robot", required=True, help="Path to robot.json") + parser.add_argument( + "--strict", + action="store_true", + help="Return exit code 1 if any duplicate or invalid marker is found", + ) + args = parser.parse_args() + + robot = load_json(args.robot) + links = robot.get("links", {}) or {} + + id_locations: Dict[int, List[Tuple[str, int, str]]] = defaultdict(list) + invalid_entries: List[str] = [] + total_markers = 0 + + for link_name, link_data in links.items(): + markers = link_data.get("markers", []) or [] + for idx, marker in enumerate(markers): + total_markers += 1 + marker_id = marker.get("id", None) + if marker_id is None: + invalid_entries.append(f"link='{link_name}' marker_index={idx}: missing id") + continue + try: + marker_id_int = int(marker_id) + except Exception: + invalid_entries.append(f"link='{link_name}' marker_index={idx}: non-integer id={marker_id!r}") + continue + + name = str(marker.get("name", f"marker_{marker_id_int}")) + id_locations[marker_id_int].append((link_name, idx, name)) + + pos = marker.get("position", None) + if pos is None or not isinstance(pos, list) or len(pos) != 3: + invalid_entries.append( + f"link='{link_name}' marker_id={marker_id_int}: invalid/missing 3D position" + ) + + duplicates = {mid: locs for mid, locs in id_locations.items() if len(locs) > 1} + + print(f"[INFO] Total marker entries: {total_markers}") + print(f"[INFO] Unique marker IDs : {len(id_locations)}") + print(f"[INFO] Duplicate IDs : {len(duplicates)}") + print(f"[INFO] Invalid entries : {len(invalid_entries)}") + + if duplicates: + print("\n[DUPLICATES]") + for mid in sorted(duplicates.keys()): + print(f" marker_id={mid}") + for link_name, idx, name in duplicates[mid]: + print(f" - link='{link_name}' marker_index={idx} name='{name}'") + + if invalid_entries: + print("\n[INVALID ENTRIES]") + for item in invalid_entries: + print(f" - {item}") + + if duplicates or invalid_entries: + print("\n[WARN] robot.json is not clean for deterministic constraint generation.") + if args.strict: + sys.exit(1) + else: + print("\n[OK] No duplicate marker IDs found.") + + +if __name__ == "__main__": + main() diff --git a/pipeline/render_3a_aruco_detection.json b/pipeline/render_3a_aruco_detection.json index 582a1e7..5df18e2 100644 --- a/pipeline/render_3a_aruco_detection.json +++ b/pipeline/render_3a_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:16Z", + "created_utc": "2026-05-29T11:11:52Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "2ee8a896-adc4-44a7-8e21-00bf2d2bdb29", + "observation_id": "3fe0abea-8ee3-488d-afdd-ca173488bac8", "type": "aruco", "marker_id": 124, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.6862086405991146 }, { - "observation_id": "122847ec-48d6-477c-a833-61a1cbb5620b", + "observation_id": "4ad28655-628f-4970-aee7-ac0464a422dd", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.953171926139578 }, { - "observation_id": "4efd7b96-c897-44c5-9f63-6dfccdb0dcf0", + "observation_id": "39c9ae85-1343-4abd-a776-aead564c7ba2", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.5964639370258038 }, { - "observation_id": "c7a43ad5-e272-4673-af80-a17d11161ae4", + "observation_id": "2d4f27e1-f896-4714-bea0-d66f354f65bc", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.3578323322772018 }, { - "observation_id": "7d8958be-4ff2-4929-a78a-0e3239574df7", + "observation_id": "2b4ed846-8c7c-40a4-9d78-6dde96b7fb77", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.4396423002158715 }, { - "observation_id": "dea83763-ba79-4211-ace5-2fb4693ec614", + "observation_id": "5ce4384f-4a4e-4dfb-9e84-cfb04b1ee009", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.23718801109435655 }, { - "observation_id": "04e86e9d-49e6-4cc3-b104-8cc4bddae5e1", + "observation_id": "b0f54f4e-c981-4c68-91f4-71796b2e1b7e", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.27797680859006363 }, { - "observation_id": "d844a883-c800-4e9c-8f99-fdb379cf9d6a", + "observation_id": "2291dd23-8cfd-4fc8-8268-3f51a4f477e3", "type": "aruco", "marker_id": 205, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.19676029488014599 }, { - "observation_id": "e1194640-f6cb-4af1-8fe3-41d19817b79e", + "observation_id": "1aa4305f-42cd-4a6b-bbd9-3c4779159e9f", "type": "aruco", "marker_id": 206, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.23511362818048806 }, { - "observation_id": "1aa5ff5a-71ef-4362-aeb0-2f000b6c7f37", + "observation_id": "67d472aa-5a55-495c-84f3-9f9867b5e6bb", "type": "aruco", "marker_id": 207, "marker_size_m": 0.025, diff --git a/pipeline/render_3a_camera_pose.json b/pipeline/render_3a_camera_pose.json index 888cc23..5431454 100644 --- a/pipeline/render_3a_camera_pose.json +++ b/pipeline/render_3a_camera_pose.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:17Z", + "created_utc": "2026-05-29T11:11:53Z", "source": { "detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_3a_aruco_detection.json", "robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json" diff --git a/pipeline/render_3b_aruco_detection.json b/pipeline/render_3b_aruco_detection.json index c254e2a..80ce028 100644 --- a/pipeline/render_3b_aruco_detection.json +++ b/pipeline/render_3b_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:16Z", + "created_utc": "2026-05-29T11:11:51Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "37e350d1-d7f5-4006-9e7c-eabb597891bb", + "observation_id": "d0acb2d6-f00e-4abb-9182-0aba44eb37b5", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.8522257252911212 }, { - "observation_id": "fb1bfc74-794b-4065-b35e-f49b5e413a94", + "observation_id": "912b4c1f-fa4d-488b-ae55-6c159b08259f", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.2874514216623714 }, { - "observation_id": "6766568d-a40c-4c97-87fd-77e5243838fb", + "observation_id": "d713f49f-d43d-4181-b2c3-802fae7ac4c6", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.5547001883002887 }, { - "observation_id": "958853a3-10c7-486a-8985-8fa8eab7fb7a", + "observation_id": "b540ca35-2454-4485-b5c9-0edcffa7d655", "type": "aruco", "marker_id": 208, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.6072727689079809 }, { - "observation_id": "0ba8ce36-cbf9-42b2-8829-3c1b5adcf247", + "observation_id": "fd9ca8aa-3703-4a75-99dd-61e9d6da7096", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.6168610191628993 }, { - "observation_id": "3e48e695-6ede-4e3d-932c-161c06a03ac7", + "observation_id": "b9dfaf55-fe73-4bac-a011-9ff31f13c491", "type": "aruco", "marker_id": 214, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.5524643209674667 }, { - "observation_id": "386a4b8c-7efc-4cad-a7e0-c02ce8d15271", + "observation_id": "5e971f45-cf73-466b-ade5-bd09a71a0f8e", "type": "aruco", "marker_id": 211, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.5730650050844824 }, { - "observation_id": "cb63df83-5a11-4455-89ae-c741e2c649dd", + "observation_id": "48a160b7-02c3-47de-8eba-01d6c15d5d42", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.4163346774695628 }, { - "observation_id": "36b8474a-1769-40d3-86ad-98031727ad2e", + "observation_id": "3280ec54-b129-45ec-bffb-7eb3c61d3bdd", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, diff --git a/pipeline/render_3b_camera_pose.json b/pipeline/render_3b_camera_pose.json index 222cfb4..069669f 100644 --- a/pipeline/render_3b_camera_pose.json +++ b/pipeline/render_3b_camera_pose.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:17Z", + "created_utc": "2026-05-29T11:11:52Z", "source": { "detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_3b_aruco_detection.json", "robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json" diff --git a/pipeline/render_3c_aruco_detection.json b/pipeline/render_3c_aruco_detection.json index dadeb5e..5e57f07 100644 --- a/pipeline/render_3c_aruco_detection.json +++ b/pipeline/render_3c_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:15Z", + "created_utc": "2026-05-29T11:11:51Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "d51ce37d-1f1f-40ad-a6a2-a6c8f49b43ff", + "observation_id": "342af2b4-08bf-4f4c-9d28-1285278e24c1", "type": "aruco", "marker_id": 219, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.7735450417034769 }, { - "observation_id": "1806bda1-4af5-4faa-b0a3-0091bd35eabc", + "observation_id": "d02166a7-4c7b-4cd0-8210-124de5d533d7", "type": "aruco", "marker_id": 218, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.7950075578135153 }, { - "observation_id": "82259a28-11c6-4355-967d-d2c525cba254", + "observation_id": "e51ac6d6-f1ea-42af-b51d-903b881ddf01", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.8202207056111124 }, { - "observation_id": "97b9df19-3ff6-4315-8c74-470848b59a8e", + "observation_id": "805ea7a3-250b-4fb6-afd6-4641c83240d6", "type": "aruco", "marker_id": 214, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.1700115058329765 }, { - "observation_id": "6e25efa5-9cfb-4415-8be0-1c253eb68b9c", + "observation_id": "ccb99515-0a32-410b-b271-75f6e0e0c1c8", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.9372326698512834 }, { - "observation_id": "8411563f-277d-44a1-b331-70ea22f8f316", + "observation_id": "f689811a-b001-4d21-87a2-e01ad38c1a01", "type": "aruco", "marker_id": 244, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.7137086345973509 }, { - "observation_id": "20756782-3ad5-4dc0-9e2b-eea48226a4e7", + "observation_id": "cc2ffd45-0d44-4dad-83b2-46e2dec24bc0", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.8888336147757544 }, { - "observation_id": "022ae18d-f34f-420e-8117-ec22aa3f52e4", + "observation_id": "f5127d88-2b68-4a22-b216-fdf19b4d72a2", "type": "aruco", "marker_id": 211, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.8902520865668376 }, { - "observation_id": "22a306fc-a02a-46c3-b9e1-d240ddb40ee6", + "observation_id": "e418b70b-2b7e-4381-b86d-b7dc425de6f0", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.7343609168978227 }, { - "observation_id": "76dba603-04ec-4e63-bc5e-8ea448020025", + "observation_id": "d8ade844-05ae-46f2-b1c1-ab84179b062f", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -586,7 +586,7 @@ "confidence": 0.72560382306579 }, { - "observation_id": "173b2222-1e89-405e-8e94-0c43f4773db1", + "observation_id": "4386d50e-2ec2-4cbb-974a-d147e3498171", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, diff --git a/pipeline/render_3c_camera_pose.json b/pipeline/render_3c_camera_pose.json index bb58865..6771057 100644 --- a/pipeline/render_3c_camera_pose.json +++ b/pipeline/render_3c_camera_pose.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T07:55:16Z", + "created_utc": "2026-05-29T11:11:52Z", "source": { "detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_3c_aruco_detection.json", "robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json" diff --git a/pipeline/run_pipeline_multiview.bat b/pipeline/run_pipeline_multiview.bat index b515877..9964ba0 100644 --- a/pipeline/run_pipeline_multiview.bat +++ b/pipeline/run_pipeline_multiview.bat @@ -38,7 +38,7 @@ python 2_estimate_camera_from_observations.py -i render_3a_aruco_detection.json echo. echo [STEP 3] Triangulate marker positions from multi-view observations -python 3_multiview_bundle_adjustment.py ^ +python 3_multiview_bundle_adjustment_v2.py ^ -det render_3a_aruco_detection.json ^ -det render_3b_aruco_detection.json ^ -det render_3c_aruco_detection.json ^ diff --git a/render.png b/render.png new file mode 100644 index 0000000..4e2b5c1 Binary files /dev/null and b/render.png differ diff --git a/render_robot.py b/render_robot.py index dc89c00..e903547 100644 --- a/render_robot.py +++ b/render_robot.py @@ -29,6 +29,8 @@ DEFAULT_MATERIALS = { STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"] +marker_export = [] + # ============================================================ # JSON LOADING # ============================================================ @@ -691,6 +693,44 @@ for link_name, link_info in links_def.items(): else: plate_obj.data.materials[0] = pla_mat + # Weltmatrix holen (inkl. ALLER Transformationen!) + mw = marker_obj.matrix_world + + world_pos = mw.translation + world_rot = mw.to_quaternion() + + # Optional: Normal in Weltkoordinaten + world_normal = (world_rot @ mathutils.Vector((0, 0, 1))).normalized() + + marker_export.append({ + "name": marker_name, + "id": marker_id, + "link": link_name, + + "position_m": [world_pos.x, world_pos.y, world_pos.z], + + # oft nützlich für Computer Vision: + "position_mm": [ + world_pos.x / scale_factor, + world_pos.y / scale_factor, + world_pos.z / scale_factor + ], + + "rotation_quaternion": [ + world_rot.w, + world_rot.x, + world_rot.y, + world_rot.z + ], + + "normal": [ + world_normal.x, + world_normal.y, + world_normal.z + ] + }) + + # ============================================================ # DEBUG WORLD AXES @@ -753,4 +793,13 @@ create_axis_arrow("AxisZ", (0, 0, 1), (0, 0, 1)) # ============================================================ bpy.ops.render.render(write_still=True) -print("Finished rendering:", OUTPUT_FILE) \ No newline at end of file +print("Finished rendering:", OUTPUT_FILE) + + + +MARKER_OUTPUT = str(Path(OUTPUT_FILE).with_name("markers.json")) + +with open(MARKER_OUTPUT, "w", encoding="utf-8") as f: + json.dump(marker_export, f, indent=2) + +print("Saved marker positions:", MARKER_OUTPUT) diff --git a/robot.json b/robot.json index d7f69ab..754b618 100644 --- a/robot.json +++ b/robot.json @@ -362,12 +362,12 @@ ], "markers":[ - {"id":124, "position":[24.75, -112, -24.75],"normal":[1,0,-1]}, + {"id":120, "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":113, "name": "aruco_113", "position":[0, -182, 30],"normal":[0,0,1]}, + {"id":101, "name": "aruco_101", "position":[ 24.75, -182, -24.75],"normal":[ 1,0,-1]}, + {"id":102, "name": "aruco_102", "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]}