RealSense Circle with Depth Detection

gistfile1.txt

import pyrealsense2 as rs
import cv2
import numpy as np

# Configure RealSense pipeline
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start streaming
profile = pipeline.start(config)

try:
    while True:
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        if not depth_frame or not color_frame:
            continue

        # Convert images to numpy arrays
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

        # Convert color image to grayscale
        gray_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)

        # Apply median blur to reduce noise
        gray_image = cv2.medianBlur(gray_image, 5)

        # Detect circles using HoughCircles
        circles = cv2.HoughCircles(
            gray_image,
            cv2.HOUGH_GRADIENT,
            dp=1,
            minDist=100, # Minimum distance between the centers of detected circles
            param1=100,  # Higher threshold for Canny edge detector
            param2=30,   # Accumulator threshold for circle detection
            minRadius=10,
            maxRadius=200
        )

        if circles is not None:
            circles = np.uint16(np.around(circles))
            for i in circles[0, :]:
                center_x, center_y, radius = i[0], i[1], i[2]

                # Draw the outer circle
                cv2.circle(color_image, (center_x, center_y), radius, (0, 255, 0), 2)
                # Draw the center of the circle
                cv2.circle(color_image, (center_x, center_y), 2, (0, 0, 255), 3)

                # Get 3D depth at the center of the circle
                depth_value = depth_frame.get_distance(center_x, center_y)
                # print(f"Circle at (x:{center_x}, y:{center_y}), Radius: {radius}, Depth: {depth_value:.2f} meters")

        # Display the resulting image
        cv2.imshow('Detected Circles with Depth', color_image)

        # Exit on 'q' key press
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

finally:
    # Stop streaming
    pipeline.stop()
    cv2.destroyAllWindows()