Python arbeiten
Looks like with this configuration the output path is working
This commit is contained in:
chk
0
programs/1_detect_aruco_observations.py
Normal file
0
programs/1_detect_aruco_observations.py
Normal file
@@ -11,11 +11,14 @@ 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_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
|
||||
|
||||
NEW:
|
||||
python3 readTwoImages.py -i img0.jpg -i img1.jpg -npz cam0.npz -npz cam1.npz -settings settings.json -outDir results/
|
||||
|
||||
Settings JSON is expected to contain:
|
||||
{
|
||||
"coordinateSystem": {
|
||||
@@ -244,7 +247,7 @@ def triangulate_center(P1: np.ndarray, P2: np.ndarray, u1: np.ndarray, u2: np.nd
|
||||
# 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_h = cv2.triangulatePoints(P1, P2, x1, x2)
|
||||
X = (X_h[:3] / X_h[3]).reshape(3)
|
||||
return X.astype(np.float32)
|
||||
|
||||
@@ -252,37 +255,47 @@ def triangulate_center(P1: np.ndarray, P2: np.ndarray, u1: np.ndarray, u2: np.nd
|
||||
# ---------------- 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('--dict', default='DICT_4X4_250',
|
||||
help="ArUco dictionary name")
|
||||
|
||||
parser.add_argument('-settings', type=str, default=None,
|
||||
help="Json settings file containing machine KnownMarkers")
|
||||
parser.add_argument('--outDir', type=str, default=None,
|
||||
help="Optional directory where all output files are written")
|
||||
|
||||
# ============================================================
|
||||
# NEW OPTIONAL PARAMETER (100% backward compatible)
|
||||
# ============================================================
|
||||
parser.add_argument('-outDir', '--outDir',
|
||||
type=str,
|
||||
default=None,
|
||||
help="Optional output directory")
|
||||
|
||||
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)
|
||||
|
||||
out_dir = None
|
||||
if args.outDir:
|
||||
out_dir = args.outDir
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
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:
|
||||
@@ -294,6 +307,7 @@ def main():
|
||||
|
||||
# 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],
|
||||
@@ -303,25 +317,33 @@ def main():
|
||||
|
||||
# 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)
|
||||
@@ -330,178 +352,303 @@ def main():
|
||||
ids2 = ids2.flatten().tolist()
|
||||
|
||||
# Neu: merken, welche Kamera welchen Marker gesehen hat
|
||||
seen_by = {} # id -> 1, 2 oder 3 (3 = beide)
|
||||
seen_by = {}
|
||||
|
||||
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]]]:
|
||||
# Build dicts
|
||||
def build_marker_dict(img, corners_list, ids, K, D, draw=False):
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
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
|
||||
|
||||
poses[int(mid)] = (
|
||||
rvec.reshape(3,1),
|
||||
tvec.reshape(3,1)
|
||||
)
|
||||
|
||||
if draw:
|
||||
cv2.drawFrameAxes(draw1, K, D, rvec, tvec, 0.02)
|
||||
cv2.drawFrameAxes(drawPNG1, K, D, rvec, tvec, 0.02)
|
||||
|
||||
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)
|
||||
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
|
||||
common_refs = [
|
||||
mid for mid in known_markers.keys()
|
||||
if (mid in centers1 and mid in centers2)
|
||||
]
|
||||
|
||||
# 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]:
|
||||
# Initial extrinsics
|
||||
def init_extrinsics_from_rigid(poses_cam, refs):
|
||||
|
||||
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_cm, t_cm = rigid_transform_no_scale(A, B)
|
||||
|
||||
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)
|
||||
# Observations
|
||||
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)
|
||||
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
|
||||
# Camera pose in machine coordinates
|
||||
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)
|
||||
# Projection matrices
|
||||
P1 = build_projection_matrix(K1, R1_opt, t1)
|
||||
P2 = build_projection_matrix(K2, R2_opt, t2)
|
||||
|
||||
# Collect markers seen by at least one camera
|
||||
# Collect markers
|
||||
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]:
|
||||
def R_to_euler_zyx(R: np.ndarray):
|
||||
|
||||
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}"))
|
||||
|
||||
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}"))
|
||||
|
||||
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
|
||||
def orientation_in_machine(mid: int):
|
||||
|
||||
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])
|
||||
|
||||
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)
|
||||
|
||||
X_mach = (
|
||||
R1_cm @ poses1[mid][1].flatten() + t1_cm
|
||||
)
|
||||
|
||||
elif mid in poses2:
|
||||
X_mach = (R2_cm @ poses2[mid][1].flatten() + t2_cm)
|
||||
|
||||
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)))
|
||||
|
||||
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)}
|
||||
"orientation_deg": {
|
||||
"roll": float(roll),
|
||||
"pitch": float(pitch),
|
||||
"yaw": float(yaw)
|
||||
}
|
||||
})
|
||||
|
||||
# Save CSV & JSON
|
||||
# ============================================================
|
||||
# OUTPUT DIRECTORY HANDLING
|
||||
# ============================================================
|
||||
|
||||
base1 = os.path.splitext(os.path.basename(args.images[0]))[0]
|
||||
base2 = os.path.splitext(os.path.basename(args.images[1]))[0]
|
||||
out_csv = os.path.join(out_dir, f"{base1}_two_cam.csv") if out_dir else f"{base1}_two_cam.csv"
|
||||
out_json = os.path.join(out_dir, f"{base1}_two_cam.json") if out_dir else f"{base1}_two_cam.json"
|
||||
|
||||
if args.outDir is not None:
|
||||
|
||||
out_dir = args.outDir
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
else:
|
||||
|
||||
out_dir = os.path.dirname(args.images[0])
|
||||
|
||||
if out_dir == "":
|
||||
out_dir = "."
|
||||
|
||||
# Save CSV & JSON
|
||||
out_csv = os.path.join(out_dir, f"{base1}_two_cam.csv")
|
||||
out_json = os.path.join(out_dir, 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}")
|
||||
|
||||
@@ -511,136 +658,219 @@ def main():
|
||||
"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},
|
||||
"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},
|
||||
"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)
|
||||
|
||||
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)
|
||||
|
||||
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.line(drawPNG1, origin, x_end, (0,0,255,255), 3)
|
||||
cv2.line(drawPNG1, origin, y_end, (0,255,0,255), 3)
|
||||
cv2.line(drawPNG1, origin, z_end, (255,0,0,255), 3)
|
||||
|
||||
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)
|
||||
|
||||
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 = os.path.join(out_dir, f"{base1}_two_cam_annotated.jpg")
|
||||
|
||||
out_img1 = os.path.join(out_dir, f"{base1}_two_cam_annotated.jpg") if out_dir else 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])
|
||||
drawPNG_1 = cv2.merge([
|
||||
drawPNG1[:, :, 0],
|
||||
drawPNG1[:, :, 1],
|
||||
drawPNG1[:, :, 2],
|
||||
alpha
|
||||
])
|
||||
|
||||
out_png1 = os.path.join(out_dir, f"{base1}_two_cam_overlay.png")
|
||||
|
||||
out_png1 = os.path.join(out_dir, f"{base1}_two_cam_overlay.png") if out_dir else 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
|
||||
# Camera 2 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)
|
||||
|
||||
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)
|
||||
|
||||
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
|
||||
|
||||
@@ -648,31 +878,44 @@ def main():
|
||||
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.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)
|
||||
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 = os.path.join(out_dir, f"{base2}_two_cam_annotated.jpg")
|
||||
|
||||
out_img2 = os.path.join(out_dir, f"{base2}_two_cam_annotated.jpg") if out_dir else 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 = os.path.join(out_dir, f"{base2}_two_cam_overlay.png") if out_dir else f"{base2}_two_cam_overlay.png"
|
||||
|
||||
drawPNG_2 = cv2.merge([
|
||||
drawPNG2[:, :, 0],
|
||||
drawPNG2[:, :, 1],
|
||||
drawPNG2[:, :, 2],
|
||||
alpha2
|
||||
])
|
||||
|
||||
out_png2 = os.path.join(out_dir, 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()
|
||||
main()
|
||||
Reference in New Issue
Block a user