Versione definitiva del programma per Aruco Tag
Misura le distanze relative tra i vari tag (il tag di riferimento e' definito dalla variabile ref_id) e la rotazione usando i quaternioni
Le immagini hanno nome del tipo 20250801.jpg e sono contenute nel folder denominato giorno
import cv2
import numpy as np
from scipy.spatial.transform import Rotation as R
import os
import csv
import sys
output_rows = []
# --- Load image ---
folder = "giorno"
arrays = []
files = [f for f in os.listdir(folder) if f.lower().endswith(".jpg")]
files.sort()
for file in files:
img_path = os.path.join(folder, file)
img = cv2.imread(img_path)
print(file[0:8])
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
parameters = cv2.aruco.DetectorParameters()
parameters.cornerRefinementMethod = 0
detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
# --- Detect markers ---
corners, ids, rejected = detector.detectMarkers(img)
if ids is None or len(ids) == 0:
print("No markers detected.")
else:
# --- Load camera calibration ---
camera_matrix = np.load("./camera_matrix.npy")
dist_coeffs = np.load("./dist_coeffs.npy")
# --- Marker size in meters ---
marker_length = 0.25
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
corners, marker_length, camera_matrix, dist_coeffs
)
def rtvec_to_matrix(rvec, tvec):
R_mat, _ = cv2.Rodrigues(rvec)
T = np.eye(4)
T[:3, :3] = R_mat
T[:3, 3] = tvec.flatten()
return T
poses = {}
for i, marker_id in enumerate(ids.flatten()):
poses[marker_id] = rtvec_to_matrix(rvecs[i], tvecs[i])
# --- Compute relative poses w.r.t marker 11 ---
ref_id = 15
if ref_id in poses:
T_ref = poses[ref_id]
T_ref_inv = np.linalg.inv(T_ref)
for marker_id, T in poses.items():
if marker_id == ref_id:
continue # skip the reference itself
# Relative transform
T_rel = T_ref_inv @ T
R_rel = T_rel[:3, :3]
t_rel = T_rel[:3, 3]
distance = np.linalg.norm(t_rel)
# Convert rotation matrix to quaternion
quat = R.from_matrix(R_rel).as_quat() # [x, y, z, w]
#print(f"Marker {marker_id} relative to Marker {ref_id}:")
#print(" Translation (m):", t_rel)
#print(" Distance (m):", distance)
#print(" Quaternion [x, y, z, w]:", quat, "\n")
#print(file[0:8],marker_id,distance,quat[3])
output_rows.append([file[0:8],marker_id, distance, quat[3]])
with open("marker.csv", "w", newline="") as f:
writer = csv.writer(f)
writer.writerow(["Date","MarkerID", "Distance_m", "Quat_w"])
writer.writerows(output_rows)
