Untitled

#!/usr/bin/env python3

import cv2
import depthai as dai
import time
import numpy as np

# Create the pipeline and configure nodes
pipeline = dai.Pipeline()

camRgb = pipeline.create(dai.node.ColorCamera)
monoLeft = pipeline.create(dai.node.MonoCamera)
monoRight = pipeline.create(dai.node.MonoCamera)
aprilTag = pipeline.create(dai.node.AprilTag)
manip = pipeline.create(dai.node.ImageManip)
stereo = pipeline.create(dai.node.StereoDepth)

xoutAprilTag = pipeline.create(dai.node.XLinkOut)
xoutAprilTagImage = pipeline.create(dai.node.XLinkOut)
xoutDepth = pipeline.create(dai.node.XLinkOut)

xoutAprilTag.setStreamName("aprilTagData")
xoutAprilTagImage.setStreamName("aprilTagImage")
xoutDepth.setStreamName("depthData")

# Cameras properties
camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_720_P)
monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
camRgb.setCamera("color")
monoLeft.setCamera("left")
monoRight.setCamera("right")

# Stereo properties
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7)
stereo.setLeftRightCheck(True)
stereo.setExtendedDisparity(False)

# Image manipulation properties
manip.initialConfig.setResize(480, 270)
manip.initialConfig.setFrameType(dai.ImgFrame.Type.GRAY8)

# AprilTag detection properties
aprilTag.initialConfig.setFamily(dai.AprilTagConfig.Family.TAG_36H11)

# Link the nodes
camRgb.video.link(manip.inputImage)
manip.out.link(aprilTag.inputImage)
aprilTag.out.link(xoutAprilTag.input)
aprilTag.passthroughInputImage.link(xoutAprilTagImage.input)
monoLeft.out.link(stereo.left)
monoRight.out.link(stereo.right)
stereo.depth.link(xoutDepth.input)

# Start the pipeline
with dai.Device(pipeline) as device:
    # Get output queues
    manipQueue = device.getOutputQueue("aprilTagImage", 8, False)
    aprilTagQueue = device.getOutputQueue("aprilTagData", 8, False)
    depthQueue = device.getOutputQueue("depthData", 8, False)

    color = (0, 255, 0)

    startTime = time.monotonic()
    counter = 0
    fps = 0

    calib = device.readCalibration()
    intrinsics = calib.getCameraIntrinsics(dai.CameraBoardSocket.CAM_A, 1280, 720)
    fx, fy = intrinsics[0][0], intrinsics[1][1]
    cx, cy = intrinsics[0][2], intrinsics[1][2]

    depth_width, depth_height = 1280, 720
    tag_width, tag_height = 480, 270

    while(True):
        # Get a depth image and manipulated image
        inFrame = manipQueue.get()
        depthFrame = depthQueue.get()

        counter += 1
        current_time = time.monotonic()
        if (current_time - startTime) > 1 :
            fps = counter / (current_time - startTime)
            counter = 0
            startTime = current_time

        monoFrame = inFrame.getFrame()
        depthData = depthFrame.getFrame()

        # Convert the image to color
        frame = cv2.cvtColor(monoFrame, cv2.COLOR_GRAY2BGR)

        # Get the detected AprilTags
        aprilTagData = aprilTagQueue.get().aprilTags
        for aprilTag in aprilTagData:
            topLeft = aprilTag.topLeft
            topRight = aprilTag.topRight
            bottomRight = aprilTag.bottomRight
            bottomLeft = aprilTag.bottomLeft

            center = (int((topLeft.x + bottomRight.x) / 2), int((topLeft.y + bottomRight.y) / 2))

            # Draw the tag's bounding box
            cv2.line(frame, (int(topLeft.x), int(topLeft.y)), (int(topRight.x), int(topRight.y)), color, 2, cv2.LINE_AA, 0)
            cv2.line(frame, (int(topRight.x), int(topRight.y)), (int(bottomRight.x), int(bottomRight.y)), color, 2, cv2.LINE_AA, 0)
            cv2.line(frame, (int(bottomRight.x), int(bottomRight.y)), (int(bottomLeft.x), int(bottomLeft.y)), color, 2, cv2.LINE_AA, 0)
            cv2.line(frame, (int(bottomLeft.x), int(bottomLeft.y)), (int(topLeft.x), int(topLeft.y)), color, 2, cv2.LINE_AA, 0)

            # Calculate the 3D position of the tag
            x_depth = int(center[0] * depth_width / tag_width)
            y_depth = int(center[1] * depth_height / tag_height)
            x_depth = np.clip(x_depth, 0, depth_width - 1)
            y_depth = np.clip(y_depth, 0, depth_height - 1)

            # Calculate the 3D position in the real world (in m)
            Z = depthData[y_depth, x_depth] / 1000
            X = (x_depth - cx) * Z / fx
            Y = (y_depth - cy) * Z / fy

            # Display robot movement
            move_x = X  # Move left/right
            move_y = Y  # Move forward/backward
            print(f"Estimated robot movement: Move forward by {move_y:.2f} m, Move by {move_x:.2f} m")

            # Draw the tag ID and estimated 3D position
            idStr = f"ID: {aprilTag.id}, Pos: ({X:.2f}, {Y:.2f}, {Z:.2f})"
            cv2.putText(frame, idStr, center, cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)

        # Calculate and display FPS
        cv2.putText(frame, "Fps: {:.2f}".format(fps), (2, frame.shape[0] - 4), cv2.FONT_HERSHEY_TRIPLEX, 0.4, (255, 255, 255))

        # Show the image with detected tags and estimated 3D position
        cv2.imshow("AprilTag Detection", frame)

        if cv2.waitKey(1) == ord('q'):
            break