diff --git a/pipeline/2_KameraPosition.py b/pipeline/2_KameraPosition.py deleted file mode 100644 index cf4c5d1..0000000 --- a/pipeline/2_KameraPosition.py +++ /dev/null @@ -1,562 +0,0 @@ -#!/usr/bin/env python3 -""" -Step 2 of the vision pipeline -============================= - -Overall workflow ----------------- -1) Detect ArUco markers in each image and store 2D corners per marker. -2) Use the Board markers from robot.json as fixed world references. -3) Estimate the camera pose for each image with a Perspective-n-Point (PnP) - solve from the detected 2D corners and the known 3D Board marker corners. -4) Use that camera pose later as the fixed extrinsic input for articulated - bundle adjustment of the robot joints. - -Mathematical methods --------------------- -- Homogeneous coordinates and rigid transforms in SE(3) -- ArUco marker geometry on a known board frame -- Perspective projection with camera intrinsics K and distortion D -- Robust pose estimation with RANSAC-PnP -- Optional nonlinear refinement by minimizing reprojection error - -Conventions ------------ -- robot.json defines the Board frame as the world frame. -- The camera pose reported by this script is the pose of the CAMERA in the - Board/world frame. -- The OpenCV rvec/tvec output is the object pose in the camera frame. - We invert that transform to obtain world_T_camera. - -Expected inputs ---------------- -- robot.json containing Board marker positions in millimeters. -- ArUco detection JSON(s) from detect_aruco_observations.py. - -Important: -The detection JSON already contains the camera intrinsics and distortion -coefficients copied from the calibration file during Step 1. -Therefore Step 2 no longer requires the original .npz file. -The detection JSON becomes the self-contained handover format between -pipeline stages. - -Output ------- -A JSON file per input detection file containing: -- camera pose (world_T_camera) -- camera pose inverse (camera_T_world) -- per-marker and global reprojection errors -- number of inlier correspondences -- list of used marker IDs -""" - -from __future__ import annotations - -import argparse -import json -import math -import os -from dataclasses import dataclass -from pathlib import Path -from typing import Any, Dict, Iterable, List, Optional, Sequence, Tuple - -import cv2 -import numpy as np - - -# ---------------------------------------------------------------------------- -# Helpers -# ---------------------------------------------------------------------------- - - -def load_robot_json(robot_json_path: str) -> Dict[str, Any]: - with open(robot_json_path, "r", encoding="utf-8") as f: - return json.load(f) - - - -def load_intrinsics_from_detection_json(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: - cam = detection_json.get("camera", {}) or {} - - if "camera_matrix" not in cam: - raise KeyError("camera_matrix missing in detection JSON") - - K = np.asarray(cam["camera_matrix"], dtype=np.float64) - - D = np.asarray( - cam.get("distortion_coefficients", [0, 0, 0, 0, 0]), - dtype=np.float64, - ).reshape(-1, 1) - - return K, D - - - -def as_float3(v: Any) -> np.ndarray: - arr = np.asarray(v, dtype=np.float64).reshape(-1) - if arr.size < 3: - arr = np.pad(arr, (0, 3 - arr.size)) - return arr[:3] - - - -def normalize(v: np.ndarray, eps: float = 1e-12) -> np.ndarray: - n = float(np.linalg.norm(v)) - if n < eps: - return v * 0.0 - return v / n - - - -def rotation_from_normal(normal: np.ndarray) -> np.ndarray: - """Return a 3x3 rotation matrix whose local +Z aligns with the given normal. - - The local x/y axes are chosen deterministically by projecting a stable world - reference axis into the tangent plane. - """ - z_axis = normalize(normal) - if np.linalg.norm(z_axis) < 1e-12: - raise ValueError("Degenerate normal vector") - - ref = np.array([1.0, 0.0, 0.0], dtype=np.float64) - if abs(float(np.dot(ref, z_axis))) > 0.9: - ref = np.array([0.0, 1.0, 0.0], dtype=np.float64) - - x_axis = ref - np.dot(ref, z_axis) * z_axis - x_axis = normalize(x_axis) - if np.linalg.norm(x_axis) < 1e-12: - ref = np.array([0.0, 1.0, 0.0], dtype=np.float64) - x_axis = ref - np.dot(ref, z_axis) * z_axis - x_axis = normalize(x_axis) - - y_axis = np.cross(z_axis, x_axis) - y_axis = normalize(y_axis) - - # Columns are the basis vectors of the local frame in world coordinates. - R = np.column_stack([x_axis, y_axis, z_axis]) - return R - - - -def rodrigues_to_matrix(rvec: np.ndarray) -> np.ndarray: - R, _ = cv2.Rodrigues(rvec.reshape(3, 1)) - return R.astype(np.float64) - - - -def matrix_to_rodrigues(R: np.ndarray) -> np.ndarray: - rvec, _ = cv2.Rodrigues(R.astype(np.float64)) - return rvec.reshape(3) - - - -def invert_rigid_transform(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: - """Invert p_cam = R * p_world + t.""" - R_inv = R.T - t_inv = -R_inv @ t.reshape(3) - return R_inv, t_inv - - - -def make_homogeneous(R: np.ndarray, t: np.ndarray) -> np.ndarray: - T = np.eye(4, dtype=np.float64) - T[:3, :3] = R - T[:3, 3] = t.reshape(3) - return T - - - -def project_points(points_3d: np.ndarray, rvec: np.ndarray, tvec: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray: - pts = points_3d.reshape(-1, 1, 3).astype(np.float64) - img_pts, _ = cv2.projectPoints(pts, rvec.reshape(3, 1), tvec.reshape(3, 1), K, D) - return img_pts.reshape(-1, 2) - - - -def rms_error(errors_px: np.ndarray) -> float: - if errors_px.size == 0: - return float("nan") - return float(np.sqrt(np.mean(np.sum(errors_px ** 2, axis=1)))) - - - -def safe_confidence(det: Dict[str, Any]) -> float: - try: - c = float(det.get("confidence", 1.0)) - except Exception: - c = 1.0 - return max(0.0, min(1.0, c)) - - -# ---------------------------------------------------------------------------- -# Robot board model -# ---------------------------------------------------------------------------- - - -@dataclass -class BoardMarker: - marker_id: int - center_world_mm: np.ndarray - normal_world: np.ndarray - size_mm: float - rotation_world: np.ndarray - - def corner_points_world_mm(self) -> np.ndarray: - """Return the 4 corner points in board/world coordinates. - - Corner order is OpenCV/ArUco compatible: - top-left, top-right, bottom-right, bottom-left in the marker local frame. - """ - s = float(self.size_mm) - half = s * 0.5 - - # Local marker corners in the marker's own frame, z=0. - corners_local = np.array([ - [-half, +half, 0.0], - [+half, +half, 0.0], - [+half, -half, 0.0], - [-half, -half, 0.0], - ], dtype=np.float64) - - corners_world = (self.rotation_world @ corners_local.T).T + self.center_world_mm.reshape(1, 3) - return corners_world - - - -def extract_board_markers(robot: Dict[str, Any]) -> Dict[int, BoardMarker]: - links = robot.get("links", {}) - if "Board" not in links: - raise KeyError("robot.json must contain links.Board for the world reference frame") - - board = links["Board"] - marker_defaults = robot.get("renderingInfo", {}).get("markerDefaults", {}) or {} - default_size_mm = float(marker_defaults.get("size", 25.0)) - - markers: Dict[int, BoardMarker] = {} - - for m in board.get("markers", []): - if not isinstance(m, dict): - continue - if "id" not in m or "position" not in m: - continue - - marker_id = int(m["id"]) - center = as_float3(m["position"]) - normal = as_float3(m.get("normal", [0, 0, 1])) - size_mm = float(m.get("size", default_size_mm)) - - # If a spin is present in the future, it can be added here. - # For the current Board markers it is not needed. - R = rotation_from_normal(normal) - - markers[marker_id] = BoardMarker( - marker_id=marker_id, - center_world_mm=center, - normal_world=normalize(normal), - size_mm=size_mm, - rotation_world=R, - ) - - if not markers: - raise ValueError("No Board markers found in robot.json") - - return markers - - -# ---------------------------------------------------------------------------- -# Observation loading -# ---------------------------------------------------------------------------- - - -def load_detection_json(path: str) -> Dict[str, Any]: - with open(path, "r", encoding="utf-8") as f: - return json.load(f) - - - -def build_pnp_correspondences( - detections: Sequence[Dict[str, Any]], - board_markers: Dict[int, BoardMarker], -) -> Tuple[np.ndarray, np.ndarray, np.ndarray, List[int]]: - """Build object/image correspondences from all detected Board markers. - - Returns: - object_points_mm: (N, 3) - image_points_px: (N, 2) - weights: (N,) - used_marker_ids: list of marker ids that contributed at least one corner - """ - object_points: List[np.ndarray] = [] - image_points: List[np.ndarray] = [] - weights: List[np.ndarray] = [] - used_marker_ids: List[int] = [] - - for det in detections: - if str(det.get("type", "")).lower() != "aruco": - continue - - try: - marker_id = int(det["marker_id"]) - except Exception: - continue - - if marker_id not in board_markers: - continue - - corners_px = np.asarray(det.get("image_points_px", []), dtype=np.float64) - if corners_px.shape != (4, 2): - continue - - marker = board_markers[marker_id] - corners_world_mm = marker.corner_points_world_mm() - - object_points.append(corners_world_mm) - image_points.append(corners_px) - - conf = safe_confidence(det) - # Repeat the marker confidence for all 4 corners. - weights.append(np.full((4,), conf, dtype=np.float64)) - used_marker_ids.append(marker_id) - - if not object_points: - raise ValueError("No usable Board marker correspondences found in detections") - - obj = np.concatenate(object_points, axis=0).astype(np.float64) - img = np.concatenate(image_points, axis=0).astype(np.float64) - w = np.concatenate(weights, axis=0).astype(np.float64) - - return obj, img, w, sorted(set(used_marker_ids)) - - -# ---------------------------------------------------------------------------- -# Pose estimation -# ---------------------------------------------------------------------------- - - -def pnp_initial_pose(object_points_mm: np.ndarray, image_points_px: np.ndarray, K: np.ndarray, D: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - """Robust initial pose estimate using RANSAC-PnP when possible.""" - obj = object_points_mm.reshape(-1, 1, 3).astype(np.float64) - img = image_points_px.reshape(-1, 1, 2).astype(np.float64) - - # Prefer RANSAC for robustness. - ok, rvec, tvec, inliers = cv2.solvePnPRansac( - obj, - img, - K, - D, - iterationsCount=200, - reprojectionError=4.0, - confidence=0.999, - flags=cv2.SOLVEPNP_EPNP, - ) - - if not ok or rvec is None or tvec is None: - # Fallback to a direct solve. - ok, rvec, tvec = cv2.solvePnP( - obj, - img, - K, - D, - flags=cv2.SOLVEPNP_ITERATIVE, - ) - if not ok: - raise RuntimeError("cv2.solvePnP failed") - inliers = np.arange(len(object_points_mm), dtype=np.int32).reshape(-1, 1) - - rvec = rvec.reshape(3) - tvec = tvec.reshape(3) - - if inliers is None: - inliers = np.arange(len(object_points_mm), dtype=np.int32).reshape(-1, 1) - - return rvec, tvec, inliers - - - -def refine_pose_with_weights( - rvec_init: np.ndarray, - tvec_init: np.ndarray, - object_points_mm: np.ndarray, - image_points_px: np.ndarray, - weights: np.ndarray, - K: np.ndarray, - D: np.ndarray, -) -> Tuple[np.ndarray, np.ndarray]: - """Optional weighted nonlinear refinement. - - Uses OpenCV's LM refinement if available; otherwise falls back to the initial pose. - """ - rvec = rvec_init.reshape(3, 1).astype(np.float64) - tvec = tvec_init.reshape(3, 1).astype(np.float64) - - # Build a diagonal weighting by repeating stronger correspondences more often is not ideal. - # Instead, use a conservative weighted refinement via OpenCV if present. - if hasattr(cv2, "solvePnPRefineLM"): - try: - cv2.solvePnPRefineLM( - object_points_mm.reshape(-1, 1, 3).astype(np.float64), - image_points_px.reshape(-1, 1, 2).astype(np.float64), - K, - D, - rvec, - tvec, - ) - return rvec.reshape(3), tvec.reshape(3) - except Exception: - pass - - return rvec_init.reshape(3), tvec_init.reshape(3) - - - -def compute_reprojection_statistics( - object_points_mm: np.ndarray, - image_points_px: np.ndarray, - rvec: np.ndarray, - tvec: np.ndarray, - K: np.ndarray, - D: np.ndarray, -) -> Dict[str, Any]: - pred = project_points(object_points_mm, rvec, tvec, K, D) - residual = image_points_px - pred - err = np.linalg.norm(residual, axis=1) - - return { - "rmse_px": float(np.sqrt(np.mean(err ** 2))) if err.size else float("nan"), - "mean_px": float(np.mean(err)) if err.size else float("nan"), - "median_px": float(np.median(err)) if err.size else float("nan"), - "max_px": float(np.max(err)) if err.size else float("nan"), - "per_corner_errors_px": err.tolist(), - "per_corner_residuals_px": residual.tolist(), - } - - - -def pose_to_camera_in_world(rvec: np.ndarray, tvec: np.ndarray) -> Dict[str, Any]: - """Convert object-to-camera pose into camera pose in world coordinates.""" - R_wc_obj = rodrigues_to_matrix(rvec) - t_wc_obj = tvec.reshape(3) - - # object/world -> camera is: p_cam = R * p_world + t - # therefore camera in world is inverse transform. - R_cw, t_cw = invert_rigid_transform(R_wc_obj, t_wc_obj) - T_world_camera = make_homogeneous(R_cw, t_cw) - T_camera_world = make_homogeneous(R_wc_obj, t_wc_obj) - - return { - "rvec_world_to_camera": rvec.reshape(3).tolist(), - "tvec_world_to_camera_mm": tvec.reshape(3).tolist(), - "R_world_to_camera": R_wc_obj.tolist(), - "T_camera_world": T_camera_world.tolist(), - "R_camera_to_world": R_cw.tolist(), - "t_camera_in_world_mm": t_cw.tolist(), - "T_world_camera": T_world_camera.tolist(), - } - - - -def estimate_camera_pose_from_detection( - detection_json: Dict[str, Any], - robot: Dict[str, Any], -) -> Dict[str, Any]: - board_markers = extract_board_markers(robot) - K, D = load_intrinsics_from_detection_json(detection_json) - - detections = detection_json.get("detections", []) - object_points_mm, image_points_px, weights, used_marker_ids = build_pnp_correspondences(detections, board_markers) - - rvec_init, tvec_init, inliers = pnp_initial_pose(object_points_mm, image_points_px, K, D) - rvec, tvec = refine_pose_with_weights(rvec_init, tvec_init, object_points_mm, image_points_px, weights, K, D) - - stats = compute_reprojection_statistics(object_points_mm, image_points_px, rvec, tvec, K, D) - pose = pose_to_camera_in_world(rvec, tvec) - - # Add a few diagnostics useful for checking against the real world. - camera_pose = { - **pose, - "statistics": { - **stats, - "num_correspondences": int(len(object_points_mm)), - "num_inliers": int(len(inliers)) if inliers is not None else int(len(object_points_mm)), - "used_marker_ids": used_marker_ids, - }, - "input": { - "detection_image_file": detection_json.get("image", {}).get("image_file"), - "camera_id": detection_json.get("camera", {}).get("camera_id"), - "marker_dictionary": detection_json.get("vision_config", {}).get("MarkerType"), - }, - } - - return camera_pose - - -# ---------------------------------------------------------------------------- -# Main -# ---------------------------------------------------------------------------- - - -def main() -> None: - parser = argparse.ArgumentParser(description="Estimate camera pose from Board ArUco markers using PnP.") - parser.add_argument("--robot", required=True, help="Path to robot.json") - parser.add_argument( - "--detections", - required=True, - nargs="+", - help="One or more detection JSON files created by detect_aruco_observations.py", - ) - parser.add_argument("--outdir", required=True, help="Directory for the pose JSON outputs") - parser.add_argument("--write-summary", action="store_true", help="Write one combined summary JSON as well") - args = parser.parse_args() - - os.makedirs(args.outdir, exist_ok=True) - - robot = load_robot_json(args.robot) - - summary: Dict[str, Any] = { - "schema_version": "1.0", - "algorithm": "board_pnp_camera_pose", - "robot_file": os.path.abspath(args.robot), - "intrinsics_source": "embedded_in_detection_json", - "results": [], - } - - for det_path in args.detections: - detection_json = load_detection_json(det_path) - pose = estimate_camera_pose_from_detection(detection_json, robot) - - base = Path(det_path).stem - out_path = Path(args.outdir) / f"{base}_camera_pose.json" - - payload = { - "schema_version": "1.0", - "created_utc": __import__("time").strftime("%Y-%m-%dT%H:%M:%SZ", __import__("time").gmtime()), - "source_detection_file": os.path.abspath(det_path), - "camera_pose": pose, - } - - with open(out_path, "w", encoding="utf-8") as f: - json.dump(payload, f, indent=2) - - summary["results"].append({ - "detection_file": os.path.abspath(det_path), - "output_file": str(out_path), - "rmse_px": pose["statistics"]["rmse_px"], - "median_px": pose["statistics"]["median_px"], - "num_correspondences": pose["statistics"]["num_correspondences"], - "used_marker_ids": pose["statistics"]["used_marker_ids"], - }) - - print(f"Saved: {out_path}") - print(f" RMSE: {pose['statistics']['rmse_px']:.3f} px") - print(f" Median: {pose['statistics']['median_px']:.3f} px") - print(f" Used markers: {pose['statistics']['used_marker_ids']}") - - if args.write_summary: - summary_path = Path(args.outdir) / "camera_pose_summary.json" - with open(summary_path, "w", encoding="utf-8") as f: - json.dump(summary, f, indent=2) - print(f"Saved summary: {summary_path}") - - -if __name__ == "__main__": - main() diff --git a/pipeline/2_Multiview.py b/pipeline/2_Multiview.py new file mode 100644 index 0000000..59c3b1e --- /dev/null +++ b/pipeline/2_Multiview.py @@ -0,0 +1,706 @@ +#!/usr/bin/env python3 + +""" +============================================================ +STEP 2b — Simultane Multiview-Optimierung für Roboterpose +============================================================ + +Ziel: + Aus mehreren ArUco-Detektionsdateien die gemeinsame + Roboterpose (x,y,z,a,b,c,e) schätzen und jede Kamera-Pose + sowie Marker-Weltpositionen ausgeben. + +Eingabe: + --robot ../robot.json + --detections render_1a_aruco_detection.json render_1b_aruco_detection.json ... + --outDir . + +Ausgabe: + multiview_pose.json + +Hinweis: + Dieses Skript verwendet die Markerpositionen aus robot.json + als kinematische Constraints und optimiert gleichzeitig: + - Roboterzustand (x,y,z,a,b,c,e) + - Kameraextrinsische Parameter pro Bild + +""" + +import argparse +import datetime +import json +import math +import os +import time +from pathlib import Path +from typing import Any, Dict, List, Tuple + +import cv2 +import numpy as np +from scipy.optimize import least_squares + +STATE_KEYS = ["x", "y", "z", "a", "b", "c", "e"] + + +# ------------------------------------------------------------------ +# JSON helpers +# ------------------------------------------------------------------ + +def load_json(path: str) -> Dict[str, Any]: + with open(path, 'r', encoding='utf-8') as f: + return json.load(f) + + +def save_json(data: Dict[str, Any], path: Path) -> None: + with open(path, 'w', encoding='utf-8') as f: + json.dump(data, f, indent=2) + + +# ------------------------------------------------------------------ +# robot.json helpers +# ------------------------------------------------------------------ + +def resolve_scalar(value: Any, default: float = 0.0) -> float: + if value is None: + return default + if isinstance(value, (int, float)): + return float(value) + try: + return float(str(value).strip()) + except ValueError: + return default + + +def resolve_vector(value: Any, default_len: int = 3) -> Tuple[float, ...]: + if value is None: + return tuple(0.0 for _ in range(default_len)) + if isinstance(value, (int, float, str)): + return (resolve_scalar(value),) + tuple(0.0 for _ in range(default_len - 1)) + if isinstance(value, (list, tuple)): + resolved = [resolve_scalar(v) for v in value] + if len(resolved) < default_len: + resolved.extend([0.0] * (default_len - len(resolved))) + return tuple(resolved[:default_len]) + return tuple(0.0 for _ in range(default_len)) + + +def parse_metric_scale(robot: Dict[str, Any]) -> float: + rendering_info = robot.get('renderingInfo', {}) or {} + metric = rendering_info.get('metric', 'mm') + return 0.001 if str(metric).strip().lower() == 'mm' else 1.0 + + +def normalize_axis(axis: Any) -> np.ndarray: + vec = np.asarray(axis, dtype=np.float64) + if vec.shape != (3,): + vec = vec.reshape(-1)[:3] + norm = np.linalg.norm(vec) + return vec / max(norm, 1e-9) + + +def euler_deg_to_matrix(euler_deg: Any) -> np.ndarray: + x_deg, y_deg, z_deg = resolve_vector(euler_deg, 3) + x = math.radians(x_deg) + y = math.radians(y_deg) + z = math.radians(z_deg) + + cx = math.cos(x) + sx = math.sin(x) + cy = math.cos(y) + sy = math.sin(y) + cz = math.cos(z) + sz = math.sin(z) + + Rx = np.array([ + [1.0, 0.0, 0.0], + [0.0, cx, -sx], + [0.0, sx, cx] + ], dtype=np.float64) + + Ry = np.array([ + [cy, 0.0, sy], + [0.0, 1.0, 0.0], + [-sy, 0.0, cy] + ], dtype=np.float64) + + Rz = np.array([ + [cz, -sz, 0.0], + [sz, cz, 0.0], + [0.0, 0.0, 1.0] + ], dtype=np.float64) + + return Rz @ Ry @ Rx + + +def transform_from_translation_rotation(translation: Any, rotation_deg: Any) -> np.ndarray: + T = np.eye(4, dtype=np.float64) + pos = np.asarray(resolve_vector(translation, 3), dtype=np.float64) + T[:3, 3] = pos + T[:3, :3] = euler_deg_to_matrix(rotation_deg) + return T + + +def axis_angle_matrix(axis: Any, angle_deg: float) -> np.ndarray: + axis_vec = normalize_axis(axis) + theta = math.radians(angle_deg) + kx, ky, kz = axis_vec + c = math.cos(theta) + s = math.sin(theta) + v = 1.0 - c + R = np.array([ + [kx * kx * v + c, kx * ky * v - kz * s, kx * kz * v + ky * s], + [ky * kx * v + kz * s, ky * ky * v + c, ky * kz * v - kx * s], + [kz * kx * v - ky * s, kz * ky * v + kx * s, kz * kz * v + c] + ], dtype=np.float64) + T = np.eye(4, dtype=np.float64) + T[:3, :3] = R + return T + + +# ------------------------------------------------------------------ +# Kinematics and marker extraction +# ------------------------------------------------------------------ + +def extract_markers(robot: Dict[str, Any], scale: float) -> Dict[int, Dict[str, Any]]: + markers = {} + links = robot.get('links', {}) or {} + marker_defaults = (robot.get('renderingInfo', {}) or {}).get('markerDefaults', {}) or {} + default_size_mm = float(marker_defaults.get('size', 25.0)) + + for link_name, link_info in links.items(): + for marker in link_info.get('markers', []) or []: + marker_id = int(marker.get('id', -1)) + if marker_id < 0: + continue + + pos = resolve_vector(marker.get('position', [0, 0, 0]), 3) + size_mm = float(marker.get('size', default_size_mm)) + markers[marker_id] = { + 'marker_id': marker_id, + 'link_name': link_name, + 'position_m': np.asarray([pos[0] * scale, pos[1] * scale, pos[2] * scale], dtype=np.float64), + 'normal': normalize_axis(resolve_vector(marker.get('normal', [0, 0, 1]), 3)), + 'spin_deg': float(marker.get('spin', 0.0)), + 'size_m': size_mm * scale, + } + + return markers + + +def marker_plane_axes(normal: np.ndarray, spin_deg: float) -> Tuple[np.ndarray, np.ndarray]: + n = normalize_axis(normal) + candidate = np.array((0.0, 0.0, 1.0), dtype=np.float64) + if abs(np.dot(n, candidate)) > 0.99: + candidate = np.array((1.0, 0.0, 0.0), dtype=np.float64) + + x_dir = np.cross(candidate, n) + x_dir /= max(np.linalg.norm(x_dir), 1e-9) + y_dir = np.cross(n, x_dir) + + if abs(spin_deg) > 1e-6: + theta = math.radians(spin_deg) + cos_t = math.cos(theta) + sin_t = math.sin(theta) + x_rot = x_dir * cos_t + np.cross(n, x_dir) * sin_t + n * np.dot(n, x_dir) * (1.0 - cos_t) + y_rot = y_dir * cos_t + np.cross(n, y_dir) * sin_t + n * np.dot(n, y_dir) * (1.0 - cos_t) + return x_rot, y_rot + + return x_dir, y_dir + + +def marker_object_corners(marker: Dict[str, Any]) -> np.ndarray: + half = marker['size_m'] * 0.5 + x_dir, y_dir = marker_plane_axes(marker['normal'], marker['spin_deg']) + corners = np.stack([ + -x_dir * half + y_dir * half, + x_dir * half + y_dir * half, + x_dir * half - y_dir * half, + -x_dir * half - y_dir * half + ], axis=0) + return marker['position_m'].reshape(1, 3) + corners + + +def build_link_chain(robot: Dict[str, Any]) -> List[str]: + links = robot.get('links', {}) or {} + ordered: List[str] = [] + remaining = set(links.keys()) + + while remaining: + progress = False + for name in list(remaining): + parent = links[name].get('parent') + if not parent or parent in ordered: + ordered.append(name) + remaining.remove(name) + progress = True + if not progress: + raise RuntimeError('Cycle detected in robot link tree or missing parent link') + return ordered + + +def compute_link_transforms(robot: Dict[str, Any], state: Dict[str, float], scale: float) -> Dict[str, np.ndarray]: + links = robot.get('links', {}) or {} + ordered_links = build_link_chain(robot) + transforms: Dict[str, np.ndarray] = {} + + for link_name in ordered_links: + link_info = links[link_name] or {} + parent_name = link_info.get('parent') + parent_transform = transforms[parent_name] if parent_name else np.eye(4, dtype=np.float64) + + mount_translation = np.asarray(resolve_vector(link_info.get('mountPosition', [0, 0, 0]), 3), dtype=np.float64) * scale + mount = transform_from_translation_rotation( + mount_translation, + link_info.get('mountRotation', [0, 0, 0]) + ) + + joint_info = link_info.get('jointToParent', {}) or {} + joint_origin = np.asarray(resolve_vector(joint_info.get('origin', [0, 0, 0]), 3), dtype=np.float64) * scale + joint = transform_from_translation_rotation( + joint_origin, + joint_info.get('rotation', [0, 0, 0]) + ) + + motion = np.eye(4, dtype=np.float64) + joint_type = str(joint_info.get('type', 'fixed')).strip().lower() + control_var = str(joint_info.get('variable', joint_info.get('control', ''))).strip().lower() + axis = resolve_vector(joint_info.get('axis', [1, 0, 0]), 3) + + if joint_type == 'linear': + motion[:3, 3] = normalize_axis(axis) * state.get(control_var, 0.0) * scale + elif joint_type == 'revolute': + motion = axis_angle_matrix(axis, state.get(control_var, 0.0)) + + transforms[link_name] = parent_transform @ mount @ joint @ motion + + return transforms + + +def compute_marker_world_position(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = np.ones(4, dtype=np.float64) + local[:3] = marker['position_m'] + world = link_transform @ local + return world[:3] + + +# ------------------------------------------------------------------ +# Camera / observation helpers +# ------------------------------------------------------------------ + +def load_intrinsics(detection_json: Dict[str, Any]) -> Tuple[np.ndarray, np.ndarray]: + cam = detection_json['camera'] + K = np.asarray(cam['camera_matrix'], dtype=np.float64) + D = np.asarray(cam.get('distortion_coefficients', [0, 0, 0, 0, 0]), dtype=np.float64).reshape(-1, 1) + return K, D + + +def collect_views_and_observations( + detection_files: List[str], + robot_markers: Dict[int, Dict[str, Any]] +) -> Tuple[List[Dict[str, Any]], List[Dict[str, Any]]]: + views: List[Dict[str, Any]] = [] + observations: List[Dict[str, Any]] = [] + + for idx, det_path in enumerate(detection_files): + detection_json = load_json(det_path) + K, D = load_intrinsics(detection_json) + views.append({ + 'index': idx, + 'source_file': os.path.abspath(det_path), + 'camera_id': detection_json.get('camera', {}).get('camera_id', f'cam{idx+1}'), + 'image_file': detection_json.get('image', {}).get('image_file'), + 'K': K, + 'D': D + }) + + for det in detection_json.get('detections', []) or []: + marker_id = int(det.get('marker_id', -1)) + if marker_id < 0 or marker_id not in robot_markers: + continue + + image_points = det.get('image_points_px') + if isinstance(image_points, list) and len(image_points) == 4: + image_points = np.asarray(image_points, dtype=np.float64) + else: + center = resolve_vector(det.get('center_px', [0, 0]), 2) + image_points = np.asarray([center], dtype=np.float64) + + confidence = float(det.get('confidence', 1.0)) + marker = robot_markers[marker_id] + observations.append({ + 'view_index': idx, + 'marker_id': marker_id, + 'marker_link_corners': marker_object_corners(marker), + 'image_points_px': image_points, + 'confidence': max(0.01, min(1.0, confidence)) + }) + + if len(views) == 0: + raise RuntimeError('No valid detection views found') + + if len(observations) == 0: + raise RuntimeError('No marker observations matched robot.json markers') + + return views, observations + + +def compute_marker_world_corners(marker: Dict[str, Any], link_transforms: Dict[str, np.ndarray]) -> np.ndarray: + link_transform = link_transforms[marker['link_name']] + local = marker_object_corners(marker) + homogeneous = np.concatenate([local, np.ones((local.shape[0], 1), dtype=np.float64)], axis=1) + world = (link_transform @ homogeneous.T).T + return world[:, :3] + + +def initial_camera_guess( + view: Dict[str, Any], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + default_state: Dict[str, float], + scale: float, + robot: Dict[str, Any] +) -> Tuple[np.ndarray, np.ndarray]: + object_points = [] + image_points = [] + + link_transforms = compute_link_transforms(robot, default_state, scale) + + for obs in observations: + if obs['view_index'] != view['index']: + continue + marker = robot_markers[obs['marker_id']] + object_points.append(compute_marker_world_corners(marker, link_transforms)) + image_points.append(obs['image_points_px']) + + if len(object_points) == 0: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + object_points = np.vstack(object_points) + image_points = np.vstack(image_points) + + if object_points.shape[0] < 4: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + success, rvec, tvec = cv2.solvePnP( + object_points, + image_points, + view['K'], + view['D'], + flags=cv2.SOLVEPNP_ITERATIVE + ) + + if not success: + return np.zeros((3, 1), dtype=np.float64), np.array([[0.0], [0.0], [1.0]], dtype=np.float64) + + return rvec, tvec + + +def project_points( + points_3d: np.ndarray, + rvec: np.ndarray, + tvec: np.ndarray, + K: np.ndarray, + D: np.ndarray +) -> np.ndarray: + projected, _ = cv2.projectPoints(points_3d, rvec, tvec, K, D) + return projected.reshape(-1, 2) + + +# ------------------------------------------------------------------ +# Optimization +# ------------------------------------------------------------------ + +def pack_parameters(robot_state: Dict[str, float], camera_params: List[Tuple[np.ndarray, np.ndarray]]) -> np.ndarray: + state_vec = np.asarray([robot_state[k] for k in STATE_KEYS], dtype=np.float64) + cams = [] + for rvec, tvec in camera_params: + cams.append(rvec.reshape(3)) + cams.append(tvec.reshape(3)) + return np.concatenate([state_vec] + cams) + + +def unpack_parameters(params: np.ndarray, n_views: int) -> Tuple[Dict[str, float], List[Tuple[np.ndarray, np.ndarray]]]: + robot_state = {STATE_KEYS[i]: float(params[i]) for i in range(len(STATE_KEYS))} + camera_params = [] + offset = len(STATE_KEYS) + for _ in range(n_views): + rvec = params[offset:offset + 3].reshape(3, 1) + tvec = params[offset + 3:offset + 6].reshape(3, 1) + camera_params.append((rvec, tvec)) + offset += 6 + return robot_state, camera_params + + +def residuals_for_parameters( + params: np.ndarray, + views: List[Dict[str, Any]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + robot: Dict[str, Any], + scale: float, + default_state: Dict[str, float] +) -> np.ndarray: + robot_state, camera_params = unpack_parameters(params, len(views)) + link_transforms = compute_link_transforms(robot, robot_state, scale) + + residuals = [] + for obs in observations: + marker = robot_markers[obs['marker_id']] + world_corners = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(world_corners, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + weight = math.sqrt(obs['confidence']) + residuals.extend((diffs * weight).reshape(-1)) + + for key in STATE_KEYS: + diff = robot_state[key] - default_state.get(key, 0.0) + if key in ('x', 'y', 'z', 'e'): + w = 0.001 + else: + w = 0.01 + residuals.append(diff * w) + + return np.asarray(residuals, dtype=np.float64) + + +def estimate_uncertainty(result: Any, n_params: int) -> np.ndarray: + if result.jac is None: + return np.full(n_params, float('nan'), dtype=np.float64) + J = result.jac + m, n = J.shape + JTJ = J.T @ J + try: + cov = np.linalg.pinv(JTJ) + except np.linalg.LinAlgError: + cov = np.linalg.pinv(JTJ + np.eye(n) * 1e-9) + residuals = result.fun + dof = max(1, m - n) + sigma2 = float(np.sum(residuals ** 2) / dof) + cov *= sigma2 + return np.sqrt(np.diag(cov)) + + +# ------------------------------------------------------------------ +# Output assembly +# ------------------------------------------------------------------ + +def camera_position_world(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray: + R, _ = cv2.Rodrigues(rvec) + return (-R.T @ tvec).reshape(3) + + +def build_output( + robot_state: Dict[str, float], + state_uncertainty: np.ndarray, + views: List[Dict[str, Any]], + camera_params: List[Tuple[np.ndarray, np.ndarray]], + observations: List[Dict[str, Any]], + robot_markers: Dict[int, Dict[str, Any]], + scale: float, + robot: Dict[str, Any], + robot_json_path: str +) -> Dict[str, Any]: + link_transforms = compute_link_transforms(robot, robot_state, scale) + + marker_summary: Dict[int, Dict[str, Any]] = {} + for marker_id, marker in robot_markers.items(): + marker_summary[marker_id] = { + 'marker_id': marker_id, + 'link_name': marker['link_name'], + 'position_world_m': compute_marker_world_position(marker, link_transforms).tolist(), + 'size_m': marker['size_m'], + 'observation_count': 0, + 'mean_confidence': None, + 'mean_reprojection_error_px': None, + 'observations': [] + } + + per_marker_errors: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + per_marker_confidences: Dict[int, List[float]] = {mid: [] for mid in marker_summary} + + link_transforms = compute_link_transforms(robot, robot_state, scale) + for obs in observations: + marker_id = obs['marker_id'] + marker = robot_markers[marker_id] + object_points_m = compute_marker_world_corners(marker, link_transforms) + rvec, tvec = camera_params[obs['view_index']] + proj = project_points(object_points_m, rvec, tvec, views[obs['view_index']]['K'], views[obs['view_index']]['D']) + diffs = proj - obs['image_points_px'] + errors = np.linalg.norm(diffs, axis=1) + repro_error = float(np.mean(errors)) + per_marker_errors[marker_id].extend(errors.tolist()) + per_marker_confidences[marker_id].append(obs['confidence']) + marker_summary[marker_id]['observation_count'] += 1 + marker_summary[marker_id]['observations'].append({ + 'view_index': obs['view_index'], + 'source_file': views[obs['view_index']]['source_file'], + 'image_file': views[obs['view_index']]['image_file'], + 'confidence': obs['confidence'], + 'mean_reprojection_error_px': repro_error, + 'corner_reprojection_errors_px': errors.tolist() + }) + + for marker_id, summary in marker_summary.items(): + if summary['observation_count'] > 0: + summary['mean_confidence'] = float(np.mean(per_marker_confidences[marker_id])) + summary['mean_reprojection_error_px'] = float(np.mean(per_marker_errors[marker_id])) + + camera_outputs = [] + for idx, view in enumerate(views): + rvec, tvec = camera_params[idx] + cam_pos = camera_position_world(rvec, tvec) + observed_count = sum(1 for obs in observations if obs['view_index'] == idx) + camera_outputs.append({ + 'view_index': idx, + 'source_file': view['source_file'], + 'camera_id': view['camera_id'], + 'camera_position_world_m': cam_pos.tolist(), + 'rvec': rvec.reshape(-1).tolist(), + 'tvec': tvec.reshape(-1).tolist(), + 'intrinsics': { + 'camera_matrix': view['K'].tolist(), + 'distortion_coefficients': view['D'].reshape(-1).tolist() + }, + 'observation_count': observed_count + }) + + robot_pose_output = { + 'state': {k: float(robot_state[k]) for k in STATE_KEYS}, + 'uncertainty': { + 'x_mm': float(state_uncertainty[0]), + 'y_mm': float(state_uncertainty[1]), + 'z_mm': float(state_uncertainty[2]), + 'a_deg': float(state_uncertainty[3]), + 'b_deg': float(state_uncertainty[4]), + 'c_deg': float(state_uncertainty[5]), + 'e_mm': float(state_uncertainty[6]) + }, + 'confidence': { + 'x': float(math.exp(-state_uncertainty[0] / 10.0)), + 'y': float(math.exp(-state_uncertainty[1] / 10.0)), + 'z': float(math.exp(-state_uncertainty[2] / 10.0)), + 'a': float(math.exp(-state_uncertainty[3] / 10.0)), + 'b': float(math.exp(-state_uncertainty[4] / 10.0)), + 'c': float(math.exp(-state_uncertainty[5] / 10.0)), + 'e': float(math.exp(-state_uncertainty[6] / max(1.0, state_uncertainty[6]))) + } + } + + return { + 'schema_version': '1.0', + 'created_utc': datetime.datetime.utcnow().isoformat() + 'Z', + 'source_robot_json': os.path.abspath(robot_json_path), + 'source_detections': [view['source_file'] for view in views], + 'robot_pose': robot_pose_output, + 'camera_poses': camera_outputs, + 'marker_positions': list(marker_summary.values()) + } + + +# ------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------ + +def main() -> None: + parser = argparse.ArgumentParser(description='Multiview optimization of robot pose and camera extrinsics') + parser.add_argument('--robot', required=True, help='Path to robot.json') + parser.add_argument('--detections', required=True, nargs='+', help='List of detection JSON files') + parser.add_argument('--outDir', required=True, help='Output directory') + parser.add_argument('--write-summary', action='store_true', help='Write summary file') + parser.add_argument('--max-iter', type=int, default=500, help='Maximum optimizer iterations') + args = parser.parse_args() + + os.makedirs(args.outDir, exist_ok=True) + + robot_json_path = os.path.abspath(args.robot) + robot = load_json(robot_json_path) + scale = parse_metric_scale(robot) + + default_state = { + k: float(robot.get('defaultPosition', {}).get(k, 0.0) or 0.0) + for k in STATE_KEYS + } + + robot_markers = extract_markers(robot, scale) + views, observations = collect_views_and_observations(args.detections, robot_markers) + + camera_guesses = [] + for view in views: + rvec, tvec = initial_camera_guess(view, observations, robot_markers, default_state, scale, robot) + camera_guesses.append((rvec, tvec)) + + x0 = pack_parameters(default_state, camera_guesses) + + progress = { + 'iter': 0, + 'last_cost': None, + 'last_print': time.time(), + 'prev_x': x0.copy() + } + + def progress_callback(xk: np.ndarray) -> None: + progress['iter'] += 1 + now = time.time() + if progress['iter'] == 1 or now - progress['last_print'] >= 1.0: + res = residuals_for_parameters(xk, views, observations, robot_markers, robot, scale, default_state) + cost = 0.5 * float(np.dot(res, res)) + delta_cost = None + convergence = '' + if progress['last_cost'] is not None: + delta_cost = cost - progress['last_cost'] + if abs(delta_cost) < 1e-3: + convergence = ' stable' + elif delta_cost < 0: + convergence = ' improving' + else: + convergence = ' worsening' + step_norm = float(np.linalg.norm(xk - progress['prev_x'])) + print( + f'[Multiview] iter={progress["iter"]:4d} cost={cost:.4f}' + + (f' delta={delta_cost:.4g}' if delta_cost is not None else '') + + f' step={step_norm:.4g}' + + convergence + ) + progress['last_cost'] = cost + progress['last_print'] = now + progress['prev_x'] = xk.copy() + + result = least_squares( + residuals_for_parameters, + x0, + args=(views, observations, robot_markers, robot, scale, default_state), + jac='2-point', + method='trf', + loss='soft_l1', + f_scale=1.0, + max_nfev=args.max_iter, + callback=progress_callback + ) + + robot_state, camera_params = unpack_parameters(result.x, len(views)) + uncertainties = estimate_uncertainty(result, len(result.x)) + + output = build_output(robot_state, uncertainties[:len(STATE_KEYS)], views, camera_params, observations, robot_markers, scale, robot, robot_json_path) + + out_path = Path(args.outDir) / 'multiview_pose.json' + save_json(output, out_path) + + print(f'Saved: {out_path}') + if args.write_summary: + summary_path = Path(args.outDir) / 'multiview_pose_summary.json' + summary = { + 'final_cost': float(result.cost), + 'status': int(result.status), + 'message': result.message, + 'robot_state': output['robot_pose'], + 'camera_count': len(views), + 'marker_count': len(robot_markers) + } + save_json(summary, summary_path) + print(f'Saved: {summary_path}') + + +if __name__ == '__main__': + main() diff --git a/pipeline/__pycache__/2_Multiview.cpython-311.pyc b/pipeline/__pycache__/2_Multiview.cpython-311.pyc new file mode 100644 index 0000000..4d7fddc Binary files /dev/null and b/pipeline/__pycache__/2_Multiview.cpython-311.pyc differ diff --git a/pipeline/multiview_pose.json b/pipeline/multiview_pose.json new file mode 100644 index 0000000..f40d99b --- /dev/null +++ b/pipeline/multiview_pose.json @@ -0,0 +1,966 @@ +{ + "schema_version": "1.0", + "created_utc": "2026-05-28T15:12:17.494654Z", + "source_robot_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\robot.json", + "source_detections": [ + "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json" + ], + "robot_pose": { + "state": { + "x": 181.54725640332114, + "y": 58.52839894680379, + "z": -117.49574729476373, + "a": -109.04068037196633, + "b": 21.99999999999997, + "c": 90.9999999999968, + "e": 10.000000000002045 + }, + "uncertainty": { + "x_mm": 1244.466529684963, + "y_mm": 7074.339663399324, + "z_mm": 820.6981657593474, + "a_deg": 826.28528886627, + "b_deg": 10766.558819701313, + "c_deg": 10766.563026961136, + "e_mm": 107665.71839050233 + }, + "confidence": { + "x": 8.984736077953398e-55, + "y": 5.825485258915721e-308, + "z": 2.2778836127227612e-36, + "a": 1.3028243192541689e-36, + "b": 0.0, + "c": 0.0, + "e": 0.36787944117144233 + } + }, + "camera_poses": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "camera_id": "cam1", + "camera_position_world_m": [ + -67.15264946093049, + -145.95139032151744, + -88.26653224765123 + ], + "rvec": [ + -3.542038128451333, + -30.5589850821012, + 16.444347919278478 + ], + "tvec": [ + 1.483017282919796, + 7.509205723706149, + -183.14933761979546 + ], + "intrinsics": { + "camera_matrix": [ + [ + 1777.77783203125, + 0.0, + 640.0 + ], + [ + 0.0, + 1500.0, + 360.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "distortion_coefficients": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "observation_count": 17 + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "camera_id": "cam1", + "camera_position_world_m": [ + 0.08505368964340965, + 0.2946432117677963, + -0.7157756836568655 + ], + "rvec": [ + -0.5316255763965235, + -3.174734119797076, + -0.7980749872854376 + ], + "tvec": [ + 0.17598061076211097, + 0.03209794584304647, + -0.7578813417585265 + ], + "intrinsics": { + "camera_matrix": [ + [ + 1777.77783203125, + 0.0, + 640.0 + ], + [ + 0.0, + 1500.0, + 360.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "distortion_coefficients": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "observation_count": 10 + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "camera_id": "cam1", + "camera_position_world_m": [ + 0.19551444508455482, + -0.4496621583583861, + 0.7007419670032374 + ], + "rvec": [ + -3.5782847330305145, + 0.5811345580146894, + 0.31790917187918544 + ], + "tvec": [ + -0.13076078659741264, + 0.0026732045097548752, + 0.8451956754988071 + ], + "intrinsics": { + "camera_matrix": [ + [ + 1777.77783203125, + 0.0, + 640.0 + ], + [ + 0.0, + 1500.0, + 360.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "distortion_coefficients": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "observation_count": 7 + } + ], + "marker_positions": [ + { + "marker_id": 210, + "link_name": "Board", + "position_world_m": [ + 0.02, + -0.02, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 3, + "mean_confidence": 0.6997700579082148, + "mean_reprojection_error_px": 220.77947555807535, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.5673043275049208, + "mean_reprojection_error_px": 583.5059207989476, + "corner_reprojection_errors_px": [ + 609.2234772782313, + 565.0260802933998, + 557.8015857749582, + 601.9725398492011 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.7813266383036261, + "mean_reprojection_error_px": 39.55489478242865, + "corner_reprojection_errors_px": [ + 48.88397493699652, + 61.30605038320363, + 46.048597197902666, + 1.980956611611768 + ] + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.7506792079160973, + "mean_reprojection_error_px": 39.27761109284978, + "corner_reprojection_errors_px": [ + 65.97074853665205, + 24.74479058079261, + 13.228233029506784, + 53.166672224447694 + ] + } + ] + }, + { + "marker_id": 211, + "link_name": "Board", + "position_world_m": [ + 0.25, + -0.01, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.6412317666518965, + "mean_reprojection_error_px": 214.33163264388395, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.6412317666518965, + "mean_reprojection_error_px": 214.33163264388395, + "corner_reprojection_errors_px": [ + 237.47818589215711, + 202.2967964175736, + 191.68363751699303, + 225.86791074881205 + ] + } + ] + }, + { + "marker_id": 215, + "link_name": "Board", + "position_world_m": [ + 0.25, + -0.09, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 2, + "mean_confidence": 0.8108527003549935, + "mean_reprojection_error_px": 195.42303307205083, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.7952557920267838, + "mean_reprojection_error_px": 205.66294162020296, + "corner_reprojection_errors_px": [ + 218.8179432625852, + 185.3722132441409, + 193.9241697940935, + 224.5374401799923 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.826449608683203, + "mean_reprojection_error_px": 185.1831245238987, + "corner_reprojection_errors_px": [ + 219.21341536613414, + 168.97047202229555, + 151.2474816881201, + 201.30112901904496 + ] + } + ] + }, + { + "marker_id": 214, + "link_name": "Board", + "position_world_m": [ + 0.35000000000000003, + -0.01, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.6041252446922665, + "mean_reprojection_error_px": 396.6842335066294, + "observations": [ + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.6041252446922665, + "mean_reprojection_error_px": 396.6842335066294, + "corner_reprojection_errors_px": [ + 396.3517608479836, + 374.63031408080843, + 399.6139298462295, + 416.1409292514962 + ] + } + ] + }, + { + "marker_id": 208, + "link_name": "Board", + "position_world_m": [ + 0.35000000000000003, + -0.09, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.6280424706698967, + "mean_reprojection_error_px": 104.53888708998042, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.6280424706698967, + "mean_reprojection_error_px": 104.53888708998042, + "corner_reprojection_errors_px": [ + 100.19696760041806, + 84.14272558467785, + 111.5985938652804, + 122.21726130954538 + ] + } + ] + }, + { + "marker_id": 206, + "link_name": "Board", + "position_world_m": [ + 0.65, + -0.01, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.33820423087105295, + "mean_reprojection_error_px": 267.0425486050223, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.33820423087105295, + "mean_reprojection_error_px": 267.0425486050223, + "corner_reprojection_errors_px": [ + 248.43466760822997, + 272.69005756404255, + 285.6169240254765, + 261.42854522234035 + ] + } + ] + }, + { + "marker_id": 205, + "link_name": "Board", + "position_world_m": [ + 0.75, + -0.09, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.28724459014346065, + "mean_reprojection_error_px": 415.8751776130689, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.28724459014346065, + "mean_reprojection_error_px": 415.8751776130689, + "corner_reprojection_errors_px": [ + 395.1459522897165, + 416.72497517179465, + 436.46281469338743, + 415.16696829737697 + ] + } + ] + }, + { + "marker_id": 207, + "link_name": "Board", + "position_world_m": [ + 0.75, + -0.01, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.27228561618555186, + "mean_reprojection_error_px": 363.40455932725575, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.27228561618555186, + "mean_reprojection_error_px": 363.40455932725575, + "corner_reprojection_errors_px": [ + 346.0438581309408, + 367.42169729635515, + 381.0737741735404, + 359.07890770818653 + ] + } + ] + }, + { + "marker_id": 217, + "link_name": "Board", + "position_world_m": [ + 0.65, + -0.09, + 0.0003 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.35596573288593486, + "mean_reprojection_error_px": 321.69325248463235, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.35596573288593486, + "mean_reprojection_error_px": 321.69325248463235, + "corner_reprojection_errors_px": [ + 299.50652112481214, + 323.01445781467953, + 343.8145434727812, + 320.4374875262566 + ] + } + ] + }, + { + "marker_id": 198, + "link_name": "Arm1", + "position_world_m": [ + 0.18154725640332114, + -0.05368067525881935, + 0.15473650216984092 + ], + "size_m": 0.025, + "observation_count": 2, + "mean_confidence": 0.16652042240877096, + "mean_reprojection_error_px": 176.61396206576376, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.20150745250271476, + "mean_reprojection_error_px": 249.4432410246182, + "corner_reprojection_errors_px": [ + 275.2716341467862, + 247.12753771344126, + 224.03573860709332, + 251.33805363115204 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.13153339231482716, + "mean_reprojection_error_px": 103.78468310690936, + "corner_reprojection_errors_px": [ + 92.57065651057769, + 103.81498423159927, + 124.48400230916597, + 94.26908937629445 + ] + } + ] + }, + { + "marker_id": 229, + "link_name": "Arm1", + "position_world_m": [ + 0.18154725640332114, + -0.10066750491630294, + 0.23149741565280188 + ], + "size_m": 0.025, + "observation_count": 2, + "mean_confidence": 0.2274747191640703, + "mean_reprojection_error_px": 114.65083791461562, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.26810902547334975, + "mean_reprojection_error_px": 157.56342751488188, + "corner_reprojection_errors_px": [ + 182.03382946831127, + 158.99106757645583, + 133.31281060220186, + 155.91600241255856 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.18684041285479083, + "mean_reprojection_error_px": 71.73824831434936, + "corner_reprojection_errors_px": [ + 62.57648563021246, + 73.24833068956802, + 93.62951004804361, + 57.49866688957334 + ] + } + ] + }, + { + "marker_id": 242, + "link_name": "Arm1", + "position_world_m": [ + 0.18154725640332114, + -0.1603704376252726, + 0.19495210369698132 + ], + "size_m": 0.025, + "observation_count": 0, + "mean_confidence": null, + "mean_reprojection_error_px": null, + "observations": [] + }, + { + "marker_id": 243, + "link_name": "Arm1", + "position_world_m": [ + 0.18154725640332114, + -0.14879162724869807, + 0.24307622602937645 + ], + "size_m": 0.025, + "observation_count": 3, + "mean_confidence": 0.9174550709990235, + "mean_reprojection_error_px": 43.36361596374159, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.8899728634608616, + "mean_reprojection_error_px": 80.72636248080838, + "corner_reprojection_errors_px": [ + 103.11767577441192, + 73.99244264213212, + 52.960515033680814, + 92.83481647300867 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.9132502955127223, + "mean_reprojection_error_px": 17.65321921352932, + "corner_reprojection_errors_px": [ + 23.787325274905296, + 20.61800212862162, + 10.499732762361031, + 15.707816688229324 + ] + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.9491420540234864, + "mean_reprojection_error_px": 31.711266196887063, + "corner_reprojection_errors_px": [ + 37.923084099569614, + 0.2134084559141012, + 42.92388177593928, + 45.78469045612526 + ] + } + ] + }, + { + "marker_id": 244, + "link_name": "Ellbow", + "position_world_m": [ + 0.30654725640332114, + 0.0, + 0.0 + ], + "size_m": 0.025, + "observation_count": 0, + "mean_confidence": null, + "mean_reprojection_error_px": null, + "observations": [] + }, + { + "marker_id": 245, + "link_name": "Ellbow", + "position_world_m": [ + 0.2715472564033211, + 0.029990577828141386, + -0.018043426546368473 + ], + "size_m": 0.025, + "observation_count": 0, + "mean_confidence": null, + "mean_reprojection_error_px": null, + "observations": [] + }, + { + "marker_id": 246, + "link_name": "Ellbow", + "position_world_m": [ + 0.2715472564033211, + -0.029990577828141386, + 0.018043426546368473 + ], + "size_m": 0.025, + "observation_count": 3, + "mean_confidence": 0.6792447690577089, + "mean_reprojection_error_px": 82.08966057312311, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.8015179238063419, + "mean_reprojection_error_px": 130.56180963001174, + "corner_reprojection_errors_px": [ + 131.5768861165912, + 148.78099370016497, + 133.10414547000457, + 108.78521323328613 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.7581700566520715, + "mean_reprojection_error_px": 38.83288809109914, + "corner_reprojection_errors_px": [ + 49.917614687022564, + 21.338048175631457, + 40.862399000804004, + 43.213490500938555 + ] + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.4780463267147134, + "mean_reprojection_error_px": 76.8742839982585, + "corner_reprojection_errors_px": [ + 35.05794226352506, + 98.03823312377735, + 122.08217872702849, + 52.31878187870311 + ] + } + ] + }, + { + "marker_id": 247, + "link_name": "Ellbow", + "position_world_m": [ + 0.23404725640332114, + -0.029990577828141386, + 0.018043426546368473 + ], + "size_m": 0.025, + "observation_count": 3, + "mean_confidence": 0.7117000257529918, + "mean_reprojection_error_px": 80.52058124342345, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.8796777163094818, + "mean_reprojection_error_px": 109.47051531989443, + "corner_reprojection_errors_px": [ + 125.73431013214108, + 126.73685991781507, + 97.3154475708235, + 88.09544365879812 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.7448005120854795, + "mean_reprojection_error_px": 40.6548721647999, + "corner_reprojection_errors_px": [ + 61.62678961364609, + 49.84332916101471, + 31.2644086226518, + 19.884961261886986 + ] + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.5106218488640142, + "mean_reprojection_error_px": 91.436356245576, + "corner_reprojection_errors_px": [ + 79.2467696422458, + 112.40480811674038, + 113.18153184339867, + 60.91231537991913 + ] + } + ] + }, + { + "marker_id": 124, + "link_name": "Arm2", + "position_world_m": [ + 0.19296563528466518, + -0.14125000632909357, + -0.1705991100086782 + ], + "size_m": 0.025, + "observation_count": 2, + "mean_confidence": 0.4106676690676095, + "mean_reprojection_error_px": 118.70435848549427, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.34091855704160245, + "mean_reprojection_error_px": 136.41410315507122, + "corner_reprojection_errors_px": [ + 122.8029119076584, + 105.17647879194196, + 150.3388262656519, + 167.33819565503265 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.4804167810936165, + "mean_reprojection_error_px": 100.99461381591732, + "corner_reprojection_errors_px": [ + 94.61023538629438, + 62.3507552530323, + 115.30488690538117, + 131.71257771896146 + ] + } + ] + }, + { + "marker_id": 122, + "link_name": "Arm2", + "position_world_m": [ + 0.20990587185770623, + -0.0854394397428592, + -0.1609965560685589 + ], + "size_m": 0.025, + "observation_count": 3, + "mean_confidence": 0.5690911450460799, + "mean_reprojection_error_px": 115.4296640273784, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.18665602922528673, + "mean_reprojection_error_px": 192.69670051979531, + "corner_reprojection_errors_px": [ + 162.8594058395046, + 189.31139967251238, + 220.7675955032362, + 197.8484010639281 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.9456847621099602, + "mean_reprojection_error_px": 41.758821983395634, + "corner_reprojection_errors_px": [ + 42.20929173703953, + 73.24612974199405, + 34.08501995181812, + 17.49484650273085 + ] + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.5749326438029929, + "mean_reprojection_error_px": 111.8334695789442, + "corner_reprojection_errors_px": [ + 122.40335309391429, + 155.4380168814154, + 101.35617142524032, + 68.13633691520678 + ] + } + ] + }, + { + "marker_id": 218, + "link_name": "Arm2", + "position_world_m": [ + 0.1701288775219771, + -0.029389256152276798, + -0.113026068880316 + ], + "size_m": 0.025, + "observation_count": 0, + "mean_confidence": null, + "mean_reprojection_error_px": null, + "observations": [] + }, + { + "marker_id": 101, + "link_name": "Arm2", + "position_world_m": [ + 0.15007697358736016, + -0.08069728605993415, + -0.16384960885532981 + ], + "size_m": 0.025, + "observation_count": 1, + "mean_confidence": 0.3325004465415643, + "mean_reprojection_error_px": 268.2779246523214, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.3325004465415643, + "mean_reprojection_error_px": 268.2779246523214, + "corner_reprojection_errors_px": [ + 252.5657721019858, + 236.30225155702897, + 285.07051528039955, + 299.1731596698715 + ] + } + ] + }, + { + "marker_id": 102, + "link_name": "Arm2", + "position_world_m": [ + 0.16622582371954672, + -0.1207918742144217, + -0.13972724080967133 + ], + "size_m": 0.025, + "observation_count": 3, + "mean_confidence": 0.8473641170835915, + "mean_reprojection_error_px": 134.3212111638231, + "observations": [ + { + "view_index": 0, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", + "confidence": 0.9494437061622057, + "mean_reprojection_error_px": 275.82196003309537, + "corner_reprojection_errors_px": [ + 239.6365759195386, + 266.3494158086631, + 310.221043133295, + 287.0808052708849 + ] + }, + { + "view_index": 1, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", + "confidence": 0.8913851020933449, + "mean_reprojection_error_px": 69.70421030419618, + "corner_reprojection_errors_px": [ + 43.54332574941685, + 33.67288896070401, + 97.392314014777, + 104.20831249188686 + ] + }, + { + "view_index": 2, + "source_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", + "image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", + "confidence": 0.7012635429952238, + "mean_reprojection_error_px": 57.43746315417774, + "corner_reprojection_errors_px": [ + 75.89223182434904, + 45.05384333908305, + 33.595730720813556, + 75.20804673246533 + ] + } + ] + }, + { + "marker_id": 219, + "link_name": "Arm2", + "position_world_m": [ + 0.1701288775219771, + -0.08455058873688898, + -0.20471154966920532 + ], + "size_m": 0.025, + "observation_count": 0, + "mean_confidence": null, + "mean_reprojection_error_px": null, + "observations": [] + } + ] +} \ No newline at end of file diff --git a/pipeline/multiview_pose_summary.json b/pipeline/multiview_pose_summary.json new file mode 100644 index 0000000..bfc954b --- /dev/null +++ b/pipeline/multiview_pose_summary.json @@ -0,0 +1,36 @@ +{ + "final_cost": 25086.49759096419, + "status": 0, + "message": "The maximum number of function evaluations is exceeded.", + "robot_state": { + "state": { + "x": 100.74147204793623, + "y": 29.650679871841962, + "z": -35.519338413527834, + "a": -119.76253895286006, + "b": 22.000000000000124, + "c": 91.00000000000006, + "e": 9.999999999999991 + }, + "uncertainty": { + "x_mm": 5958.291819057341, + "y_mm": 1161.4246555312698, + "z_mm": 548.5614212936815, + "a_deg": 3375.9445110397996, + "b_deg": 13762.367585907954, + "c_deg": 13762.368814943879, + "e_mm": 137623.68210441121 + }, + "confidence": { + "x": 1.7166198945166287e-259, + "y": 3.630513831255408e-51, + "z": 1.5006526335068334e-24, + "a": 2.424335742250214e-147, + "b": 0.0, + "c": 0.0, + "e": 0.36787944117144233 + } + }, + "camera_count": 3, + "marker_count": 23 +} \ No newline at end of file diff --git a/pipeline/render_1a_aruco_detection.json b/pipeline/render_1a_aruco_detection.json index 24b1a5f..36ec0d1 100644 --- a/pipeline/render_1a_aruco_detection.json +++ b/pipeline/render_1a_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T14:19:26Z", + "created_utc": "2026-05-28T15:11:26Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "e42f5548-1d19-4c52-95b2-8bf325ee6c9d", + "observation_id": "a0864373-2016-4c0c-be4d-c8b66b185bd7", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.9494437061622057 }, { - "observation_id": "aac1b8f3-148b-4581-85f3-f61aeb42e46f", + "observation_id": "2d237a4b-33db-4a97-aad0-2b69922e5c27", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.8899728634608616 }, { - "observation_id": "abc1f47b-193a-4dc8-a7b0-27bfdbb0b05d", + "observation_id": "a01227d1-d980-435e-ad0f-453789a6e941", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.5673043275049208 }, { - "observation_id": "37d67d6b-080d-4fe5-8d41-01529b86f337", + "observation_id": "8044b2a7-1af6-457d-b112-8566c669e961", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.8796777163094818 }, { - "observation_id": "3de53c1a-8522-488d-b41a-7aeee4806ca7", + "observation_id": "dac561f0-1939-438d-9ed1-5d057d5132dd", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.8015179238063419 }, { - "observation_id": "e1a0ab10-e50b-4a82-b411-82fb981162bf", + "observation_id": "33c3c11c-f7d2-4038-ac2c-e4332bcf5b79", "type": "aruco", "marker_id": 101, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.3325004465415643 }, { - "observation_id": "c920e3b3-46df-42ee-8131-d89c5b52a02d", + "observation_id": "3ae8ec72-f806-457c-8c60-403428368c0a", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.7952557920267838 }, { - "observation_id": "795ff943-1c0e-4689-a441-3a854d550c49", + "observation_id": "755f017d-cb90-461d-bd57-25768e592696", "type": "aruco", "marker_id": 124, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.34091855704160245 }, { - "observation_id": "4bf69404-e235-4ae6-9f9a-9c63e2110464", + "observation_id": "45854ae6-7ba9-4906-87a6-2c7d9356cb85", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.26810902547334975 }, { - "observation_id": "d8199773-c76d-4f4b-98c5-20a73fd9a07c", + "observation_id": "de3abf0b-4ae2-4fee-9215-2039b0f5e3da", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -586,7 +586,7 @@ "confidence": 0.18665602922528673 }, { - "observation_id": "86709fde-f2c4-454d-8a8d-fa65f085b53f", + "observation_id": "5577fb36-2186-41f8-8fbf-40266f9e767c", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, @@ -640,7 +640,7 @@ "confidence": 0.20150745250271476 }, { - "observation_id": "f33274fe-ef96-4f6d-9277-2fcc92cf630f", + "observation_id": "54aec13c-2b54-49d8-a9c6-e873eb94dfef", "type": "aruco", "marker_id": 211, "marker_size_m": 0.025, @@ -694,7 +694,7 @@ "confidence": 0.6412317666518965 }, { - "observation_id": "de2af2cc-6cc7-476b-9d91-faa7cc5c6ec9", + "observation_id": "4cf08f15-d8f2-416d-8397-8a68f564d0a8", "type": "aruco", "marker_id": 208, "marker_size_m": 0.025, @@ -748,7 +748,7 @@ "confidence": 0.6280424706698967 }, { - "observation_id": "ecb219ea-1f29-47b4-bddb-ae7b6396099a", + "observation_id": "72d6524a-3710-46b2-b036-5031b7d2e648", "type": "aruco", "marker_id": 217, "marker_size_m": 0.025, @@ -802,7 +802,7 @@ "confidence": 0.35596573288593486 }, { - "observation_id": "8aa4042f-e046-4153-adef-90d26b3b2851", + "observation_id": "160f705f-87e0-41e2-90f6-b39252bff719", "type": "aruco", "marker_id": 206, "marker_size_m": 0.025, @@ -856,7 +856,7 @@ "confidence": 0.33820423087105295 }, { - "observation_id": "cfc91889-c49d-4e09-9646-d5ac0df35e0e", + "observation_id": "03b79b60-9fb5-4aa8-82b6-303911389999", "type": "aruco", "marker_id": 205, "marker_size_m": 0.025, @@ -910,7 +910,7 @@ "confidence": 0.28724459014346065 }, { - "observation_id": "70120b46-c57d-438a-893f-81c26ae4703f", + "observation_id": "bc11412b-140e-48b9-9c27-aed1d54bd4b2", "type": "aruco", "marker_id": 207, "marker_size_m": 0.025, diff --git a/pipeline/render_1a_aruco_detection_camera_pose.json b/pipeline/render_1a_aruco_detection_camera_pose.json index 88d8f12..c3d174e 100644 --- a/pipeline/render_1a_aruco_detection_camera_pose.json +++ b/pipeline/render_1a_aruco_detection_camera_pose.json @@ -1,295 +1,68 @@ { - "schema_version": "1.0", - "created_utc": "2026-05-28T14:19:28Z", - "source_detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", + "source_detection_json": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a_aruco_detection.json", "camera_pose": { - "rvec_world_to_camera": [ - -2.305496407968605, - 1.065207351740574, - 0.34998974882711126 + "camera_position_world_mm": [ + -170.9856733813686, + -471.25256740269396, + 878.0637115557477 ], - "tvec_world_to_camera_mm": [ - -315.2306259816743, - 224.77488089057, - 1383.92954200185 - ], - "R_world_to_camera": [ + "rotation_matrix_world_to_camera": [ [ - 0.6485090690393105, - -0.7612056883925573, - 0.0013738022347707657 + 0.6349401350180643, + -0.7720226356185085, + 0.028845710875162883 ], [ - -0.6120529322947699, - -0.5203636712935098, - 0.5954937931392015 + -0.5994288656515463, + -0.5158601484722065, + -0.6120239719503914 ], [ - -0.45257838596550426, - -0.38702396509357573, - -0.8033587336925552 + 0.48737671258169724, + 0.3713076316356623, + -0.7903129650475013 ] ], - "T_camera_world": [ + "rotation_matrix_camera_to_world": [ [ - 0.6485090690393105, - -0.7612056883925573, - 0.0013738022347707657, - -315.2306259816743 + 0.6349401350180643, + -0.5994288656515463, + 0.48737671258169724 ], [ - -0.6120529322947699, - -0.5203636712935098, - 0.5954937931392015, - 224.77488089057 + -0.7720226356185085, + -0.5158601484722065, + 0.3713076316356623 ], [ - -0.45257838596550426, - -0.38702396509357573, - -0.8033587336925552, - 1383.92954200185 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0 + 0.028845710875162883, + -0.6120239719503914, + -0.7903129650475013 ] ], - "R_camera_to_world": [ - [ - 0.6485090690393105, - -0.6120529322947699, - -0.45257838596550426 - ], - [ - -0.7612056883925573, - -0.5203636712935098, - -0.38702396509357573 - ], - [ - 0.0013738022347707657, - 0.5954937931392015, - -0.8033587336925552 - ] + "rvec": [ + 2.291386890593113, + -1.0684818045197042, + 0.4021828449915483 ], - "t_camera_in_world_mm": [ - 968.3406431525125, - 412.6232353376735, - 978.3729024968281 - ], - "T_world_camera": [ - [ - 0.6485090690393105, - -0.6120529322947699, - -0.45257838596550426, - 968.3406431525125 - ], - [ - -0.7612056883925573, - -0.5203636712935098, - -0.38702396509357573, - 412.6232353376735 - ], - [ - 0.0013738022347707657, - 0.5954937931392015, - -0.8033587336925552, - 978.3729024968281 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "statistics": { - "rmse_px": 53.69444650385422, - "mean_px": 43.49018862623262, - "median_px": 32.08915387366267, - "max_px": 131.0132247190388, - "per_corner_errors_px": [ - 125.9940054900667, - 105.07352467385594, - 109.87324347189022, - 131.0132247190388, - 21.010672127270144, - 30.17780962535308, - 29.741870343336966, - 21.53517675057069, - 30.674698289199373, - 37.37789632268799, - 33.503609458125965, - 26.755846339359543, - 52.6597777482743, - 55.943114202091685, - 56.41293588599435, - 53.3845318388676, - 24.140302976164055, - 12.501614463984566, - 17.843775799913118, - 27.851972943047976, - 19.55527279482321, - 15.006143006879194, - 8.168997067942962, - 18.41951241518959, - 26.644168238518596, - 43.73515602327787, - 39.76763972637787, - 23.72433018345239, - 42.69531998291903, - 60.42053547730011, - 53.73185743455883, - 36.347500219111076 - ], - "per_corner_residuals_px": [ - [ - -111.43384038596344, - 58.79616174774151 - ], - [ - -94.21398419609403, - 46.520648848501196 - ], - [ - -93.31959118739167, - 57.996409644488836 - ], - [ - -110.44277538824701, - 70.4759421066691 - ], - [ - 19.119375834118955, - -8.711934971827702 - ], - [ - 26.07607978564579, - -15.190070993800134 - ], - [ - 28.267108793747752, - -9.249292511446242 - ], - [ - 21.386189257435035, - -2.528783645053636 - ], - [ - 14.391649857517677, - -27.089066604665845 - ], - [ - 22.623064650364768, - -29.75405987987233 - ], - [ - 20.48965174053592, - -26.507848239232487 - ], - [ - 14.349578701348491, - -22.58240253890682 - ], - [ - 42.68032051979753, - -30.845460489754316 - ], - [ - 45.092250608114796, - -33.11073786135313 - ], - [ - 46.99580232025039, - -31.20599140474883 - ], - [ - 45.653152604068396, - -27.671246754794254 - ], - [ - 16.64076508745518, - -17.488257920248316 - ], - [ - 7.241353209464364, - -10.190837448506784 - ], - [ - 7.5129531818588475, - -16.185050796483836 - ], - [ - 16.94668756026067, - -22.102990240127752 - ], - [ - 19.553105326365085, - 0.2911463137773467 - ], - [ - 11.302035895770928, - 9.871591186511665 - ], - [ - 7.972569331990371, - 1.780632287325858 - ], - [ - 17.27663470149446, - -6.38720056084361 - ], - [ - -23.527458763101663, - 12.504814483751204 - ], - [ - -37.370264800659925, - 22.720633378253694 - ], - [ - -37.18919281759213, - 14.086486679897916 - ], - [ - -23.33139615759319, - 4.299976277948048 - ], - [ - -18.10794101712986, - 38.66513701726649 - ], - [ - -31.863312707890373, - 51.33585891599432 - ], - [ - -35.3592943065546, - 40.45779046747623 - ], - [ - -21.569770185610196, - 29.255525739904954 - ] - ], - "num_correspondences": 32, - "num_inliers": 12, - "used_marker_ids": [ - 205, - 206, - 207, - 208, - 210, - 211, - 215, - 217 - ] - }, - "input": { - "detection_image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png", - "camera_id": "cam1", - "marker_dictionary": "DICT_4X4_250" + "tvec": [ + -280.5803545388054, + 191.8018727459298, + 952.2592454759357 + ] + }, + "reprojection_error_px": { + "mean": 2.4146498455755063, + "max": 4.819015530727733, + "per_marker": { + "210": 2.3216790986770164, + "215": 2.074953107734691, + "211": 4.819015530727733, + "208": 1.4794063810568414, + "217": 1.782847281148554, + "206": 2.714840043046132, + "205": 2.5310207449984325, + "207": 1.593436577214649 } } } \ No newline at end of file diff --git a/pipeline/render_1a_aruco_detection_camera_pose_debug.png b/pipeline/render_1a_aruco_detection_camera_pose_debug.png new file mode 100644 index 0000000..7976699 Binary files /dev/null and b/pipeline/render_1a_aruco_detection_camera_pose_debug.png differ diff --git a/pipeline/render_1b_aruco_detection.json b/pipeline/render_1b_aruco_detection.json index e87435f..820bd35 100644 --- a/pipeline/render_1b_aruco_detection.json +++ b/pipeline/render_1b_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T14:19:27Z", + "created_utc": "2026-05-28T15:11:26Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "d4cf6b6e-663e-436a-ad4c-132053ca70cb", + "observation_id": "5f47acb3-66c2-48ba-b83b-d8e07aa2f6ec", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.8913851020933449 }, { - "observation_id": "b1f9a655-bfec-42c0-a0d3-ad3a68b9264a", + "observation_id": "4d7aea08-c7e4-45a9-8fba-c738360a4a09", "type": "aruco", "marker_id": 124, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.4804167810936165 }, { - "observation_id": "f57082f6-b122-486f-865e-a8e51e51d23a", + "observation_id": "3675fbfb-7387-4ab7-8cdd-c581dbb46a48", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.9132502955127223 }, { - "observation_id": "9624c46a-50be-4f00-92d6-9eef055042f4", + "observation_id": "f430fa70-8024-4ea7-9318-ad33908e0622", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.9456847621099602 }, { - "observation_id": "12527ff5-5df0-4959-a066-7295c6775787", + "observation_id": "3351b253-537e-4499-8114-52e7873706c4", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.7448005120854795 }, { - "observation_id": "82614732-b88e-4380-bc1c-e2313ffef74e", + "observation_id": "d39337f3-7a27-4ce7-adf0-24fbe9ac65ea", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.7581700566520715 }, { - "observation_id": "0903ff2c-f515-4ac7-8c96-b4247ff223c7", + "observation_id": "f3bd397e-3c02-4645-b43a-3f292bdf29fa", "type": "aruco", "marker_id": 215, "marker_size_m": 0.025, @@ -424,7 +424,7 @@ "confidence": 0.826449608683203 }, { - "observation_id": "53acd5d5-179a-4f73-80b7-f399b6c209e9", + "observation_id": "e76f83e2-0558-4588-bed4-de9ba1644b18", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, @@ -478,7 +478,7 @@ "confidence": 0.7813266383036261 }, { - "observation_id": "e9fd52e4-06a1-41fa-8388-c0e5faca2813", + "observation_id": "36a7eaf1-6608-450c-90c2-6a8284457c88", "type": "aruco", "marker_id": 229, "marker_size_m": 0.025, @@ -532,7 +532,7 @@ "confidence": 0.18684041285479083 }, { - "observation_id": "98334e32-87c0-40ee-84e5-745f6f4250d4", + "observation_id": "18049602-5f59-48b6-914b-cefe7203fe3b", "type": "aruco", "marker_id": 198, "marker_size_m": 0.025, diff --git a/pipeline/render_1b_aruco_detection_camera_pose.json b/pipeline/render_1b_aruco_detection_camera_pose.json deleted file mode 100644 index f09cac8..0000000 --- a/pipeline/render_1b_aruco_detection_camera_pose.json +++ /dev/null @@ -1,169 +0,0 @@ -{ - "schema_version": "1.0", - "created_utc": "2026-05-28T14:19:28Z", - "source_detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b_aruco_detection.json", - "camera_pose": { - "rvec_world_to_camera": [ - -3.4928668596759627, - -0.11465199431472019, - 0.06891346455898573 - ], - "tvec_world_to_camera_mm": [ - -218.99274893021345, - -66.7998001007427, - 991.433880749304 - ], - "R_world_to_camera": [ - [ - 0.99716158725115, - 0.07035390512768569, - -0.026815982996183867 - ], - [ - 0.0566912850549916, - -0.9359657619679411, - -0.3474970368543954 - ], - [ - -0.04954661552094863, - 0.34499046429873387, - -0.9372975581070098 - ] - ], - "T_camera_world": [ - [ - 0.99716158725115, - 0.07035390512768569, - -0.026815982996183867, - -218.99274893021345 - ], - [ - 0.0566912850549916, - -0.9359657619679411, - -0.3474970368543954, - -66.7998001007427 - ], - [ - -0.04954661552094863, - 0.34499046429873387, - -0.9372975581070098, - 991.433880749304 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "R_camera_to_world": [ - [ - 0.99716158725115, - 0.0566912850549916, - -0.04954661552094863 - ], - [ - 0.07035390512768569, - -0.9359657619679411, - 0.34499046429873387 - ], - [ - -0.026815982996183867, - -0.3474970368543954, - -0.9372975581070098 - ] - ], - "t_camera_in_world_mm": [ - 271.2803169327997, - -389.1505655599084, - 900.1833170218047 - ], - "T_world_camera": [ - [ - 0.99716158725115, - 0.0566912850549916, - -0.04954661552094863, - 271.2803169327997 - ], - [ - 0.07035390512768569, - -0.9359657619679411, - 0.34499046429873387, - -389.1505655599084 - ], - [ - -0.026815982996183867, - -0.3474970368543954, - -0.9372975581070098, - 900.1833170218047 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "statistics": { - "rmse_px": 0.9028284747812563, - "mean_px": 0.7517926037815363, - "median_px": 0.87428115526563, - "max_px": 1.5028790321047885, - "per_corner_errors_px": [ - 0.11338351412627667, - 0.7806424610163631, - 0.13345378642964098, - 1.1182055161944655, - 1.5028790321047885, - 0.967919849514897, - 1.1773769846431092, - 0.22047968622274988 - ], - "per_corner_residuals_px": [ - [ - -0.08913552529247681, - -0.07007623995662016 - ], - [ - -0.7008974849087508, - -0.343722806328401 - ], - [ - -0.021934401844987406, - 0.13163888152104164 - ], - [ - 1.012165401384209, - 0.4752944105377992 - ], - [ - 1.4812157895764813, - 0.254254144213121 - ], - [ - -0.9440788085447593, - 0.21350418811266536 - ], - [ - -0.7765629048541882, - -0.8849670156405409 - ], - [ - 0.03208204880576204, - 0.2181330653094733 - ] - ], - "num_correspondences": 8, - "num_inliers": 5, - "used_marker_ids": [ - 210, - 215 - ] - }, - "input": { - "detection_image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1b.png", - "camera_id": "cam1", - "marker_dictionary": "DICT_4X4_250" - } - } -} \ No newline at end of file diff --git a/pipeline/render_1c_aruco_detection.json b/pipeline/render_1c_aruco_detection.json index 7abcea3..35cfe7d 100644 --- a/pipeline/render_1c_aruco_detection.json +++ b/pipeline/render_1c_aruco_detection.json @@ -1,6 +1,6 @@ { "schema_version": "1.0", - "created_utc": "2026-05-28T14:19:27Z", + "created_utc": "2026-05-28T15:11:27Z", "vision_config": { "MarkerType": "DICT_4X4_250", "MarkerSize": 0.025 @@ -46,7 +46,7 @@ }, "detections": [ { - "observation_id": "42546de4-23fc-497d-a4b8-7e3b5c317cd5", + "observation_id": "2dd90714-6c72-4eef-add7-476c5f768882", "type": "aruco", "marker_id": 102, "marker_size_m": 0.025, @@ -100,7 +100,7 @@ "confidence": 0.7012635429952238 }, { - "observation_id": "6032f35e-1d39-49f1-8b05-160df4253252", + "observation_id": "2b3b1621-eabd-4e73-8638-4830ff614dca", "type": "aruco", "marker_id": 122, "marker_size_m": 0.025, @@ -154,7 +154,7 @@ "confidence": 0.5749326438029929 }, { - "observation_id": "efc9d683-cf6e-4f5e-bf08-a48e7831174e", + "observation_id": "c87294de-3f5e-4ecd-b7ad-1665989bc0ae", "type": "aruco", "marker_id": 243, "marker_size_m": 0.025, @@ -208,7 +208,7 @@ "confidence": 0.9491420540234864 }, { - "observation_id": "d325590b-19cf-490b-8034-1de37cc026d3", + "observation_id": "34650367-22d5-4d90-9a88-282957232989", "type": "aruco", "marker_id": 246, "marker_size_m": 0.025, @@ -262,7 +262,7 @@ "confidence": 0.4780463267147134 }, { - "observation_id": "fabdc47b-250c-4fc2-83d2-6393801f420c", + "observation_id": "e1603207-bb10-4f2c-9dd8-353447302ecf", "type": "aruco", "marker_id": 247, "marker_size_m": 0.025, @@ -316,7 +316,7 @@ "confidence": 0.5106218488640142 }, { - "observation_id": "d44cf9ae-09cd-4be5-87b0-27137bf0249e", + "observation_id": "bbb6f5cd-12a4-4078-9f2b-d1c7160eb5f4", "type": "aruco", "marker_id": 214, "marker_size_m": 0.025, @@ -370,7 +370,7 @@ "confidence": 0.6041252446922665 }, { - "observation_id": "e20bc567-22b3-4fd1-aafd-2be185f0db54", + "observation_id": "c88f0aac-ae92-4559-9469-8ae96275d717", "type": "aruco", "marker_id": 210, "marker_size_m": 0.025, diff --git a/pipeline/render_1c_aruco_detection_camera_pose.json b/pipeline/render_1c_aruco_detection_camera_pose.json deleted file mode 100644 index 4e97dad..0000000 --- a/pipeline/render_1c_aruco_detection_camera_pose.json +++ /dev/null @@ -1,169 +0,0 @@ -{ - "schema_version": "1.0", - "created_utc": "2026-05-28T14:19:28Z", - "source_detection_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c_aruco_detection.json", - "camera_pose": { - "rvec_world_to_camera": [ - 2.263468911707335, - 0.4185279076232256, - -0.15618269854175848 - ], - "tvec_world_to_camera_mm": [ - -131.52071411361564, - 6.261008191633575, - 845.2175730094486 - ], - "R_world_to_camera": [ - [ - 0.9373310430689994, - 0.34765394455176385, - 0.023393386603497504 - ], - [ - 0.24733770712755493, - -0.6165675091799039, - -0.7474413456964855 - ], - [ - -0.24542733004306072, - 0.7063860427990407, - -0.6639157960213375 - ] - ], - "T_camera_world": [ - [ - 0.9373310430689994, - 0.34765394455176385, - 0.023393386603497504, - -131.52071411361564 - ], - [ - 0.24733770712755493, - -0.6165675091799039, - -0.7474413456964855, - 6.261008191633575 - ], - [ - -0.24542733004306072, - 0.7063860427990407, - -0.6639157960213375, - 845.2175730094486 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "R_camera_to_world": [ - [ - 0.9373310430689994, - 0.24733770712755493, - -0.24542733004306072 - ], - [ - 0.34765394455176385, - -0.6165675091799039, - 0.7063860427990407 - ], - [ - 0.023393386603497504, - -0.7474413456964855, - -0.6639157960213375 - ] - ], - "t_camera_in_world_mm": [ - 329.1693569840543, - -547.46586742482, - 568.9097490955903 - ], - "T_world_camera": [ - [ - 0.9373310430689994, - 0.24733770712755493, - -0.24542733004306072, - 329.1693569840543 - ], - [ - 0.34765394455176385, - -0.6165675091799039, - 0.7063860427990407, - -547.46586742482 - ], - [ - 0.023393386603497504, - -0.7474413456964855, - -0.6639157960213375, - 568.9097490955903 - ], - [ - 0.0, - 0.0, - 0.0, - 1.0 - ] - ], - "statistics": { - "rmse_px": 1.013497154241286, - "mean_px": 0.959948878595011, - "median_px": 0.98147711476875, - "max_px": 1.4670431124238368, - "per_corner_errors_px": [ - 0.7453912348426285, - 0.7217726973711857, - 1.0945509565073668, - 0.3852000087970229, - 0.8849242405465457, - 1.4670431124238368, - 1.0780299889909544, - 1.3026787892805474 - ], - "per_corner_residuals_px": [ - [ - -0.14935206506288523, - -0.7302753272853124 - ], - [ - -0.6208469528596652, - -0.3681098854898437 - ], - [ - 0.5489523368783011, - 0.9469388196853288 - ], - [ - 0.3286776763773105, - 0.20087317349123168 - ], - [ - 0.6565456329204267, - 0.5933285290629442 - ], - [ - -1.440736791251254, - 0.27657366477171763 - ], - [ - -0.6545732662175396, - -0.8565526815772273 - ], - [ - 1.3003287403698778, - -0.07821249906197636 - ] - ], - "num_correspondences": 8, - "num_inliers": 8, - "used_marker_ids": [ - 210, - 214 - ] - }, - "input": { - "detection_image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1c.png", - "camera_id": "cam1", - "marker_dictionary": "DICT_4X4_250" - } - } -} \ No newline at end of file diff --git a/pipeline/run.bat b/pipeline/run.bat index 5d56680..fb817a1 100644 --- a/pipeline/run.bat +++ b/pipeline/run.bat @@ -3,4 +3,4 @@ python3 1_detect_aruco_observations.py --image render_1a.png -npz render.npz -ro python3 1_detect_aruco_observations.py --image render_1b.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir . python3 1_detect_aruco_observations.py --image render_1c.png -npz render.npz -robot ../robot.json -cameraId cam1 -outDir . -python3 2_KameraPosition.py --robot ../robot.json --detections render_1a_aruco_detection.json render_1b_aruco_detection.json render_1c_aruco_detection.json --outdir . --write-summary \ No newline at end of file +python3 2_Multiview.py --robot ../robot.json --detections render_1a_aruco_detection.json render_1b_aruco_detection.json render_1c_aruco_detection.json --outDir . diff --git a/render.png b/render.png index b3de365..acb9a3d 100644 Binary files a/render.png and b/render.png differ