jakaskerl Hey Jaka thanks for the reply, the depth calculation is on and i thought i have to align it to one of the mono cameras ?
Here is the rest off the code:
# Funktion zur Erkennung von ArUco-Markern
def detect_aruco_markers(frame):
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
parameters = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, parameters)
markerCorners, markerIds, _ = detector.detectMarkers(frame)
return markerCorners, markerIds
# Hauptfunktion
def main():
pipeline = create_pipeline()
# Mit der Pipeline verbinden
with dai.Device(pipeline) as device:
q_rgb = device.getOutputQueue(name="rgb", maxSize=4, blocking=False)
q_depth = device.getOutputQueue(name="depth", maxSize=4, blocking=False)
q_spatial_data = device.getOutputQueue(name="spatialData", maxSize=4, blocking=False)
spatial_data_config_in = device.getInputQueue("spatialLocationCalculatorConfig")
while True:
in_rgb = q_rgb.get()
in_depth = q_depth.get()
frame_rgb = in_rgb.getCvFrame()
frame_depth = in_depth.getFrame()
# ArUco-Marker erkennen
markerCorners, markerIds = detect_aruco_markers(frame_rgb)
if markerIds is not None:
#Marker Visualisieren
frame_rgb = cv2.aruco.drawDetectedMarkers(frame_rgb, markerCorners, markerIds)
config = dai.SpatialLocationCalculatorConfig()
for corners in markerCorners:
# Berechnung des Mittelpunkts des Markers
c = corners[0]
center_x = int(sum([point[0] for point in c]) / 4)
center_y = int(sum([point[1] for point in c]) / 4)
# Definieren einer festen Größe für den ROI oder Anpassen basierend auf der Größe des Markers
roi_size = 0.1 # Zum Beispiel 10% des Bildes für Breite und Höhe
roi = dai.Rect(
max(0, center_x/frame_rgb.shape[1] - roi_size / 2),
max(0, center_y/frame_rgb.shape[0] - roi_size / 2),
min(roi_size, 1 - (center_x/frame_rgb.shape[1])),
min(roi_size, 1 - (center_y/frame_rgb.shape[0]))
)
# Hinzufügen des ROI zur Konfiguration
configData = dai.SpatialLocationCalculatorConfigData()
configData.depthThresholds.lowerThreshold = 100
configData.depthThresholds.upperThreshold = 10000
configData.roi = roi
config.addROI(configData)
# Senden der SpatialLocationCalculator-Konfiguration
spatial_data_config_in.send(config)
# Überprüfen, ob räumliche Daten vorhanden sind
spatial_data = q_spatial_data.tryGet()
if spatial_data is not None:
for data in spatial_data.getSpatialLocations():
roi = data.config.roi
roi = roi.denormalize(frame_rgb.shape[1], frame_rgb.shape[0])
top_left = roi.topLeft()
bottom_right = roi.bottomRight()
center_x = int((top_left.x + bottom_right.x) / 2)
center_y = int((top_left.y + bottom_right.y) / 2)
spatial_x = data.spatialCoordinates.x
spatial_y = data.spatialCoordinates.y
spatial_z = data.spatialCoordinates.z
# Text für die Anzeige vorbereiten
text = f"X: {spatial_x:.1f} mm, Y: {spatial_y:.1f} mm, Z: {spatial_z:.1f} mm"
# Setzen des Textes auf das RGB-Bild
cv2.putText(frame_rgb, text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
# Ergebnisse anzeigen
frame_depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(frame_depth, alpha=0.09), cv2.COLORMAP_JET)
frame_depth_colormap_rotated = cv2.rotate(frame_depth_colormap, cv2.ROTATE_90_COUNTERCLOCKWISE) # Tiefenbild um 90 Grad drehen
frame_rgb_rotated = cv2.rotate(frame_rgb, cv2.ROTATE_90_COUNTERCLOCKWISE) # RGB-Bild um 90 Grad drehen
cv2.imshow('RGB', frame_rgb_rotated) # Gedrehtes RGB-Bild anzeigen
cv2.imshow('Depth', frame_depth_colormap_rotated) # Gedrehtes Tiefenbild mit Farbschema anzeigen
if cv2.waitKey(1) == ord('q'):
break
if name == "main":
main()
cv2.destroyAllWindows()
