Questo test prende un icosaedro sul quale sono stati apposti degli aruco tags (in modo da avere almeno 3 facce sempre visibili alla camera) e tramite le normali calcola la posizione del centro dell'icosaedro tramite opencv
Un limite di questo metodo nel costruirselo da soli e' che e' difficile centrare il centro dell'aruco tag sul centro della faccia...e questo limita molto la precisione del posizionamento del centro dell'icosaedro dato che e' calcolata dall'intersezione delle normali agli aruco tags
"""
Calibrazione della camera RGB della RealSense D415 con scacchiera.
Requisiti:
pip install pyrealsense2 opencv-contrib-python numpy
Uso:
1. Stampa checkerboard_A4.pdf A SCALA 100% (non "adatta alla pagina").
2. Misura col righello il quadratino di verifica stampato sul foglio:
se non è esattamente SQUARE_SIZE_M metri di lato, correggi la costante
qui sotto con la misura reale.
3. Esegui lo script, inquadra la scacchiera da tante angolazioni/distanze/
inclinazioni diverse (coprendo tutta l'inquadratura: centro, angoli,
vicino, lontano, inclinata). Premi SPAZIO per catturare un frame quando
gli angoli vengono rilevati (disegnati a colori), ESC/'q' per terminare
e calibrare.
4. Servono almeno 15-20 catture buone, distribuite in tutta l'immagine,
per una calibrazione affidabile.
Il risultato (camera_matrix, dist_coeffs) viene salvato in
d415_calibration.npz, pronto per essere caricato nello script ArUco.
"""
import cv2
import numpy as np
import pyrealsense2 as rs
# ------------------------- CONFIGURAZIONE -------------------------
# Angoli interni della scacchiera (colonne, righe) = (num_quadrati_x - 1, num_quadrati_y - 1)
PATTERN_SIZE = (9, 6)
# Lato di un quadrato in METRI: usa il valore misurato col righello sul foglio stampato
SQUARE_SIZE_M = 0.020
FRAME_W, FRAME_H, FPS = 1280, 720, 30
OUT_NPZ = "d415_calibration.npz"
MIN_CAPTURES = 15
def build_object_points(pattern_size, square_size):
cols, rows = pattern_size
objp = np.zeros((rows * cols, 3), np.float32)
objp[:, :2] = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2) * square_size
return objp
def main():
objp_template = build_object_points(PATTERN_SIZE, SQUARE_SIZE_M)
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, FRAME_W, FRAME_H, rs.format.bgr8, FPS)
profile = pipeline.start(config)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
obj_points = [] # punti 3D reali (nel piano della scacchiera)
img_points = [] # punti 2D corrispondenti nell'immagine
img_size = None
print("SPAZIO = cattura frame corrente, 'q'/ESC = termina e calibra")
try:
while True:
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
if not color_frame:
continue
frame = np.asanyarray(color_frame.get_data())
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
img_size = gray.shape[::-1] # (w, h)
found, corners = cv2.findChessboardCorners(
gray, PATTERN_SIZE,
flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
)
display = frame.copy()
if found:
corners_refined = cv2.cornerSubPix(
gray, corners, (11, 11), (-1, -1), criteria
)
cv2.drawChessboardCorners(display, PATTERN_SIZE, corners_refined, found)
else:
corners_refined = None
cv2.putText(display, f"Catture: {len(obj_points)}/{MIN_CAPTURES}+",
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
(0, 255, 0) if found else (0, 0, 255), 2)
cv2.imshow("Calibrazione D415 - SPAZIO=cattura, q=fine", display)
key = cv2.waitKey(1) & 0xFF
if key == ord(' ') and found:
obj_points.append(objp_template.copy())
img_points.append(corners_refined)
print(f"Cattura #{len(obj_points)} registrata")
elif key in (ord('q'), 27):
break
finally:
pipeline.stop()
cv2.destroyAllWindows()
if len(obj_points) < 4:
print("Troppo poche catture per calibrare (minimo consigliato 15-20).")
return
print(f"Calibrazione in corso con {len(obj_points)} catture...")
ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
obj_points, img_points, img_size, None, None
)
# Errore di riproiezione medio, indicatore della qualità della calibrazione.
# Uso numpy invece di cv2.norm: su alcune build di OpenCV (es. 5.0) cv2.norm
# può lamentare un mismatch di tipo (CV_32FC1 vs CV_32FC2) anche se le shape
# sono corrette, quindi si aggira il problema calcolando la norma a mano.
total_error = 0
for i in range(len(obj_points)):
proj, _ = cv2.projectPoints(obj_points[i], rvecs[i], tvecs[i], camera_matrix, dist_coeffs)
proj = np.asarray(proj, dtype=np.float64).reshape(-1, 2)
observed = np.asarray(img_points[i], dtype=np.float64).reshape(-1, 2)
error = np.linalg.norm(observed - proj, axis=1).mean()
total_error += error
mean_error = total_error / len(obj_points)
print("RMS calibrazione:", ret)
print("Errore di riproiezione medio (px, minore è meglio, <0.5 è ottimo):", mean_error)
print("Camera matrix:\n", camera_matrix)
print("Distortion coeffs:\n", dist_coeffs.ravel())
np.savez(OUT_NPZ, camera_matrix=camera_matrix, dist_coeffs=dist_coeffs,
image_size=img_size, mean_reproj_error=mean_error)
print(f"Salvato in {OUT_NPZ}")
if __name__ == "__main__":
main()
"""
Stima della posizione 3D del centro di un icosaedro con marker ArUco sulle facce,
tramite intersezione ai minimi quadrati delle normali dei marker rilevati
in uno stream video dalla RealSense D415 (canale RGB), via pyrealsense2 -
stesso approccio usato in calibrate_d415.py, che negozia la risoluzione con
la camera in modo affidabile (a differenza di cv2.VideoCapture generico via
UVC/V4L2, che spesso resta bloccato a risoluzioni basse). Il punto stimato
viene mediato sugli ultimi 10 frame per ridurre il jitter.
Richiede: pyrealsense2, opencv-contrib-python (per il modulo cv2.aruco), numpy
pip install pyrealsense2 opencv-contrib-python numpy
"""
from collections import deque
import cv2
import numpy as np
import pyrealsense2 as rs
# ------------------------- CONFIGURAZIONE -------------------------
# Calibrazione camera: SOSTITUISCI con i valori reali della tua camera
# (ottenuti con calibrate_d415.py). Senza calibrazione corretta la stima
# del centro sarà sbagliata.
CAMERA_MATRIX = np.array([
[931.62977526, 0.0, 648.9939992],
[ 0.0, 935.13433783, 366.09135937],
[ 0.0, 0.0, 1.0]
], dtype=np.float64)
DIST_COEFFS = np.zeros((5, 1), dtype=np.float64)
# Risoluzione/fps del canale RGB richiesti alla D415. Deve combaciare con la
# risoluzione usata in calibrate_d415.py (FRAME_W/FRAME_H), altrimenti
# fx/fy/cx/cy non sono più validi.
FRAME_W, FRAME_H, FPS = 1280, 720, 30
MARKER_LENGTH = 0.026 # lato del marker in metri: misuralo con un calibro
ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
DETECTOR_PARAMS = cv2.aruco.DetectorParameters()
DETECTOR = cv2.aruco.ArucoDetector(ARUCO_DICT, DETECTOR_PARAMS)
# Se true, inverte il segno della normale (dipende da come è orientato
# il marker rispetto alla faccia: prova entrambi i valori e guarda quale
# fa convergere il punto rosso dentro il solido)
FLIP_NORMAL = True
# Numero di stime consecutive su cui calcolare la media mobile del centro
SMOOTHING_WINDOW = 10
WINDOW_NAME = "Icosaedro - stima centro 3D"
def marker_object_points(length):
"""Punti 3D degli angoli del marker nel suo sistema locale, piano z=0,
ordine: alto-sx, alto-dx, basso-dx, basso-sx (ordine standard ArUco)."""
h = length / 2.0
return np.array([
[-h, h, 0],
[ h, h, 0],
[ h, -h, 0],
[-h, -h, 0]
], dtype=np.float64)
OBJ_POINTS = marker_object_points(MARKER_LENGTH)
def estimate_marker_pose(corners, camera_matrix):
"""Pose di un singolo marker via solvePnP (sostituisce l'estimatePoseSingleMarkers
ormai deprecato). Ritorna rvec, tvec."""
ok, rvec, tvec = cv2.solvePnP(
OBJ_POINTS, corners.reshape(-1, 2), camera_matrix, DIST_COEFFS,
flags=cv2.SOLVEPNP_IPPE_SQUARE
)
return rvec, tvec
def closest_point_to_lines(points, directions):
"""
Trova il punto 3D che minimizza la somma delle distanze quadratiche da un
insieme di rette skew, ciascuna definita da (punto, direzione unitaria).
Risolve A x = b con A = sum(I - d d^T), b = sum((I - d d^T) p).
"""
A = np.zeros((3, 3))
b = np.zeros(3)
I = np.eye(3)
for p, d in zip(points, directions):
d = d / np.linalg.norm(d)
M = I - np.outer(d, d)
A += M
b += M @ p
x, *_ = np.linalg.lstsq(A, b, rcond=None)
return x
def open_realsense_color_stream():
"""Apre il pipeline RealSense sul solo canale colore, come in
calibrate_d415.py. Ritorna il pipeline avviato."""
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, FRAME_W, FRAME_H, rs.format.bgr8, FPS)
profile = pipeline.start(config)
color_profile = rs.video_stream_profile(profile.get_stream(rs.stream.color))
intr = color_profile.get_intrinsics()
print(f"Stream RGB D415 avviato: {intr.width}x{intr.height}@{FPS}fps")
if (intr.width, intr.height) != (FRAME_W, FRAME_H):
print("ATTENZIONE: la risoluzione effettiva riportata dalla camera "
f"({intr.width}x{intr.height}) non combacia con quella richiesta "
f"({FRAME_W}x{FRAME_H}). Verifica che FRAME_W/FRAME_H siano supportati "
"da questo sensore (realsense-viewer per l'elenco modalità).")
return pipeline
def main():
pipeline = open_realsense_color_stream()
# Finestra ridimensionabile dall'utente (trascinando i bordi)
cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)
sign = -1.0 if FLIP_NORMAL else 1.0
# Buffer circolare con gli ultimi N centri stimati, per la media mobile
center_history = deque(maxlen=SMOOTHING_WINDOW)
try:
while True:
frames = pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
if not color_frame:
continue
frame = np.asanyarray(color_frame.get_data())
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, _ = DETECTOR.detectMarkers(gray)
line_points, line_dirs = [], []
if ids is not None:
cv2.aruco.drawDetectedMarkers(frame, corners, ids)
for c in corners:
rvec, tvec = estimate_marker_pose(c, CAMERA_MATRIX)
R, _ = cv2.Rodrigues(rvec)
# Normale della faccia in coordinate camera (asse Z locale del marker)
normal = R @ np.array([0.0, 0.0, 1.0])
center = tvec.reshape(3)
# Il centro dell'icosaedro sta lungo la normale, dalla parte
# opposta a dove punta il marker (verso "dentro" al solido)
line_points.append(center)
line_dirs.append(sign * normal)
cv2.drawFrameAxes(frame, CAMERA_MATRIX, DIST_COEFFS, rvec, tvec, MARKER_LENGTH * 0.5)
if len(line_points) >= 2:
center3d = closest_point_to_lines(line_points, line_dirs)
center_history.append(center3d)
# Media mobile sugli ultimi SMOOTHING_WINDOW valori disponibili
center_avg = np.mean(center_history, axis=0)
img_pt, _ = cv2.projectPoints(
center_avg.reshape(1, 3), np.zeros(3), np.zeros(3),
CAMERA_MATRIX, DIST_COEFFS
)
x, y = img_pt.ravel().astype(int)
cv2.circle(frame, (x, y), 6, (0, 0, 255), -1)
cv2.putText(
frame,
f"centro (media {len(center_history)}/{SMOOTHING_WINDOW}): "
f"{center_avg[0]:.3f}, {center_avg[1]:.3f}, {center_avg[2]:.3f} m",
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2
)
else:
cv2.putText(frame, "servono >=2 marker visibili", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 165, 255), 2)
cv2.imshow(WINDOW_NAME, frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
finally:
pipeline.stop()
cv2.destroyAllWindows()
if __name__ == "__main__":
main()
