From 1008c9f7fd533855fe1abbde321e884426baba60 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 29 May 2026 12:55:24 +0200 Subject: [PATCH] GPT runde --- .../{ => 2_Multiview_Trial}/2_Multiview.py | 0 .../2_estimate_camera_pose_from_aruco_json.py | 691 ------------------ pipeline/3_multiview_bundle_adjustment.py | 674 +++++++++++++++++ pipeline/aruco_positions_initial.json | 123 ++++ pipeline/aruco_positions_optimized.json | 123 ++++ pipeline/render_3a_aruco_detection.json | 24 +- pipeline/render_3a_camera_pose.json | 8 +- pipeline/render_3b_aruco_detection.json | 22 +- pipeline/render_3b_camera_pose.json | 8 +- pipeline/render_3c_aruco_detection.json | 24 +- pipeline/render_3c_camera_pose.json | 6 +- pipeline/run_pipeline_multiview.bat | 51 ++ 12 files changed, 1017 insertions(+), 737 deletions(-) rename pipeline/{ => 2_Multiview_Trial}/2_Multiview.py (100%) delete mode 100644 pipeline/2_estimate_camera_pose_from_aruco_json.py create mode 100644 pipeline/3_multiview_bundle_adjustment.py create mode 100644 pipeline/aruco_positions_initial.json create mode 100644 pipeline/aruco_positions_optimized.json create mode 100644 pipeline/run_pipeline_multiview.bat diff --git a/pipeline/2_Multiview.py b/pipeline/2_Multiview_Trial/2_Multiview.py similarity index 100% rename from pipeline/2_Multiview.py rename to pipeline/2_Multiview_Trial/2_Multiview.py diff --git a/pipeline/2_estimate_camera_pose_from_aruco_json.py b/pipeline/2_estimate_camera_pose_from_aruco_json.py deleted file mode 100644 index 9cc3b50..0000000 --- a/pipeline/2_estimate_camera_pose_from_aruco_json.py +++ /dev/null @@ -1,691 +0,0 @@ -#!/usr/bin/env python3 -""" -2_estimate_camera_pose_from_aruco_json.py - -Berechnet die Kameraposition im Maschinen-/Board-Koordinatensystem -aus: - 1. einer ArUco-Detections-JSON - 2. robots.json mit bekannten Marker-Positionen - -NEU: -- Marker-Orientierungen unterstützt -- Default: Board-Marker zeigen nach +Z -- Qualitätsbewertung erweitert -- Speichert ALLE erkannten Marker -- Speichert auch fehlgeschlagene Lösungen -- Bewertet Kamerageometrie -- Bewertet Markerabdeckung -- Bewertet Sichtwinkel -- Bewertet Markeranzahl -- Speichert vollständige Rohdaten - -Benötigt: - pip install opencv-python numpy -""" - -import argparse -import json -import math -from pathlib import Path - -import cv2 -import numpy as np - - -# ============================================================ -# Hilfsfunktionen -# ============================================================ - -def normalize(v): - n = np.linalg.norm(v) - - if n < 1e-9: - return v - - return v / n - - -def rotation_matrix_from_axes(x_axis, y_axis, z_axis): - - R = np.column_stack([ - normalize(x_axis), - normalize(y_axis), - normalize(z_axis) - ]) - - return R.astype(np.float32) - - -def rvec_tvec_to_camera_pose(rvec, tvec): - """ - OpenCV: - X_cam = R * X_world + t - - Kamera im Weltkoordinatensystem: - C = -R^T * t - """ - - R_cw, _ = cv2.Rodrigues(rvec) - - R_wc = R_cw.T - - cam_pos = -R_wc @ tvec - - return R_wc, cam_pos.reshape(3) - - -def rotation_matrix_to_euler_zyx(R): - - yaw = math.degrees(math.atan2(R[1, 0], R[0, 0])) - - sp = math.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2) - - pitch = math.degrees(math.atan2(-R[2, 0], sp)) - - roll = math.degrees(math.atan2(R[2, 1], R[2, 2])) - - return roll, pitch, yaw - - -# ============================================================ -# Marker-Orientierung -# ============================================================ - -def get_marker_rotation(marker): - - # Explizite Rotation vorhanden? - if "rotation_matrix" in marker: - return np.array( - marker["rotation_matrix"], - dtype=np.float32 - ) - - # Default: - # Marker zeigt nach +Z - # - # x = rechts - # y = oben - # z = aus Board heraus (+Z) - - x_axis = np.array([1, 0, 0], dtype=np.float32) - y_axis = np.array([0, 1, 0], dtype=np.float32) - z_axis = np.array([0, 0, 1], dtype=np.float32) - - return rotation_matrix_from_axes( - x_axis, - y_axis, - z_axis - ) - - -# ============================================================ -# Marker Lookup -# ============================================================ - -def build_marker_lookup(robot_data): - - marker_lookup = {} - - length_units = str(robot_data.get("units", {}).get("length", "")).strip().lower() - length_scale = 1.0 - if length_units in ("mm", "millimeter", "millimeters"): - length_scale = 1.0 / 1000.0 - elif length_units in ("cm", "centimeter", "centimeters"): - length_scale = 1.0 / 100.0 - - markers = [] - - # Neues Format: links -> Board -> markers - links = robot_data.get("links", {}) - - - board = links.get("Board") - - if board and "markers" in board: - markers = board["markers"] - - - # Fallback für altes Format - if not markers: - markers = robot_data.get("Marker", []) - - for marker in markers: - - - marker_id = int(marker.get("id", -1)) - - if marker_id < 0: - continue - - # Nur absolute Marker verwenden - if "position" not in marker: - continue - - pos = marker["position"] - - if pos is None: - continue - - if len(pos) != 3: - continue - - rotation = get_marker_rotation(marker) - - marker_lookup[marker_id] = { - "position": np.array( - pos, - dtype=np.float32 - ) * np.float32(length_scale), - "rotation": rotation, - "on": marker.get("on", "unknown") - } - - return marker_lookup - - -# ============================================================ -# Marker-Eckpunkte -# ============================================================ - -def build_marker_object_points( - marker_center_world, - marker_rotation, - marker_size_m): - - half = marker_size_m / 2.0 - - # OpenCV Corner Reihenfolge - local = np.array([ - [-half, half, 0.0], - [ half, half, 0.0], - [ half, -half, 0.0], - [-half, -half, 0.0], - ], dtype=np.float32) - - rotated = (marker_rotation @ local.T).T - - return rotated + marker_center_world.reshape(1, 3) - - -# ============================================================ -# Qualitätsmetriken -# ============================================================ - -def compute_marker_spread(points_3d): - - if len(points_3d) < 2: - return 0.0 - - mins = np.min(points_3d, axis=0) - maxs = np.max(points_3d, axis=0) - - diag = np.linalg.norm(maxs - mins) - - return float(diag) - - -def compute_viewing_angles( - camera_position, - marker_lookup, - used_markers): - - results = [] - - for marker_id in used_markers: - - marker = marker_lookup[marker_id] - - pos = marker["position"] - - R = marker["rotation"] - - normal = R[:, 2] - - to_camera = normalize(camera_position - pos) - - dot = np.clip( - np.dot(normal, to_camera), - -1.0, - 1.0 - ) - - angle = math.degrees(math.acos(dot)) - - results.append(angle) - - if len(results) == 0: - return { - "mean": None, - "max": None - } - - return { - "mean": float(np.mean(results)), - "max": float(np.max(results)) - } - - -def compute_pose_quality( - rms, - max_err, - num_markers, - marker_spread, - viewing_angle_mean): - - score = 100.0 - - # Reprojection Error - score -= rms * 8.0 - - # Max Error - score -= max_err * 2.0 - - # Wenige Marker - if num_markers < 2: - score -= 50 - - elif num_markers < 4: - score -= 25 - - elif num_markers < 6: - score -= 10 - - # Schlechte räumliche Verteilung - if marker_spread < 0.10: - score -= 30 - - elif marker_spread < 0.25: - score -= 15 - - # Schlechter Blickwinkel - if viewing_angle_mean is not None: - - if viewing_angle_mean > 70: - score -= 25 - - elif viewing_angle_mean > 50: - score -= 10 - - score = max(0.0, min(100.0, score)) - - return float(score) - - -# ============================================================ -# Main -# ============================================================ - -def main(): - - parser = argparse.ArgumentParser() - - parser.add_argument( - "--detections", - default=None - ) - - parser.add_argument( - "--robots", - required=True - ) - - parser.add_argument( - "--min-confidence", - type=float, - default=0.5 - ) - - parser.add_argument( - "--max-reprojection-error", - type=float, - default=3.0 - ) - - parser.add_argument( - "--outDir", - default=None - ) - - - args = parser.parse_args() - - # ============================================================ - # JSON laden - # ============================================================ - - with open(args.detections, "r", encoding="utf-8") as f: - detection_data = json.load(f) - - with open(args.robots, "r", encoding="utf-8") as f: - robot_data = json.load(f) - - # ============================================================ - # Kamera - # ============================================================ - - K = np.array( - detection_data["camera"]["camera_matrix"], - dtype=np.float32 - ) - - D = np.array( - detection_data["camera"]["distortion_coefficients"], - dtype=np.float32 - ).reshape(-1, 1) - - # ============================================================ - # Marker laden - # ============================================================ - - known_markers = build_marker_lookup(robot_data) - - # ============================================================ - # Punktlisten - # ============================================================ - - object_points = [] - image_points = [] - - used_markers = [] - rejected_markers = [] - - detections = detection_data["detections"] - - for det in detections: - - marker_id = int(det["marker_id"]) - - confidence = float( - det.get("confidence", 1.0) - ) - - reason = None - - if confidence < args.min_confidence: - reason = "low_confidence" - - elif marker_id not in known_markers: - reason = "unknown_marker" - - if reason is not None: - - rejected_markers.append({ - "marker_id": marker_id, - "reason": reason, - "confidence": confidence - }) - - continue - - marker_info = known_markers[marker_id] - - marker_center_world = marker_info["position"] - - marker_rotation = marker_info["rotation"] - - marker_size = float( - det["marker_size_m"] - ) - - obj_pts = build_marker_object_points( - marker_center_world, - marker_rotation, - marker_size - ) - - img_pts = np.array( - det["image_points_px"], - dtype=np.float32 - ) - - object_points.append(obj_pts) - image_points.append(img_pts) - - used_markers.append(marker_id) - - # ============================================================ - # Ausgabe vorbereiten - # ============================================================ - - out = { - "success": False, - "camera_pose": None, - "quality": {}, - "used_markers": [], - "rejected_markers": rejected_markers, - "all_detected_markers": [ - int(d["marker_id"]) - for d in detections - ] - } - - # ============================================================ - # Zu wenige Marker - # ============================================================ - - if len(object_points) == 0: - - out["quality"] = { - "error": "no_known_markers", - "num_detected_markers": len(detections), - "num_used_markers": 0 - } - - - if args.outDir: - out_dir = Path(args.outDir) - out_dir.mkdir(parents=True, exist_ok=True) - - out_file = out_dir / ( - Path(args.detections).stem + ".camera_pose.json" - ) - else: - out_file = Path(args.detections).with_suffix( - ".camera_pose.json" - ) - - - with open(out_file, "w", encoding="utf-8") as f: - json.dump(out, f, indent=2) - - print("Keine bekannten Marker gefunden.") - print(out_file) - - return - - # ============================================================ - # solvePnP - # ============================================================ - - object_points = np.concatenate( - object_points, - axis=0 - ) - - image_points = np.concatenate( - image_points, - axis=0 - ) - - success, rvec, tvec = cv2.solvePnP( - object_points, - image_points, - K, - D, - flags=cv2.SOLVEPNP_ITERATIVE - ) - - if not success: - - out["quality"] = { - "error": "solvepnp_failed", - "num_used_markers": len(set(used_markers)) - } - - out_file = Path(args.detections).with_suffix( - ".camera_pose.json" - ) - - with open(out_file, "w", encoding="utf-8") as f: - json.dump(out, f, indent=2) - - print("solvePnP fehlgeschlagen") - return - - # ============================================================ - # Kamera Pose - # ============================================================ - - R_wc, cam_pos = rvec_tvec_to_camera_pose( - rvec, - tvec - ) - - roll, pitch, yaw = rotation_matrix_to_euler_zyx( - R_wc - ) - - # ============================================================ - # Reprojektion - # ============================================================ - - projected, _ = cv2.projectPoints( - object_points, - rvec, - tvec, - K, - D - ) - - projected = projected.reshape(-1, 2) - - reproj_error = np.linalg.norm( - projected - image_points, - axis=1 - ) - - rms = float( - np.sqrt(np.mean(reproj_error ** 2)) - ) - - max_err = float( - np.max(reproj_error) - ) - - # ============================================================ - # Qualität - # ============================================================ - - marker_positions = np.array([ - known_markers[mid]["position"] - for mid in set(used_markers) - ]) - - marker_spread = compute_marker_spread( - marker_positions - ) - - viewing = compute_viewing_angles( - cam_pos, - known_markers, - set(used_markers) - ) - - quality_score = compute_pose_quality( - rms, - max_err, - len(set(used_markers)), - marker_spread, - viewing["mean"] - ) - - # ============================================================ - # Ausgabe - # ============================================================ - - print() - print("==================================================") - print("KAMERA-POSE") - print("==================================================") - - print() - print("Position [m]") - print(f"X = {cam_pos[0]:.6f}") - print(f"Y = {cam_pos[1]:.6f}") - print(f"Z = {cam_pos[2]:.6f}") - - print() - print("Orientation [deg]") - print(f"Roll = {roll:.3f}") - print(f"Pitch = {pitch:.3f}") - print(f"Yaw = {yaw:.3f}") - - print() - print("Reprojection") - print(f"RMS = {rms:.3f}px") - print(f"MAX = {max_err:.3f}px") - - print() - print("Quality") - print(f"Score = {quality_score:.1f}/100") - print(f"Marker Spread = {marker_spread:.3f}m") - - if viewing["mean"] is not None: - print( - f"Mean Viewing Angle = " - f"{viewing['mean']:.1f}deg" - ) - - # ============================================================ - # JSON speichern - # ============================================================ - - out["success"] = True - - out["camera_pose"] = { - "position_m": { - "x": float(cam_pos[0]), - "y": float(cam_pos[1]), - "z": float(cam_pos[2]), - }, - "orientation_deg": { - "roll": float(roll), - "pitch": float(pitch), - "yaw": float(yaw), - }, - "rotation_matrix_world_from_camera": ( - R_wc.tolist() - ) - } - - out["quality"] = { - "pose_quality_score": quality_score, - "reprojection_rms_px": rms, - "reprojection_max_px": max_err, - "num_detected_markers": len(detections), - "num_used_markers": len(set(used_markers)), - "marker_spread_m": marker_spread, - "mean_viewing_angle_deg": viewing["mean"], - "max_viewing_angle_deg": viewing["max"], - "pose_stable": - max_err <= args.max_reprojection_error - } - - out["used_markers"] = sorted( - list(set(used_markers)) - ) - - out_file = Path(args.detections).with_suffix( - ".camera_pose.json" - ) - - with open(out_file, "w", encoding="utf-8") as f: - json.dump(out, f, indent=2) - - print() - print("Gespeichert:") - print(out_file) - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/pipeline/3_multiview_bundle_adjustment.py b/pipeline/3_multiview_bundle_adjustment.py new file mode 100644 index 0000000..705f90f --- /dev/null +++ b/pipeline/3_multiview_bundle_adjustment.py @@ -0,0 +1,674 @@ +#!/usr/bin/env python3 +""" +3_multiview_bundle_adjustment.py + +Multi-view ArUco marker position optimization with geometric constraints. + +Strategy: +1. Triangulate markers from multi-view observations (rough initial estimate) +2. Optimize marker positions with constraints: + - Same-link distance constraints: markers on same link keep fixed relative distance + - Joint-axis constraints: markers on linked arms via joint keep distance along joint axis +3. Use bundle adjustment: minimize reprojection error + constraint violations + +Key insight: We do NOT use robot.json marker positions (they're in different joint config). +We only use robot.json hierarchy to DEFINE constraints. + +Dependencies: numpy, opencv-python, scipy +""" + +from __future__ import annotations + +import argparse +import json +import os +import sys +import time +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 + elif length_unit in ("cm", "centimeter", "centimeters"): + return 1.0 / 100.0 + return 1.0 + + +# =================================================================== +# Constraint definitions (from robot.json structure ONLY) +# =================================================================== + +class MarkerDistanceConstraint: + """ + Two markers on the same link must maintain a fixed distance. + We compute this distance from their relative positions in robot.json, + but scaled to meters. The ACTUAL 3D positions will be optimized, + but their distance must stay constant. + """ + def __init__(self, marker_id_a: int, marker_id_b: int, + link_name: str, target_distance: float, weight: float = 1.0): + self.marker_id_a = marker_id_a + self.marker_id_b = marker_id_b + self.link_name = link_name + self.target_distance = target_distance + self.weight = weight + + +class JointAxisConstraint: + """ + Two markers on linked arms (connected by joint around axis). + If joint axis is [1,0,0], then delta_x component must stay constant. + """ + def __init__(self, marker_id_parent: int, marker_id_child: int, + parent_link: str, child_link: str, + joint_axis: np.ndarray, # unit vector [x,y,z] + target_delta_along_axis: float, + weight: float = 1.0): + self.marker_id_parent = marker_id_parent + self.marker_id_child = marker_id_child + self.parent_link = parent_link + self.child_link = child_link + self.joint_axis = joint_axis / (np.linalg.norm(joint_axis) + 1e-9) + self.target_delta_along_axis = target_delta_along_axis + self.weight = weight + + +def extract_constraints_from_hierarchy(robot_data: Dict[str, Any], + length_scale: float) -> Tuple[Dict, List]: + """ + Extract constraint definitions from robot.json. + + NOTE: We only extract CONSTRAINT TYPES, not actual marker positions. + Marker positions will come from triangulation. + + Returns: + marker_to_link: {marker_id -> link_name} + constraints: List of constraint objects + """ + constraints = [] + marker_to_link = {} + + links = robot_data.get("links", {}) + + # Build map: link_name -> markers on that link + link_markers = {} + for link_name, link_data in links.items(): + markers = link_data.get("markers", []) + marker_ids = [] + marker_positions = {} + + for marker in markers: + marker_id = int(marker.get("id", -1)) + pos = marker.get("position", None) + + if marker_id >= 0 and pos is not None and len(pos) == 3: + marker_ids.append(marker_id) + marker_positions[marker_id] = np.array(pos, dtype=np.float32) * np.float32(length_scale) + marker_to_link[marker_id] = link_name + + link_markers[link_name] = (marker_ids, marker_positions) + + # Extract same-link distance constraints + for link_name, (marker_ids, positions) in link_markers.items(): + if len(marker_ids) >= 2: + # For each pair, compute their distance + for i in range(len(marker_ids)): + for j in range(i + 1, len(marker_ids)): + mid_a = marker_ids[i] + mid_b = marker_ids[j] + pos_a = positions[mid_a] + pos_b = positions[mid_b] + target_dist = float(np.linalg.norm(pos_b - pos_a)) + + constraint = MarkerDistanceConstraint( + mid_a, mid_b, link_name, target_dist, weight=1.0 + ) + constraints.append(constraint) + + # Extract joint-axis constraints (if we have markers on parent and child links) + for link_name, link_data in links.items(): + parent_link = link_data.get("parent", None) + + if parent_link is None: + continue + + joint_info = link_data.get("jointToParent", {}) + if not joint_info: + continue + + joint_axis = joint_info.get("axis", [1, 0, 0]) + joint_axis = np.array(joint_axis, dtype=np.float32) + + # Get markers on this link and parent link + child_marker_ids, child_positions = link_markers.get(link_name, ([], {})) + parent_marker_ids, parent_positions = link_markers.get(parent_link, ([], {})) + + if len(child_marker_ids) > 0 and len(parent_marker_ids) > 0: + # Create constraint between first marker of parent and first marker of child + for mid_parent in parent_marker_ids: + for mid_child in child_marker_ids: + + if mid_parent == mid_child: + continue + + pos_parent = parent_positions[mid_parent] + pos_child = child_positions[mid_child] + + delta = pos_child - pos_parent + + target_delta_along_axis = float( + np.dot(delta, joint_axis) + ) + + constraint = JointAxisConstraint( + mid_parent, + mid_child, + parent_link, + link_name, + joint_axis, + target_delta_along_axis, + weight=0.5 + ) + + constraints.append(constraint) + + return marker_to_link, constraints + +# === +# Helper Function +# + +def print_constraints_with_errors( + title: str, + constraints: List, + positions: Dict[int, np.ndarray] +) -> None: + + print(f"\n[INFO] {title}") + + for idx, constraint in enumerate(constraints): + + # ---------------------------------------------------------- + # Distance constraint + # ---------------------------------------------------------- + + if isinstance(constraint, MarkerDistanceConstraint): + + if ( + constraint.marker_id_a not in positions or + constraint.marker_id_b not in positions + ): + continue + + pos_a = positions[constraint.marker_id_a] + pos_b = positions[constraint.marker_id_b] + + actual_dist = np.linalg.norm(pos_b - pos_a) + + error_m = actual_dist - constraint.target_distance + + 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*1000:.2f} mm | " + f"actual={actual_dist*1000:.2f} mm | " + f"error={error_m*1000:+.2f} mm" + ) + + # ---------------------------------------------------------- + # Joint-axis constraint + # ---------------------------------------------------------- + + elif isinstance(constraint, JointAxisConstraint): + + if ( + constraint.marker_id_parent not in positions or + constraint.marker_id_child not in positions + ): + continue + + pos_parent = positions[constraint.marker_id_parent] + pos_child = positions[constraint.marker_id_child] + + delta = pos_child - pos_parent + + actual = np.dot(delta, constraint.joint_axis) + + error_m = actual - constraint.target_delta_along_axis + + 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*1000:.2f} mm | " + f"actual={actual*1000:.2f} mm | " + f"error={error_m*1000:+.2f} mm" + ) + +# =================================================================== +# Multi-view loading +# =================================================================== + +def load_observations_and_poses( + detection_files: List[str], + pose_files: List[str] +) -> Tuple[Dict, List, List]: + """Load observations 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 = {} + cameras = [] + obs_metadata = [] + + 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) + + # Camera intrinsics + cam_section = det.get("camera", {}) + K = np.array(cam_section.get("camera_matrix", []), dtype=np.float32).reshape(3, 3) + D = np.array(cam_section.get("distortion_coefficients", []), dtype=np.float32).reshape(-1, 1) + + # Camera pose + pose_section = pose_data.get("camera_pose", {}) + world_to_cam = pose_section.get("world_to_camera", {}) + R_wc = np.array(world_to_cam.get("rotation_matrix", []), dtype=np.float32).reshape(3, 3) + t_wc = np.array(world_to_cam.get("translation_m", []), dtype=np.float32).reshape(3) + + cameras.append((K, D, R_wc, t_wc)) + + # Extract observations + detections = det.get("detections", []) + 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.float32) + + # Undistort to normalized coordinates + pts = center_px.reshape(1, 1, 2).astype(np.float32) + und = cv2.undistortPoints(pts, K, D, P=None) + norm_coords = und.reshape(2).astype(np.float32) + + if marker_id not in marker_observations: + marker_observations[marker_id] = [] + + marker_observations[marker_id].append((cam_idx, norm_coords)) + + return marker_observations, cameras, obs_metadata + + +# =================================================================== +# Initial triangulation +# =================================================================== + +def triangulate_marker_initial( + marker_id: int, + observations: List[Tuple[int, np.ndarray]], + cameras: List[Tuple] +) -> Optional[np.ndarray]: + """Rough triangulation using first two views.""" + if len(observations) < 2: + return None + + K1, D1, R_wc_1, t_wc_1 = cameras[observations[0][0]] + K2, D2, R_wc_2, t_wc_2 = cameras[observations[1][0]] + + norm_coords_1 = observations[0][1] + norm_coords_2 = observations[1][1] + + # Convert to pixel coordinates + x1_px = K1[0, 0] * norm_coords_1[0] + K1[0, 2] + y1_px = K1[1, 1] * norm_coords_1[1] + K1[1, 2] + + x2_px = K2[0, 0] * norm_coords_2[0] + K2[0, 2] + y2_px = K2[1, 1] * norm_coords_2[1] + K2[1, 2] + + P1 = K1 @ np.hstack([R_wc_1, t_wc_1.reshape(3, 1)]) + P2 = K2 @ np.hstack([R_wc_2, t_wc_2.reshape(3, 1)]) + + try: + X_h = cv2.triangulatePoints(P1, P2, + np.array([[x1_px], [y1_px]]).astype(np.float32), + np.array([[x2_px], [y2_px]]).astype(np.float32)) + X = (X_h[:3] / X_h[3]).reshape(3).astype(np.float32) + return X + except Exception: + return None + + +def initial_triangulation( + marker_observations: Dict[int, List[Tuple[int, np.ndarray]]], + cameras: List[Tuple] +) -> Dict[int, np.ndarray]: + """Compute initial marker positions via triangulation.""" + triangulated = {} + + 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 + + +# =================================================================== +# Bundle adjustment with constraints +# =================================================================== + +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], + constraints: List, + lambda_constraint: float = 100.0 +) -> np.ndarray: + """ + Compute residuals for bundle adjustment. + + marker_positions_flat: [x0, y0, z0, x1, y1, z1, ...] + + Returns: [reprojection_residuals, constraint_residuals] + """ + marker_dict = {} + for i, marker_id in enumerate(marker_ids): + marker_dict[marker_id] = marker_positions_flat[i*3:(i+1)*3] + + residuals = [] + + # Reprojection residuals + 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] + + # Project + X_cam = R_wc @ X_world + t_wc + if X_cam[2] > 0.001: # in front of camera + proj_norm = X_cam[:2] / X_cam[2] + residual = proj_norm - norm_coords_obs + residuals.append(residual[0]) + residuals.append(residual[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 = np.linalg.norm(pos_b - pos_a) + residual = (actual_dist - constraint.target_distance) * constraint.weight * lambda_constraint + residuals.append(residual) + + 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_along_axis = np.dot(delta, constraint.joint_axis) + residual = (actual_delta_along_axis - constraint.target_delta_along_axis) * constraint.weight * lambda_constraint + residuals.append(residual) + + return np.array(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], + constraints: List, + lambda_constraint: float = 100.0, + max_iterations: int = 50 +) -> Dict[int, np.ndarray]: + """ + Optimize marker positions using scipy least squares. + """ + try: + from scipy.optimize import least_squares + except ImportError: + print("[WARN] scipy not available, skipping optimization. Using initial triangulation.") + return initial_positions + + marker_ids = sorted(initial_positions.keys()) + x0 = np.concatenate([initial_positions[mid] for mid in marker_ids]) + + def residuals_fn(x): + 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 * len(marker_ids), + verbose=1 + ) + + optimized_flat = result.x + optimized = {} + + for i, marker_id in enumerate(marker_ids): + optimized[marker_id] = optimized_flat[i*3:(i+1)*3] + + print(f"[INFO] Optimization complete. Final cost: {np.sum(result.fun**2):.6f}") + + return optimized + + +# =================================================================== +# Main +# =================================================================== + +def main() -> None: + parser = argparse.ArgumentParser( + description="Multi-view bundle adjustment with 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" + ) + + args = parser.parse_args() + + if len(args.detections) != len(args.poses): + print(f"[ERROR] Mismatch: {len(args.detections)} vs {len(args.poses)}") + sys.exit(1) + + # Load robot + robot_data = load_json(args.robot) + length_scale = get_length_scale(robot_data) + + print("[STEP 1] Extract constraint definitions from robot.json hierarchy...") + marker_to_link, constraints = extract_constraints_from_hierarchy(robot_data, length_scale) + print(f"[INFO] Extracted {len(constraints)} constraints") + + 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"Marker {constraint.marker_id_a} <-> {constraint.marker_id_b} | " + f"TargetDistance={constraint.target_distance:.6f} m | " + f"Weight={constraint.weight}" + ) + + elif isinstance(constraint, JointAxisConstraint): + axis_str = np.array2string( + constraint.joint_axis, + precision=3, + suppress_small=True + ) + + print( + f" [{idx:03d}] JOINT_AXIS | " + f"{constraint.parent_link}({constraint.marker_id_parent}) -> " + f"{constraint.child_link}({constraint.marker_id_child}) | " + f"Axis={axis_str} | " + f"TargetDelta={constraint.target_delta_along_axis:.6f} m | " + f"Weight={constraint.weight}" + ) + + print("\n[STEP 2] Load observations and camera poses...") + marker_observations, cameras, _ = load_observations_and_poses(args.detections, args.poses) + print(f"[INFO] {len(cameras)} cameras, {len(marker_observations)} observed markers") + + print("\n[STEP 3] Initial triangulation...") + initial_pos = initial_triangulation(marker_observations, cameras) + print(f"[INFO] Triangulated {len(initial_pos)} markers") + + # ------------------------------------------------------------ + # Save initial triangulated positions (before optimization) + # ------------------------------------------------------------ + + 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.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "summary": { + "stage": "initial_triangulation", + "num_cameras": len(cameras), + "num_markers": len(initial_pos) + }, + "markers": initial_output_markers + } + + out_dir = args.outDir or os.path.dirname(args.detections[0]) or "." + os.makedirs(resolve_path(out_dir), exist_ok=True) + + 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("\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 + ) + + + # Output + 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.0", + "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_dir = args.outDir or os.path.dirname(args.detections[0]) or "." + os.makedirs(resolve_path(out_dir), exist_ok=True) + 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 __name__ == "__main__": + main() diff --git a/pipeline/aruco_positions_initial.json b/pipeline/aruco_positions_initial.json new file mode 100644 index 0000000..fad9d3a --- /dev/null +++ b/pipeline/aruco_positions_initial.json @@ -0,0 +1,123 @@ +{ + "schema_version": "1.0", + "created_utc": "2026-05-29T07:55:17Z", + "summary": { + "stage": "initial_triangulation", + "num_cameras": 3, + "num_markers": 8 + }, + "markers": [ + { + "marker_id": 122, + "position_m": [ + 0.18520598113536835, + -0.21684008836746216, + 0.13996362686157227 + ], + "position_mm": [ + 185.2059783935547, + -216.840087890625, + 139.963623046875 + ], + "link": "Arm2" + }, + { + "marker_id": 198, + "position_m": [ + 0.12119491398334503, + -0.046654392033815384, + 0.13240119814872742 + ], + "position_mm": [ + 121.19491577148438, + -46.65439224243164, + 132.4011993408203 + ], + "link": "Arm1" + }, + { + "marker_id": 210, + "position_m": [ + 0.020141778513789177, + -0.01963355578482151, + 7.823568921594415e-06 + ], + "position_mm": [ + 20.14177894592285, + -19.633556365966797, + 0.007823568768799305 + ], + "link": "Board" + }, + { + "marker_id": 211, + "position_m": [ + 0.24960945546627045, + -0.009326362982392311, + -0.0005378762143664062 + ], + "position_mm": [ + 249.6094512939453, + -9.326362609863281, + -0.5378761887550354 + ], + "link": "Board" + }, + { + "marker_id": 214, + "position_m": [ + 0.3498840034008026, + -0.009714338928461075, + -2.6824214728549123e-05 + ], + "position_mm": [ + 349.8840026855469, + -9.714339256286621, + -0.026824215427041054 + ], + "link": "Board" + }, + { + "marker_id": 215, + "position_m": [ + 0.2497633397579193, + -0.0896778479218483, + -0.00011857203207910061 + ], + "position_mm": [ + 249.76333618164062, + -89.67784881591797, + -0.11857203394174576 + ], + "link": "Board" + }, + { + "marker_id": 229, + "position_m": [ + 0.12001997977495193, + -0.13270698487758636, + 0.14000186324119568 + ], + "position_mm": [ + 120.01998138427734, + -132.7069854736328, + 140.00186157226562 + ], + "link": "Arm1" + }, + { + "marker_id": 243, + "position_m": [ + 0.11871032416820526, + -0.17057909071445465, + 0.09964759647846222 + ], + "position_mm": [ + 118.7103271484375, + -170.57908630371094, + 99.64759826660156 + ], + "link": "Arm1" + } + ] +} \ No newline at end of file diff --git a/pipeline/aruco_positions_optimized.json b/pipeline/aruco_positions_optimized.json new file mode 100644 index 0000000..f9941b9 --- /dev/null +++ b/pipeline/aruco_positions_optimized.json @@ -0,0 +1,123 @@ +{ + "schema_version": "1.0", + "created_utc": "2026-05-29T07:55:18Z", + "summary": { + "num_cameras": 3, + "num_markers": 8, + "num_constraints": 124 + }, + "markers": [ + { + "marker_id": 122, + "position_m": [ + 0.17089422820752076, + -0.25275415714542, + 0.1764152549525782 + ], + "position_mm": [ + 170.89422820752077, + -252.75415714541998, + 176.4152549525782 + ], + "link": "Arm2" + }, + { + "marker_id": 198, + "position_m": [ + 0.11820089367579532, + -0.047715698531904785, + 0.13484286564203027 + ], + "position_mm": [ + 118.20089367579533, + -47.715698531904785, + 134.84286564203026 + ], + "link": "Arm1" + }, + { + "marker_id": 210, + "position_m": [ + 0.019901746801827637, + -0.019634665689316648, + 9.617254460632054e-06 + ], + "position_mm": [ + 19.901746801827638, + -19.63466568931665, + 0.009617254460632054 + ], + "link": "Board" + }, + { + "marker_id": 211, + "position_m": [ + 0.2499096083030422, + -0.00981801346837422, + 7.989879950350478e-05 + ], + "position_mm": [ + 249.9096083030422, + -9.81801346837422, + 0.07989879950350479 + ], + "link": "Board" + }, + { + "marker_id": 214, + "position_m": [ + 0.3499095633806716, + -0.009897723015074432, + 0.00018350870717776096 + ], + "position_mm": [ + 349.90956338067156, + -9.897723015074432, + 0.18350870717776097 + ], + "link": "Board" + }, + { + "marker_id": 215, + "position_m": [ + 0.24984585113464844, + -0.08981799168050893, + 8.39071085193236e-05 + ], + "position_mm": [ + 249.84585113464843, + -89.81799168050892, + 0.08390710851932359 + ], + "link": "Board" + }, + { + "marker_id": 229, + "position_m": [ + 0.11749065839484758, + -0.137499932562534, + 0.14103531580163794 + ], + "position_mm": [ + 117.49065839484759, + -137.499932562534, + 141.03531580163795 + ], + "link": "Arm1" + }, + { + "marker_id": 243, + "position_m": [ + 0.11810035461635551, + -0.17482765346898027, + 0.10853212349724542 + ], + "position_mm": [ + 118.10035461635552, + -174.82765346898026, + 108.53212349724542 + ], + "link": "Arm1" + } + ] +} \ No newline at end of file diff --git a/pipeline/render_3a_aruco_detection.json b/pipeline/render_3a_aruco_detection.json index 2796b2d..582a1e7 100644 --- a/pipeline/render_3a_aruco_detection.json +++ b/pipeline/render_3a_aruco_detection.json @@ -1,12 +1,12 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T06:03:07Z", + "created_utc": "2026-05-29T07:55:16Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 }, "camera": { - "camera_id": "cam1", + "camera_id": "cam3", "intrinsics_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render.npz", "camera_matrix": [ [ @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "f95b7df5-d56a-4e50-baae-ed98245255b0", + "observation_id": "2ee8a896-adc4-44a7-8e21-00bf2d2bdb29", "type": "aruco", "marker_id": 124, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.6862086405991146 }, { - "observation_id": "d6a89cdb-43b2-4494-84c8-47d5de39baa8", + "observation_id": "122847ec-48d6-477c-a833-61a1cbb5620b", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.953171926139578 }, { - "observation_id": "05209fc7-239c-43ca-8cf8-7566f456041d", + "observation_id": "4efd7b96-c897-44c5-9f63-6dfccdb0dcf0", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.5964639370258038 }, { - "observation_id": "52848f64-67df-4213-91b2-ffdfe9064ee2", + "observation_id": "c7a43ad5-e272-4673-af80-a17d11161ae4", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.3578323322772018 }, { - "observation_id": "7bc13599-644e-4dbd-8ad2-ad5f80056d8c", + "observation_id": "7d8958be-4ff2-4929-a78a-0e3239574df7", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.4396423002158715 }, { - "observation_id": "41826dad-da5d-4fb1-87e4-757a2c21171a", + "observation_id": "dea83763-ba79-4211-ace5-2fb4693ec614", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.23718801109435655 }, { - "observation_id": "f354d0f3-34a9-428e-887d-a4364525a2b0", + "observation_id": "04e86e9d-49e6-4cc3-b104-8cc4bddae5e1", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.27797680859006363 }, { - "observation_id": "447b944c-8faf-41e5-b724-af8f221c64fc", + "observation_id": "d844a883-c800-4e9c-8f99-fdb379cf9d6a", "type": "aruco", "marker_id": 205, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.19676029488014599 }, { - "observation_id": "51c410f3-45e5-4d70-9bd8-535c1306e4cf", + "observation_id": "e1194640-f6cb-4af1-8fe3-41d19817b79e", "type": "aruco", "marker_id": 206, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.23511362818048806 }, { - "observation_id": "61058f2a-9c08-4073-a43e-7a907cf8fb75", + "observation_id": "1aa5ff5a-71ef-4362-aeb0-2f000b6c7f37", "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 14be9b7..888cc23 100644 --- a/pipeline/render_3a_camera_pose.json +++ b/pipeline/render_3a_camera_pose.json @@ -1,12 +1,12 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T06:03:09Z", + "created_utc": "2026-05-29T07:55:17Z", "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" }, "camera": { - "camera_id": "cam1", + "camera_id": "cam3", "camera_matrix": [ [ 1777.77783203125, @@ -123,7 +123,7 @@ ], "orientation_deg": { "roll": 122.43758392333984, - "pitch": -35.883320699421276, + "pitch": -35.88331985473633, "yaw": -19.852933883666992 } }, @@ -202,7 +202,7 @@ ], "orientation_std_deg": { "roll": 0.1051783179305808, - "pitch": 0.14376921142986104, + "pitch": 0.13780027424955138, "yaw": 0.02230052560885805 } } diff --git a/pipeline/render_3b_aruco_detection.json b/pipeline/render_3b_aruco_detection.json index 4801ba7..c254e2a 100644 --- a/pipeline/render_3b_aruco_detection.json +++ b/pipeline/render_3b_aruco_detection.json @@ -1,12 +1,12 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T06:03:08Z", + "created_utc": "2026-05-29T07:55:16Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 }, "camera": { - "camera_id": "cam1", + "camera_id": "cam2", "intrinsics_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render.npz", "camera_matrix": [ [ @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "df664593-ada8-4c44-a00f-8936a037e7eb", + "observation_id": "37e350d1-d7f5-4006-9e7c-eabb597891bb", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.8522257252911212 }, { - "observation_id": "a7f8731a-1479-4db3-af34-2b5542362b93", + "observation_id": "fb1bfc74-794b-4065-b35e-f49b5e413a94", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.2874514216623714 }, { - "observation_id": "3cf6936d-c17e-46be-960e-fec43a1a8a34", + "observation_id": "6766568d-a40c-4c97-87fd-77e5243838fb", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.5547001883002887 }, { - "observation_id": "1eeec489-e960-4f11-bcc5-d84cfd85678d", + "observation_id": "958853a3-10c7-486a-8985-8fa8eab7fb7a", "type": "aruco", "marker_id": 208, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.6072727689079809 }, { - "observation_id": "7f948349-46ae-4ada-bf71-a87a11853b58", + "observation_id": "0ba8ce36-cbf9-42b2-8829-3c1b5adcf247", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.6168610191628993 }, { - "observation_id": "22b325e5-012e-458f-8841-176fddce4f01", + "observation_id": "3e48e695-6ede-4e3d-932c-161c06a03ac7", "type": "aruco", "marker_id": 214, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.5524643209674667 }, { - "observation_id": "297f15ef-5a04-4fde-ad31-715064472cab", + "observation_id": "386a4b8c-7efc-4cad-a7e0-c02ce8d15271", "type": "aruco", "marker_id": 211, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.5730650050844824 }, { - "observation_id": "5a57e9e6-1e2e-4061-aafe-72b4d412b042", + "observation_id": "cb63df83-5a11-4455-89ae-c741e2c649dd", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.4163346774695628 }, { - "observation_id": "8b92d6ef-360b-40af-9d92-c1f0e013c0ee", + "observation_id": "36b8474a-1769-40d3-86ad-98031727ad2e", "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 d70fff4..222cfb4 100644 --- a/pipeline/render_3b_camera_pose.json +++ b/pipeline/render_3b_camera_pose.json @@ -1,12 +1,12 @@ { "schema_version": "1.0", - "created_utc": "2026-05-29T06:03:10Z", + "created_utc": "2026-05-29T07:55:17Z", "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" }, "camera": { - "camera_id": "cam1", + "camera_id": "cam2", "camera_matrix": [ [ 1777.77783203125, @@ -118,7 +118,7 @@ ], "orientation_deg": { "roll": 129.43496704101562, - "pitch": 7.254831367062062, + "pitch": 7.254830837249756, "yaw": 6.209517478942871 } }, @@ -197,7 +197,7 @@ ], "orientation_std_deg": { "roll": 0.38938838441106427, - "pitch": 0.37119422591459295, + "pitch": 0.36987460773433695, "yaw": 0.11587592903072558 } } diff --git a/pipeline/render_3c_aruco_detection.json b/pipeline/render_3c_aruco_detection.json index 98ca3f6..dadeb5e 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-29T06:03:09Z", + "created_utc": "2026-05-29T07:55:15Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "c428f7ca-c6c6-4208-994c-69a56fa87a1d", + "observation_id": "d51ce37d-1f1f-40ad-a6a2-a6c8f49b43ff", "type": "aruco", "marker_id": 219, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.7735450417034769 }, { - "observation_id": "00e68272-4cb7-4b93-98f8-efcc188685da", + "observation_id": "1806bda1-4af5-4faa-b0a3-0091bd35eabc", "type": "aruco", "marker_id": 218, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.7950075578135153 }, { - "observation_id": "ad0954b8-da38-4d82-825c-a05e65b0c5db", + "observation_id": "82259a28-11c6-4355-967d-d2c525cba254", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.8202207056111124 }, { - "observation_id": "94886815-8819-4fd0-9dd9-0579e8bb356a", + "observation_id": "97b9df19-3ff6-4315-8c74-470848b59a8e", "type": "aruco", "marker_id": 214, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.1700115058329765 }, { - "observation_id": "850986ee-399f-423c-8ffc-38c8b18b764d", + "observation_id": "6e25efa5-9cfb-4415-8be0-1c253eb68b9c", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.9372326698512834 }, { - "observation_id": "fca3abcc-7c19-415d-bf78-c47360086985", + "observation_id": "8411563f-277d-44a1-b331-70ea22f8f316", "type": "aruco", "marker_id": 244, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.7137086345973509 }, { - "observation_id": "2a004519-5a1e-4b51-ac4e-f2e15b9f8ddd", + "observation_id": "20756782-3ad5-4dc0-9e2b-eea48226a4e7", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.8888336147757544 }, { - "observation_id": "d3d0a119-6c30-492f-9f4c-6e3fb13d8985", + "observation_id": "022ae18d-f34f-420e-8117-ec22aa3f52e4", "type": "aruco", "marker_id": 211, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.8902520865668376 }, { - "observation_id": "93aa8eca-a954-4c99-a946-ce5f70ccfaaf", + "observation_id": "22a306fc-a02a-46c3-b9e1-d240ddb40ee6", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.7343609168978227 }, { - "observation_id": "f5108f58-1608-4b8f-b8a0-a8b31179e150", + "observation_id": "76dba603-04ec-4e63-bc5e-8ea448020025", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -586,7 +586,7 @@ "confidence": 0.72560382306579 }, { - "observation_id": "2ca775d6-ed4d-4ad0-85d1-1110d61be0a0", + "observation_id": "173b2222-1e89-405e-8e94-0c43f4773db1", "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 3306c55..bb58865 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-29T06:03:10Z", + "created_utc": "2026-05-29T07:55:16Z", "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" @@ -131,7 +131,7 @@ ], "orientation_deg": { "roll": 149.0894775390625, - "pitch": 27.466156494334694, + "pitch": 27.466156005859375, "yaw": 42.493896484375 } }, @@ -210,7 +210,7 @@ ], "orientation_std_deg": { "roll": 4.593062572697299e-13, - "pitch": 4.604301919561157e-13, + "pitch": 4.725448419113441e-13, "yaw": 1.6845365562674818e-13 } } diff --git a/pipeline/run_pipeline_multiview.bat b/pipeline/run_pipeline_multiview.bat new file mode 100644 index 0000000..b515877 --- /dev/null +++ b/pipeline/run_pipeline_multiview.bat @@ -0,0 +1,51 @@ +@echo off +REM 3_pipeline_multiview.bat +REM Multi-camera ArUco detection and pose estimation pipeline + +REM Example: 3 camera views +REM Usage: run_pipeline_multiview.bat + +cd /d "%~dp0" + +set ROBOT_JSON=..\robot.json +set OUT_DIR=. + +REM Camera 1 +set IMG_1=render_3c.png +set NPZ_1=render.npz +set CAM_ID_1=cam1 + +REM Camera 2 (example - you need actual second camera image) +set IMG_2=render_3b.png +set NPZ_2=render.npz +set CAM_ID_2=cam2 + +REM Camera 3 (example) +set IMG_3=render_3a.png +set NPZ_3=render.npz +set CAM_ID_3=cam3 + +echo [STEP 1] Detect ArUco markers from all cameras +python 1_detect_aruco_observations.py -i %IMG_1% -npz %NPZ_1% -robot %ROBOT_JSON% -cameraId %CAM_ID_1% -outDir %OUT_DIR% +python 1_detect_aruco_observations.py -i %IMG_2% -npz %NPZ_2% -robot %ROBOT_JSON% -cameraId %CAM_ID_2% -outDir %OUT_DIR% +python 1_detect_aruco_observations.py -i %IMG_3% -npz %NPZ_3% -robot %ROBOT_JSON% -cameraId %CAM_ID_3% -outDir %OUT_DIR% + +echo. +echo [STEP 2] Estimate camera poses from detections +python 2_estimate_camera_from_observations.py -i render_3c_aruco_detection.json -robot %ROBOT_JSON% -outDir %OUT_DIR% +python 2_estimate_camera_from_observations.py -i render_3b_aruco_detection.json -robot %ROBOT_JSON% -outDir %OUT_DIR% +python 2_estimate_camera_from_observations.py -i render_3a_aruco_detection.json -robot %ROBOT_JSON% -outDir %OUT_DIR% + +echo. +echo [STEP 3] Triangulate marker positions from multi-view observations +python 3_multiview_bundle_adjustment.py ^ + -det render_3a_aruco_detection.json ^ + -det render_3b_aruco_detection.json ^ + -det render_3c_aruco_detection.json ^ + -pose render_3a_camera_pose.json ^ + -pose render_3b_camera_pose.json ^ + -pose render_3c_camera_pose.json ^ + -robot %ROBOT_JSON% -lambdaWeight 100.0 + +echo. +echo [DONE] Pipeline complete