674 lines
28 KiB
Python
Executable File
674 lines
28 KiB
Python
Executable File
#!/usr/bin/env python3
|
||
"""
|
||
readCamPositionTwo.py
|
||
|
||
Two-camera ArUco detection with joint optimization of both camera extrinsics
|
||
against known machine-frame reference markers, plus triangulation of unknown
|
||
marker positions. Outputs camera pose and marker poses in machine coordinates,
|
||
with CSV and JSON similar to the single-camera script.
|
||
|
||
Dependencies: numpy, opencv-python (cv2)
|
||
Optional but NOT required: SciPy (we implement a simple Levenberg–Marquardt).
|
||
|
||
Usage example:
|
||
python3 readTwoImages.py -i snapshot_video0_1764531874081.jpg -i snapshot_video1_1764531874081.jpg -npz callibration_cam0.npz -npz callibration_cam1.npz -settings settings.json
|
||
python3 readTwoImages.py -i snapshot_video0_1764524369655.jpg -i snapshot_video1_1764524369655.jpg -npz callibration_cam0.npz -npz callibration_cam1.npz -settings settings.json
|
||
|
||
python3 readTwoImages.py -i snapshot_video0_1765009029764.jpg -i snapshot_video1_1765009029764.jpg -npz callibration_cam0.npz -npz callibration_cam1.npz -settings settings.json
|
||
|
||
Settings JSON is expected to contain:
|
||
{
|
||
"coordinateSystem": {
|
||
"KnownMarkers": {
|
||
"50": [0.0, 0.0, 0.0],
|
||
"71": [0.140, 0.0, 0.0],
|
||
"101": [0.0, -0.080, 0.0]
|
||
}
|
||
}
|
||
}
|
||
|
||
Author: M365 Copilot (generated)
|
||
"""
|
||
import argparse
|
||
import os
|
||
import sys
|
||
import json
|
||
import time
|
||
from typing import Dict, Tuple, List
|
||
import numpy as np
|
||
import cv2
|
||
|
||
# ---------------- Configuration defaults ----------------
|
||
AXIS_LENGTH_M = 0.05
|
||
|
||
# ---------------- Utilities ----------------
|
||
def load_intrinsics_npz(npz_path: str) -> Tuple[np.ndarray, np.ndarray]:
|
||
print("NPZ reading of file:", npz_path)
|
||
if os.path.exists(npz_path):
|
||
data = np.load(npz_path)
|
||
for k in ('camera_matrix', 'mtx', 'K'):
|
||
if k in data:
|
||
camera_matrix = data[k].astype(np.float32)
|
||
break
|
||
else:
|
||
raise KeyError("Camera matrix not found in NPZ.")
|
||
for k in ('dist_coeffs', 'dist', 'D'):
|
||
if k in data:
|
||
dist = data[k].astype(np.float32).reshape(-1,1)
|
||
break
|
||
else:
|
||
dist = np.zeros((5,1), dtype=np.float32)
|
||
print("NPZ loaded:", npz_path)
|
||
return camera_matrix, dist
|
||
# Fallback default intrinsics
|
||
camera_matrix = np.array([[1400, 0, 640],
|
||
[0, 1400, 360],
|
||
[0, 0, 1]], dtype=np.float32)
|
||
dist_coeffs = np.zeros((5,1), dtype=np.float32)
|
||
print("[WARN] Using default approximate intrinsics.")
|
||
return camera_matrix, dist_coeffs
|
||
|
||
|
||
def get_aruco_detector(dict_name: str):
|
||
mapping = {
|
||
'DICT_4X4_250': cv2.aruco.DICT_4X4_250,
|
||
'DICT_5X5_100': cv2.aruco.DICT_5X5_100,
|
||
'DICT_6X6_250': cv2.aruco.DICT_6X6_250,
|
||
'DICT_ARUCO_ORIGINAL': cv2.aruco.DICT_ARUCO_ORIGINAL,
|
||
}
|
||
if dict_name not in mapping:
|
||
dict_id = cv2.aruco.DICT_4X4_250
|
||
else:
|
||
dict_id = mapping[dict_name]
|
||
dictionary = cv2.aruco.getPredefinedDictionary(dict_id)
|
||
try:
|
||
params = cv2.aruco.DetectorParameters()
|
||
except Exception:
|
||
params = cv2.aruco.DetectorParameters_create()
|
||
try:
|
||
detector = cv2.aruco.ArucoDetector(dictionary, params)
|
||
return detector, None
|
||
except Exception:
|
||
return None, (dictionary, params)
|
||
|
||
|
||
def detect_markers(image: np.ndarray, detector_tuple):
|
||
detector, fallback = detector_tuple
|
||
if detector is not None:
|
||
corners, ids, rejected = detector.detectMarkers(image)
|
||
else:
|
||
dictionary, params = fallback
|
||
corners, ids, rejected = cv2.aruco.detectMarkers(image, dictionary, parameters=params)
|
||
return corners, ids, rejected
|
||
|
||
|
||
def corners_to_image_points(corners: np.ndarray) -> np.ndarray:
|
||
return corners.reshape(4,2).astype(np.float32)
|
||
|
||
|
||
def marker_center_from_corners(corners: np.ndarray) -> np.ndarray:
|
||
pts = corners.reshape(4,2)
|
||
return pts.mean(axis=0).astype(np.float32)
|
||
|
||
|
||
def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
|
||
R, _ = cv2.Rodrigues(rvec)
|
||
return R
|
||
|
||
|
||
def rigid_transform_no_scale(A: np.ndarray, B: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||
"""Find R,t s.t. B ≈ R A + t. A,B: Nx3."""
|
||
assert A.shape == B.shape and A.shape[1] == 3, "A and B must be Nx3"
|
||
N = A.shape[0]
|
||
if N < 2:
|
||
raise ValueError("Need at least 2 points; 3+ recommended.")
|
||
centroid_A = A.mean(axis=0)
|
||
centroid_B = B.mean(axis=0)
|
||
AA = A - centroid_A
|
||
BB = B - centroid_B
|
||
H = AA.T @ BB
|
||
U, S, Vt = np.linalg.svd(H)
|
||
R = Vt.T @ U.T
|
||
if np.linalg.det(R) < 0:
|
||
Vt[-1, :] *= -1
|
||
R = Vt.T @ U.T
|
||
t = centroid_B - R @ centroid_A
|
||
return R.astype(np.float32), t.astype(np.float32)
|
||
|
||
|
||
def undistort_to_normalized(points_px: np.ndarray, K: np.ndarray, D: np.ndarray) -> np.ndarray:
|
||
"""points_px: Nx2 pixel. Returns Nx2 normalized coords (x,y) where projection is x=Xp/Z, y=Yp/Z.
|
||
cv2.undistortPoints with P=None yields normalized coordinates.
|
||
"""
|
||
pts = points_px.reshape(-1,1,2).astype(np.float32)
|
||
und = cv2.undistortPoints(pts, K, D, P=None) # returns Nx1x2
|
||
return und.reshape(-1,2)
|
||
|
||
|
||
# ---------------- Joint optimization (LM, numerical Jacobian) ----------------
|
||
|
||
def pack_params(omega1, t1, omega2, t2) -> np.ndarray:
|
||
return np.hstack([omega1, t1, omega2, t2]).astype(np.float64)
|
||
|
||
|
||
def unpack_params(theta: np.ndarray):
|
||
omega1 = theta[0:3]
|
||
t1 = theta[3:6]
|
||
omega2 = theta[6:9]
|
||
t2 = theta[9:12]
|
||
return omega1, t1, omega2, t2
|
||
|
||
|
||
def residuals_centers_normalized(theta: np.ndarray,
|
||
X_mach: np.ndarray,
|
||
obs1_norm: np.ndarray,
|
||
obs2_norm: np.ndarray) -> np.ndarray:
|
||
"""
|
||
Compute residuals in normalized coordinates (no intrinsics, no distortion).
|
||
For camera j: X_cam = R_j X_mach + t_j; proj: (x/z, y/z).
|
||
Returns stacked residuals [r1; r2] shape (4N,), where N = number of references.
|
||
"""
|
||
omega1, t1, omega2, t2 = unpack_params(theta)
|
||
R1 = cv2.Rodrigues(omega1)[0]
|
||
R2 = cv2.Rodrigues(omega2)[0]
|
||
# Camera 1 projections
|
||
X_cam1 = (R1 @ X_mach.T + t1.reshape(3,1)).T # Nx3
|
||
uv1 = X_cam1[:, :2] / X_cam1[:, 2:3]
|
||
r1 = (obs1_norm - uv1).reshape(-1)
|
||
# Camera 2 projections
|
||
X_cam2 = (R2 @ X_mach.T + t2.reshape(3,1)).T
|
||
uv2 = X_cam2[:, :2] / X_cam2[:, 2:3]
|
||
r2 = (obs2_norm - uv2).reshape(-1)
|
||
return np.hstack([r1, r2])
|
||
|
||
|
||
def numerical_jacobian(f, theta: np.ndarray, eps: float, *args) -> np.ndarray:
|
||
"""Finite-difference Jacobian: forward differences."""
|
||
r0 = f(theta, *args)
|
||
m = r0.size
|
||
n = theta.size
|
||
J = np.zeros((m, n), dtype=np.float64)
|
||
for k in range(n):
|
||
th = theta.copy()
|
||
th[k] += eps
|
||
rk = f(th, *args)
|
||
J[:, k] = (rk - r0) / eps
|
||
return J, r0
|
||
|
||
|
||
def lm_solve(theta0: np.ndarray,
|
||
X_mach: np.ndarray,
|
||
obs1_norm: np.ndarray,
|
||
obs2_norm: np.ndarray,
|
||
max_iter: int = 50,
|
||
eps_jac: float = 1e-6,
|
||
lambda_init: float = 1e-3) -> Tuple[np.ndarray, Dict]:
|
||
"""Simple Levenberg–Marquardt on center normalized residuals."""
|
||
lam = lambda_init
|
||
theta = theta0.copy()
|
||
history = {"iters": [], "rms": []}
|
||
for it in range(max_iter):
|
||
J, r = numerical_jacobian(residuals_centers_normalized, theta, eps_jac,
|
||
X_mach, obs1_norm, obs2_norm)
|
||
rms = float(np.sqrt(np.mean(r*r))) if r.size else 0.0
|
||
history["iters"].append(it)
|
||
history["rms"].append(rms)
|
||
# Normal equations with damping
|
||
JTJ = J.T @ J
|
||
g = J.T @ r
|
||
H = JTJ + lam * np.eye(JTJ.shape[0])
|
||
try:
|
||
delta = -np.linalg.solve(H, g)
|
||
except np.linalg.LinAlgError:
|
||
# Fallback to least squares
|
||
delta, *_ = np.linalg.lstsq(H, -g, rcond=None)
|
||
theta_trial = theta + delta
|
||
r_trial = residuals_centers_normalized(theta_trial, X_mach, obs1_norm, obs2_norm)
|
||
rms_trial = float(np.sqrt(np.mean(r_trial*r_trial)))
|
||
if rms_trial < rms: # improve
|
||
theta = theta_trial
|
||
lam *= 0.5
|
||
else:
|
||
lam *= 2.0
|
||
# Convergence criteria
|
||
if np.linalg.norm(delta) < 1e-9 or abs(rms - rms_trial) < 1e-9:
|
||
break
|
||
return theta, history
|
||
|
||
|
||
# ---------------- Triangulation ----------------
|
||
|
||
def build_projection_matrix(K: np.ndarray, R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||
return K @ np.hstack([R, t.reshape(3,1)])
|
||
|
||
|
||
def triangulate_center(P1: np.ndarray, P2: np.ndarray, u1: np.ndarray, u2: np.ndarray) -> np.ndarray:
|
||
# u1,u2: (2,) pixel coordinates
|
||
x1 = u1.reshape(2,1)
|
||
x2 = u2.reshape(2,1)
|
||
X_h = cv2.triangulatePoints(P1, P2, x1, x2) # 4x1 homogeneous in machine frame if P maps machine->pixels
|
||
X = (X_h[:3] / X_h[3]).reshape(3)
|
||
return X.astype(np.float32)
|
||
|
||
|
||
# ---------------- Main ----------------
|
||
def main():
|
||
print("Started")
|
||
parser = argparse.ArgumentParser(description="Two-camera ArUco joint optimization and triangulation")
|
||
parser.add_argument('-i', '--images', action='append', required=True,
|
||
help="Two image paths: first camera then second camera")
|
||
parser.add_argument('-npz', '--npz', action='append', required=True,
|
||
help="Two NPZ intrinsics paths: cam1 then cam2")
|
||
parser.add_argument('-markerSizeMM', '--markerSizeMM', type=float, default=25.0,
|
||
help="Marker side length in millimeters")
|
||
parser.add_argument('--dict', default='DICT_4X4_250', help="ArUco dictionary name")
|
||
parser.add_argument('-settings', type=str, default=None,
|
||
help="Json settings file containing machine KnownMarkers")
|
||
args = parser.parse_args()
|
||
|
||
if len(args.images) != 2 or len(args.npz) != 2:
|
||
print("[ERROR] Provide exactly two images and two intrinsics NPZ files.")
|
||
sys.exit(1)
|
||
|
||
img1 = cv2.imread(args.images[0])
|
||
img2 = cv2.imread(args.images[1])
|
||
draw1 = img1.copy()
|
||
draw2 = img2.copy()
|
||
h, w = draw1.shape[:2]
|
||
#drawPNG1 = np.zeros((h, w, 4), dtype=np.uint8) # fully transparent
|
||
drawPNG1 = np.zeros((h, w, 3), dtype=np.uint8)
|
||
# Also prepare a matching canvas for camera2 to keep the layout tidy
|
||
drawPNG2 = np.zeros((h, w, 3), dtype=np.uint8)
|
||
|
||
if img1 is None or img2 is None:
|
||
print("[ERROR] Cannot read one of the images.")
|
||
sys.exit(1)
|
||
|
||
K1, D1 = load_intrinsics_npz(args.npz[0])
|
||
K2, D2 = load_intrinsics_npz(args.npz[1])
|
||
|
||
# Marker 3D local geometry (square corners)
|
||
half = (args.markerSizeMM / 1000.0) / 2.0
|
||
obj_points = np.array([
|
||
[-half, half, 0.0],
|
||
[ half, half, 0.0],
|
||
[ half, -half, 0.0],
|
||
[-half, -half, 0.0],
|
||
], dtype=np.float32)
|
||
|
||
# Read settings for machine known markers
|
||
known_markers: Dict[int, np.ndarray] = {}
|
||
if args.settings is not None and os.path.exists(args.settings):
|
||
with open(args.settings, 'r') as f:
|
||
settings = json.load(f)
|
||
for marker_id, coords in settings['coordinateSystem']['KnownMarkers'].items():
|
||
known_markers[int(marker_id)] = np.array(coords, dtype=np.float32)
|
||
print("[INFO] Loaded KnownMarkers from settings.")
|
||
else:
|
||
# Fallback defaults
|
||
known_markers = {
|
||
50: np.array([0.0, 0.0, 0.0], dtype=np.float32),
|
||
71: np.array([0.140, 0.0, 0.0], dtype=np.float32),
|
||
101: np.array([0.0, -0.080, 0.0], dtype=np.float32),
|
||
}
|
||
print("[WARN] Using default KnownMarkers; provide -settings for your machine.")
|
||
|
||
# Detect markers in both images
|
||
detector_tuple = get_aruco_detector(args.dict)
|
||
corners1, ids1, _ = detect_markers(img1, detector_tuple)
|
||
corners2, ids2, _ = detect_markers(img2, detector_tuple)
|
||
if ids1 is None or ids2 is None:
|
||
print("[ERROR] No markers detected in one or both images.")
|
||
sys.exit(1)
|
||
|
||
ids1 = ids1.flatten().tolist()
|
||
ids2 = ids2.flatten().tolist()
|
||
|
||
# Neu: merken, welche Kamera welchen Marker gesehen hat
|
||
seen_by = {} # id -> 1, 2 oder 3 (3 = beide)
|
||
for mid in ids1:
|
||
seen_by[mid] = seen_by.get(mid, 0) | 1
|
||
for mid in ids2:
|
||
seen_by[mid] = seen_by.get(mid, 0) | 2
|
||
|
||
# Build dicts: id -> corners, center, rvec/tvec (per-camera PnP)
|
||
def build_marker_dict(img, corners_list, ids, K, D, draw = False) -> Tuple[Dict[int,np.ndarray], Dict[int,np.ndarray], Dict[int,Tuple[np.ndarray,np.ndarray]]]:
|
||
centers = {}
|
||
corners_dict = {}
|
||
poses = {}
|
||
for i, mid in enumerate(ids):
|
||
C = corners_list[i]
|
||
corners_dict[int(mid)] = C
|
||
centers[int(mid)] = marker_center_from_corners(C)
|
||
# Pose from single marker
|
||
img_pts = corners_to_image_points(C)
|
||
success, rvec, tvec = cv2.solvePnP(obj_points, img_pts, K, D, flags=cv2.SOLVEPNP_IPPE_SQUARE)
|
||
if not success:
|
||
success, rvec, tvec = cv2.solvePnP(obj_points, img_pts, K, D)
|
||
if success:
|
||
poses[int(mid)] = (rvec.reshape(3,1), tvec.reshape(3,1))
|
||
if(draw):
|
||
cv2.drawFrameAxes(draw1, K, D, rvec, tvec, 0.02) # slim orientation axes
|
||
cv2.drawFrameAxes(drawPNG1, K, D, rvec, tvec, 0.02) # slim orientation axes
|
||
return centers, corners_dict, poses
|
||
|
||
centers1, corners_dict1, poses1 = build_marker_dict(img1, corners1, ids1, K1, D1, draw = True)
|
||
centers2, corners_dict2, poses2 = build_marker_dict(img2, corners2, ids2, K2, D2)
|
||
|
||
# Common reference markers present in both images and known in machine frame
|
||
common_refs = [mid for mid in known_markers.keys() if (mid in centers1 and mid in centers2)]
|
||
if len(common_refs) < 3:
|
||
print(f"[ERROR] Need ≥3 common reference markers in both cameras; found {len(common_refs)}: {common_refs}")
|
||
sys.exit(1)
|
||
|
||
# Initial extrinsics from rigid fits per camera using tvec centers of references
|
||
# For camera j, A = camera-frame positions of reference markers (from PnP tvec), B = machine positions
|
||
def init_extrinsics_from_rigid(poses_cam: Dict[int,Tuple[np.ndarray,np.ndarray]], refs: List[int]) -> Tuple[np.ndarray,np.ndarray]:
|
||
A = []
|
||
B = []
|
||
for mid in refs:
|
||
if mid in poses_cam:
|
||
_, tvec = poses_cam[mid]
|
||
A.append(tvec.flatten())
|
||
B.append(known_markers[mid])
|
||
if len(A) < 2:
|
||
raise RuntimeError("Insufficient reference poses for rigid transform init.")
|
||
A = np.stack(A, axis=0)
|
||
B = np.stack(B, axis=0)
|
||
R_cm, t_cm = rigid_transform_no_scale(A, B) # camera->machine
|
||
# Convert to machine->camera
|
||
R_mc = R_cm.T
|
||
t_mc = - R_mc @ t_cm
|
||
return R_mc.astype(np.float32), t_mc.astype(np.float32)
|
||
|
||
R1_init, t1_init = init_extrinsics_from_rigid(poses1, common_refs)
|
||
R2_init, t2_init = init_extrinsics_from_rigid(poses2, common_refs)
|
||
|
||
# Observations: reference centers (pixels) -> normalized
|
||
X_mach_refs = np.stack([known_markers[mid] for mid in common_refs], axis=0).astype(np.float32)
|
||
obs1_px = np.stack([centers1[mid] for mid in common_refs], axis=0).astype(np.float32)
|
||
obs2_px = np.stack([centers2[mid] for mid in common_refs], axis=0).astype(np.float32)
|
||
obs1_norm = undistort_to_normalized(obs1_px, K1, D1)
|
||
obs2_norm = undistort_to_normalized(obs2_px, K2, D2)
|
||
|
||
# Pack initial params as Rodrigues vectors
|
||
omega1_init = cv2.Rodrigues(R1_init)[0].reshape(3)
|
||
omega2_init = cv2.Rodrigues(R2_init)[0].reshape(3)
|
||
theta0 = pack_params(omega1_init, t1_init.reshape(3), omega2_init, t2_init.reshape(3))
|
||
|
||
theta_opt, hist = lm_solve(theta0, X_mach_refs, obs1_norm, obs2_norm,
|
||
max_iter=60, eps_jac=1e-6, lambda_init=1e-3)
|
||
|
||
omega1, t1, omega2, t2 = unpack_params(theta_opt)
|
||
R1_opt = cv2.Rodrigues(omega1)[0]
|
||
R2_opt = cv2.Rodrigues(omega2)[0]
|
||
|
||
# Camera pose in machine coordinates (camera->machine): R_cm = R^T, t_cm = -R^T t
|
||
R1_cm = R1_opt.T
|
||
t1_cm = - R1_cm @ t1
|
||
R2_cm = R2_opt.T
|
||
t2_cm = - R2_cm @ t2
|
||
|
||
# Build projection matrices for triangulation (machine -> pixels)
|
||
P1 = build_projection_matrix(K1, R1_opt, t1)
|
||
P2 = build_projection_matrix(K2, R2_opt, t2)
|
||
|
||
# Collect markers seen by at least one camera
|
||
all_ids = set(ids1) | set(ids2)
|
||
# Output structures
|
||
rows = [("id", "x_mm", "y_mm", "z_mm", "roll_deg", "pitch_deg", "yaw_deg", "seen_by")]
|
||
marker_list = []
|
||
|
||
# Camera orientations in Euler (ZYX)
|
||
def R_to_euler_zyx(R: np.ndarray) -> Tuple[float,float,float]:
|
||
yaw = float(np.degrees(np.arctan2(R[1,0], R[0,0])))
|
||
sp = np.sqrt(R[2,1]**2 + R[2,2]**2)
|
||
pitch = float(np.degrees(np.arctan2(-R[2,0], sp)))
|
||
roll = float(np.degrees(np.arctan2(R[2,1], R[2,2])))
|
||
return roll, pitch, yaw
|
||
|
||
cam1_roll, cam1_pitch, cam1_yaw = R_to_euler_zyx(R1_cm)
|
||
cam2_roll, cam2_pitch, cam2_yaw = R_to_euler_zyx(R2_cm)
|
||
|
||
# Camera rows
|
||
c1_mm = (t1_cm * 1000.0).tolist()
|
||
rows.append(("camera 0", f"{c1_mm[0]:.2f}", f"{c1_mm[1]:.2f}", f"{c1_mm[2]:.2f}", f"{cam1_roll:.3f}", f"{cam1_pitch:.3f}", f"{cam1_yaw:.3f}"))
|
||
c2_mm = (t2_cm * 1000.0).tolist()
|
||
rows.append(("camera 1", f"{c2_mm[0]:.2f}", f"{c2_mm[1]:.2f}", f"{c2_mm[2]:.2f}", f"{cam2_roll:.3f}", f"{cam2_pitch:.3f}", f"{cam2_yaw:.3f}"))
|
||
|
||
# Triangulate and output markers
|
||
def orientation_in_machine(mid: int) -> Tuple[float,float,float]:
|
||
# Prefer camera1 orientation, else camera2
|
||
if mid in poses1:
|
||
Rm_cam = rvec_to_R(poses1[mid][0])
|
||
Rm_machine = R1_cm @ Rm_cam
|
||
elif mid in poses2:
|
||
Rm_cam = rvec_to_R(poses2[mid][0])
|
||
Rm_machine = R2_cm @ Rm_cam
|
||
else:
|
||
Rm_machine = np.eye(3, dtype=np.float32)
|
||
return R_to_euler_zyx(Rm_machine)
|
||
|
||
# Residual report for references
|
||
# Recompute residual RMS in pixels for references (after optimization)
|
||
refs_rms_px = []
|
||
for j,(K,R_opt,t_opt,centers_px) in enumerate([(K1,R1_opt,t1,centers1),(K2,R2_opt,t2,centers2)]):
|
||
errs = []
|
||
for mid in common_refs:
|
||
X = known_markers[mid]
|
||
X_cam = R_opt @ X + t_opt
|
||
x = K @ X_cam
|
||
u_pred = x[0]/x[2]
|
||
v_pred = x[1]/x[2]
|
||
u_obs, v_obs = centers_px[mid]
|
||
errs.append(np.hypot(u_obs-u_pred, v_obs-v_pred))
|
||
refs_rms_px.append(float(np.sqrt(np.mean(np.square(errs))) if errs else 0.0))
|
||
|
||
# Compute per-marker positions
|
||
for mid in sorted(all_ids):
|
||
# Triangulate if seen in both
|
||
if mid in centers1 and mid in centers2:
|
||
X_mach = triangulate_center(P1, P2, centers1[mid], centers2[mid])
|
||
elif mid in poses1:
|
||
# Fallback single-camera: transform tvec (camera->machine)
|
||
X_mach = (R1_cm @ poses1[mid][1].flatten() + t1_cm)
|
||
elif mid in poses2:
|
||
X_mach = (R2_cm @ poses2[mid][1].flatten() + t2_cm)
|
||
else:
|
||
continue
|
||
roll, pitch, yaw = orientation_in_machine(mid)
|
||
x_mm, y_mm, z_mm = (X_mach * 1000.0).tolist()
|
||
rows.append((mid, f"{x_mm:.2f}", f"{y_mm:.2f}", f"{z_mm:.2f}", f"{roll:.3f}", f"{pitch:.3f}", f"{yaw:.3f}", seen_by.get(mid, 0)))
|
||
marker_list.append({
|
||
"id": int(mid),
|
||
"position_mm": [float(x_mm), float(y_mm), float(z_mm)],
|
||
"orientation_deg": {"roll": float(roll), "pitch": float(pitch), "yaw": float(yaw)}
|
||
})
|
||
|
||
# Save CSV & JSON
|
||
base1 = os.path.splitext(args.images[0])[0]
|
||
base2 = os.path.splitext(args.images[1])[0]
|
||
out_csv = f"{base1}_two_cam.csv"
|
||
out_json = f"{base1}_two_cam.json"
|
||
|
||
try:
|
||
import csv
|
||
with open(out_csv, 'w', newline='') as f:
|
||
writer = csv.writer(f)
|
||
writer.writerows(rows)
|
||
print(f"[INFO] Saved CSV poses to '{out_csv}'.")
|
||
except Exception as e:
|
||
print(f"[WARN] Could not save CSV: {e}")
|
||
|
||
json_data = {
|
||
"metadata": {
|
||
"timestamp": time.strftime("%Y-%m-%d %H:%M:%S"),
|
||
"reference_markers": common_refs,
|
||
"dict": args.dict,
|
||
"marker_size_mm": args.markerSizeMM,
|
||
"rms_refs_px_cam1": refs_rms_px[0] if refs_rms_px else None,
|
||
"rms_refs_px_cam2": refs_rms_px[1] if refs_rms_px else None,
|
||
"description": "Two-camera joint optimization with triangulation"
|
||
},
|
||
"cameras": [
|
||
{
|
||
"id": "camera1",
|
||
"position_mm": [float(x) for x in (t1_cm * 1000.0)],
|
||
"orientation_deg": {"roll": cam1_roll, "pitch": cam1_pitch, "yaw": cam1_yaw},
|
||
},
|
||
{
|
||
"id": "camera2",
|
||
"position_mm": [float(x) for x in (t2_cm * 1000.0)],
|
||
"orientation_deg": {"roll": cam2_roll, "pitch": cam2_pitch, "yaw": cam2_yaw},
|
||
}
|
||
],
|
||
"markers": marker_list
|
||
}
|
||
|
||
try:
|
||
with open(out_json, 'w', encoding='utf-8') as f:
|
||
json.dump(json_data, f, indent=4)
|
||
print(f"[INFO] Saved JSON poses to '{out_json}'.")
|
||
except Exception as e:
|
||
print(f"[WARN] Could not save JSON: {e}")
|
||
|
||
# Annotate images with machine axes using camera1 extrinsics
|
||
try:
|
||
R_machine_to_cam1 = R1_opt
|
||
t_machine_to_cam1 = t1
|
||
machine_axes = np.float32([
|
||
[0.0, 0.0, 0.0],
|
||
[0.200, 0.0, 0.0],
|
||
[0.0, -0.100, 0.0],
|
||
[0.0, 0.0, 0.100],
|
||
])
|
||
rvec_global, _ = cv2.Rodrigues(R_machine_to_cam1)
|
||
imgpts, _ = cv2.projectPoints(machine_axes, rvec_global, t_machine_to_cam1, K1, D1)
|
||
origin = tuple(np.round(imgpts[0].ravel()).astype(int))
|
||
x_end = tuple(np.round(imgpts[1].ravel()).astype(int))
|
||
y_end = tuple(np.round(imgpts[2].ravel()).astype(int))
|
||
z_end = tuple(np.round(imgpts[3].ravel()).astype(int))
|
||
|
||
# Draw marker outlines only (omit default small id labels) — we draw larger IDs below
|
||
cv2.aruco.drawDetectedMarkers(draw1, corners1)
|
||
cv2.aruco.drawDetectedMarkers(drawPNG1, corners1)
|
||
# Draw larger blue ID labels (keep default marker outlines as-is)
|
||
try:
|
||
for i, mid in enumerate(ids1):
|
||
try:
|
||
pts = corners1[i].reshape((4, 2))
|
||
center = tuple(np.round(pts.mean(axis=0)).astype(int))
|
||
except Exception:
|
||
continue
|
||
text = str(int(mid))
|
||
# Offset: 5px more to the right and 5px up (y axis is downwards)
|
||
pos = (int(center[0]) + 15, int(center[1]) - 15)
|
||
cv2.putText(draw1, text, pos, cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,0,0), 3, lineType=cv2.LINE_AA)
|
||
cv2.putText(drawPNG1, text, pos, cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,0,0,255), 3, lineType=cv2.LINE_AA)
|
||
except Exception:
|
||
pass
|
||
cv2.line(draw1, origin, x_end, (0,0,255), 3)
|
||
cv2.line(draw1, origin, y_end, (0,255,0), 3)
|
||
cv2.line(draw1, origin, z_end, (255,0,0), 3)
|
||
|
||
# Draw lines (RGBA colors: B,G,R,A). A=255 for fully opaque.
|
||
cv2.line(drawPNG1, origin, x_end, (0, 0, 255, 255), 3) # Red X
|
||
cv2.line(drawPNG1, origin, y_end, (0, 255, 0, 255), 3) # Green Y
|
||
cv2.line(drawPNG1, origin, z_end, (255, 0, 0, 255), 3) # Blue Z
|
||
|
||
cv2.putText(draw1, "X (200 mm)", x_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2)
|
||
cv2.putText(draw1, "Y (-100 mm)", y_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
|
||
cv2.putText(draw1, "+Z (100 mm)", z_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0), 2)
|
||
|
||
|
||
# Try to draw text (might be jaggy on transparent BG in older OpenCV)
|
||
cv2.putText(drawPNG1, "X (200 mm)", x_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255, 255), 2)
|
||
cv2.putText(drawPNG1, "Y (-100 mm)", y_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0, 255), 2)
|
||
cv2.putText(drawPNG1, "+Z (100 mm)", z_end, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0, 255), 2)
|
||
|
||
|
||
out_img1 = f"{base1}_two_cam_annotated.jpg"
|
||
cv2.imwrite(out_img1, draw1)
|
||
print(f"[INFO] Annotated image saved as '{out_img1}'.")
|
||
|
||
# Save as transparent PNG
|
||
|
||
gray = cv2.cvtColor(drawPNG1, cv2.COLOR_BGR2GRAY)
|
||
_, alpha = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY)
|
||
|
||
# 5) Merge BGR + alpha → RGBA transparent overlay
|
||
drawPNG_1 = cv2.merge([drawPNG1[:, :, 0], drawPNG1[:, :, 1], drawPNG1[:, :, 2], alpha])
|
||
|
||
out_png1 = f"{base1}_two_cam_overlay.png"
|
||
cv2.imwrite(out_png1, drawPNG_1)
|
||
|
||
except Exception as e:
|
||
print(f"[WARN] Failed to draw machine axes: {e}")
|
||
|
||
# Also annotate the second camera image and produce a transparent overlay
|
||
try:
|
||
machine_axes2 = np.float32([
|
||
[0.0, 0.0, 0.0],
|
||
[0.200, 0.0, 0.0],
|
||
[0.0, -0.100, 0.0],
|
||
[0.0, 0.0, 0.100],
|
||
])
|
||
rvec_global2, _ = cv2.Rodrigues(R2_opt)
|
||
imgpts2, _ = cv2.projectPoints(machine_axes2, rvec_global2, t2, K2, D2)
|
||
origin2 = tuple(np.round(imgpts2[0].ravel()).astype(int))
|
||
x_end2 = tuple(np.round(imgpts2[1].ravel()).astype(int))
|
||
y_end2 = tuple(np.round(imgpts2[2].ravel()).astype(int))
|
||
z_end2 = tuple(np.round(imgpts2[3].ravel()).astype(int))
|
||
|
||
# Draw marker outlines only (omit default small id labels) — we draw larger IDs below
|
||
cv2.aruco.drawDetectedMarkers(draw2, corners2)
|
||
cv2.aruco.drawDetectedMarkers(drawPNG2, corners2)
|
||
# Draw larger blue ID labels (keep default marker outlines as-is)
|
||
try:
|
||
for i, mid in enumerate(ids2):
|
||
try:
|
||
pts = corners2[i].reshape((4, 2))
|
||
center = tuple(np.round(pts.mean(axis=0)).astype(int))
|
||
except Exception:
|
||
continue
|
||
text = str(int(mid))
|
||
# Offset: 5px more to the right and 5px up (y axis is downwards)
|
||
pos = (int(center[0]) + 13, int(center[1]) + 3)
|
||
cv2.putText(draw2, text, pos, cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,0,0), 3, lineType=cv2.LINE_AA)
|
||
cv2.putText(drawPNG2, text, pos, cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255,0,0,255), 3, lineType=cv2.LINE_AA)
|
||
except Exception:
|
||
pass
|
||
|
||
cv2.line(draw2, origin2, x_end2, (0,0,255), 3)
|
||
cv2.line(draw2, origin2, y_end2, (0,255,0), 3)
|
||
cv2.line(draw2, origin2, z_end2, (255,0,0), 3)
|
||
|
||
cv2.line(drawPNG2, origin2, x_end2, (0, 0, 255, 255), 3)
|
||
cv2.line(drawPNG2, origin2, y_end2, (0, 255, 0, 255), 3)
|
||
cv2.line(drawPNG2, origin2, z_end2, (255, 0, 0, 255), 3)
|
||
|
||
cv2.putText(draw2, "X (200 mm)", x_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2)
|
||
cv2.putText(draw2, "Y (-100 mm)", y_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
|
||
cv2.putText(draw2, "+Z (100 mm)", z_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255,0,0), 2)
|
||
|
||
cv2.putText(drawPNG2, "X (200 mm)", x_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255, 255), 2)
|
||
cv2.putText(drawPNG2, "Y (-100 mm)", y_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0, 255), 2)
|
||
cv2.putText(drawPNG2, "+Z (100 mm)", z_end2, cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0, 255), 2)
|
||
|
||
out_img2 = f"{base2}_two_cam_annotated.jpg"
|
||
cv2.imwrite(out_img2, draw2)
|
||
print(f"[INFO] Annotated image saved as '{out_img2}'.")
|
||
|
||
gray2 = cv2.cvtColor(drawPNG2, cv2.COLOR_BGR2GRAY)
|
||
_, alpha2 = cv2.threshold(gray2, 0, 255, cv2.THRESH_BINARY)
|
||
drawPNG_2 = cv2.merge([drawPNG2[:, :, 0], drawPNG2[:, :, 1], drawPNG2[:, :, 2], alpha2])
|
||
out_png2 = f"{base2}_two_cam_overlay.png"
|
||
cv2.imwrite(out_png2, drawPNG_2)
|
||
print(f"[INFO] Overlay PNG saved as '{out_png2}'.")
|
||
|
||
except Exception as e:
|
||
print(f"[WARN] Failed to draw machine axes for camera2: {e}")
|
||
|
||
if __name__ == '__main__':
|
||
main()
|