Versione definitiva misura distanze con Aruco tag

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)