Multiview
This commit is contained in:
chk
@@ -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()
|
||||
706
pipeline/2_Multiview.py
Normal file
706
pipeline/2_Multiview.py
Normal file
@@ -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()
|
||||
BIN
pipeline/__pycache__/2_Multiview.cpython-311.pyc
Normal file
BIN
pipeline/__pycache__/2_Multiview.cpython-311.pyc
Normal file
Binary file not shown.
966
pipeline/multiview_pose.json
Normal file
966
pipeline/multiview_pose.json
Normal file
@@ -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": []
|
||||
}
|
||||
]
|
||||
}
|
||||
36
pipeline/multiview_pose_summary.json
Normal file
36
pipeline/multiview_pose_summary.json
Normal file
@@ -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
|
||||
}
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
"rvec": [
|
||||
2.291386890593113,
|
||||
-1.0684818045197042,
|
||||
0.4021828449915483
|
||||
],
|
||||
[
|
||||
-0.7612056883925573,
|
||||
-0.5203636712935098,
|
||||
-0.38702396509357573
|
||||
],
|
||||
[
|
||||
0.0013738022347707657,
|
||||
0.5954937931392015,
|
||||
-0.8033587336925552
|
||||
]
|
||||
],
|
||||
"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
|
||||
"tvec": [
|
||||
-280.5803545388054,
|
||||
191.8018727459298,
|
||||
952.2592454759357
|
||||
]
|
||||
},
|
||||
"input": {
|
||||
"detection_image_file": "C:\\Users\\kech\\SynologyDrive\\2026-AppServer-AppRobot\\appRobotRendering\\pipeline\\render_1a.png",
|
||||
"camera_id": "cam1",
|
||||
"marker_dictionary": "DICT_4X4_250"
|
||||
"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
|
||||
}
|
||||
}
|
||||
}
|
||||
BIN
pipeline/render_1a_aruco_detection_camera_pose_debug.png
Normal file
BIN
pipeline/render_1a_aruco_detection_camera_pose_debug.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.1 MiB |
@@ -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,
|
||||
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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,
|
||||
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
python3 2_Multiview.py --robot ../robot.json --detections render_1a_aruco_detection.json render_1b_aruco_detection.json render_1c_aruco_detection.json --outDir .
|
||||
|
||||
BIN
render.png
BIN
render.png
Binary file not shown.
|
Before Width: | Height: | Size: 1.3 MiB After Width: | Height: | Size: 1.3 MiB |
Reference in New Issue
Block a user