transferred aruco marker position tracker from other repo

d435i-aruco/aruco_depth-test.py

@@ -0,0 +1,76 @@
+import pyrealsense2 as rs
+import numpy as np
+import cv2
+import cv2.aruco as aruco
+
+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)
+
+profile = pipeline.start(config)
+
+# brooo I forgot to use rs-align to align the two cameras since there is physics offset :((
+align_to = rs.stream.color
+align = rs.align(align_to)
+
+# using 6x6_250 Dict
+aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
+aruco_params = aruco.DetectorParameters_create()
+
+try:
+    while True:
+        frames = pipeline.wait_for_frames()
+
+        # Align the depth frame to color frame
+        aligned_frames = align.process(frames)
+
+        # Get aligned frames
+        aligned_depth_frame = aligned_frames.get_depth_frame()
+        color_frame = aligned_frames.get_color_frame()
+
+        if not aligned_depth_frame or not color_frame:
+            continue
+
+        # Convert images to numpy arrays
+        depth_image = np.asanyarray(aligned_depth_frame.get_data())
+        color_image = np.asanyarray(color_frame.get_data())
+
+        # Aruco marker detection
+        gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
+        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=aruco_params)
+
+        # Visualize the depth image
+        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
+
+        if ids is not None:
+            # Color image detection
+            aruco.drawDetectedMarkers(color_image, corners, ids)
+
+            # Depth image detection
+            aruco.drawDetectedMarkers(depth_colormap, corners, ids)
+
+            for i in range(len(ids)):
+                # Get the corner coordinates to draw the line
+                corner = corners[i][0]
+                x = int(corner[:, 0].mean())
+                y = int(corner[:, 1].mean())
+
+                # Get depth value from depth image
+                depth = aligned_depth_frame.get_distance(x, y)
+
+                # Display the distance and ID in color image
+                cv2.putText(color_image, f"ID {ids[i][0]}: {depth:.2f}m", (x, y), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 2)
+
+                # depth colormap display
+                cv2.putText(depth_colormap, f"ID {ids[i][0]}: {depth:.2f}m", (x, y), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 0), 2)
+
+        cv2.imshow('RealSense Color', color_image)
+        cv2.imshow('RealSense Depth', depth_colormap)
+
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            break
+
+finally:
+    pipeline.stop()
+    cv2.destroyAllWindows()

d435i-aruco/aruco_module.py

@@ -0,0 +1,97 @@
+import pyrealsense2 as rs
+import numpy as np
+import cv2
+import cv2.aruco as aruco
+
+class ArucoDetector:
+    def __init__(self):
+        self.pipeline = rs.pipeline()
+        self.config = rs.config()
+        self.config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
+        self.config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
+
+        # Align the two cameras since there is physical offset
+        self.align_to = rs.stream.color
+        self.align = rs.align(self.align_to)
+
+        # Use 6x6_250 Dict aruco library
+        self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
+        self.aruco_params = aruco.DetectorParameters_create()
+
+        # Dictionary to hold marker positions
+        self.marker_positions = {}
+
+        # Variable to hold camera intrinsics
+        self.intrinsics = None
+
+    def start_stream(self):
+        # Start the pipeline
+        self.profile = self.pipeline.start(self.config)
+        # Get the camera intrinsics from the color stream
+        self.intrinsics = self.profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
+
+    def stop_stream(self):
+        # Stop the pipeline
+        self.pipeline.stop()
+
+    def process_frames(self):
+        frames = self.pipeline.wait_for_frames()
+        aligned_frames = self.align.process(frames)
+        aligned_depth_frame = aligned_frames.get_depth_frame()
+        color_frame = aligned_frames.get_color_frame()
+
+        if not aligned_depth_frame or not color_frame:
+            return None, None, None
+
+        # Convert color image to numpy array
+        color_image = np.asanyarray(color_frame.get_data())
+
+        return aligned_depth_frame, color_image
+
+    def update_marker_positions(self, color_image, depth_frame):
+        # Detect ArUco markers
+        gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
+        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruco_params)
+
+        if ids is not None:
+            # Draw detected markers
+            aruco.drawDetectedMarkers(color_image, corners, ids)
+
+            for i in range(len(ids)):
+                # Average the corners for better center estimation
+                center = np.mean(corners[i][0], axis=0)
+                x_pixel, y_pixel = int(center[0]), int(center[1])
+
+                # Get depth value
+                depth = depth_frame.get_distance(x_pixel, y_pixel)
+
+                # Deproject pixels to 3D space
+                point_3d = rs.rs2_deproject_pixel_to_point(self.intrinsics, [x_pixel, y_pixel], depth)
+
+                # Adjust y-coordinate so that up is positive y
+                x, y, z = point_3d
+                y = -y  
+
+                # Convert from meters to centimeters
+                # consistently 6mm too much?
+                x_cm, y_cm, z_cm = x * 100, y * 100, z * 100
+
+                # Update the dictionary with the latest position data
+                self.marker_positions[ids[i][0]] = [x_cm, y_cm, z_cm]
+
+                # Display the 3D coordinates next to the marker
+                # display x, y, z in different lines
+                coord_text = (f"ID {ids[i][0]}:\n"
+                              f"X={x_cm:.2f}cm\n"
+                              f"Y={y_cm:.2f}cm\n"
+                              f"Z={z_cm:.2f}cm")
+                for j, line in enumerate(coord_text.split('\n')):
+                    y_offset = y_pixel + 20 + (15 * j)
+                    cv2.putText(color_image, line, (x_pixel + 20, y_offset), 
+                                cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 2)
+        return color_image
+
+    def get_marker_position(self, marker_id):
+        # Retrieve the latest recorded position of a marker by its ID
+        # return is x, y, z as a list of floats
+        return self.marker_positions.get(marker_id)

d435i-aruco/aruco_pose-tracker.py

@@ -0,0 +1,81 @@
+import pyrealsense2 as rs
+import numpy as np
+import cv2
+import cv2.aruco as aruco
+
+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 the pipeline
+profile = pipeline.start(config)
+
+# forgot i had to align the two camera since there is physical offset :(((
+align_to = rs.stream.color
+align = rs.align(align_to)
+
+# Use 6x6_250 Dict aruco library
+aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
+aruco_params = aruco.DetectorParameters_create()
+
+# Get the camera intrinsics from the color stream
+intrinsics = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
+
+# Dictionary to hold marker positions
+marker_positions = {}
+# Function to retrieve the latest recorded position of a marker by its ID
+def get_marker_position(marker_id):
+    return marker_positions.get(marker_id, "Marker not found")
+
+try:
+    while True:
+        frames = pipeline.wait_for_frames()
+        aligned_frames = align.process(frames)
+        aligned_depth_frame = aligned_frames.get_depth_frame()
+        color_frame = aligned_frames.get_color_frame()
+
+        if not aligned_depth_frame or not color_frame:
+            continue
+
+        # Convert images to numpy arrays
+        depth_image = np.asanyarray(aligned_depth_frame.get_data())
+        color_image = np.asanyarray(color_frame.get_data())
+
+        # Detect ArUco markers
+        gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
+        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=aruco_params)
+
+        if ids is not None:
+            # Draw detected markers
+            aruco.drawDetectedMarkers(color_image, corners, ids)
+
+            for i in range(len(ids)):
+                # Average the corners for better center estimation
+                center = np.mean(corners[i][0], axis=0)
+                x_pixel, y_pixel = int(center[0]), int(center[1])
+
+                # Get depth value
+                depth = aligned_depth_frame.get_distance(x_pixel, y_pixel)
+
+                # Deproject pixels to 3D space
+                point_3d = rs.rs2_deproject_pixel_to_point(intrinsics, [x_pixel, y_pixel], depth)
+
+                # Adjust y-coordinate to have positive values above the center
+                # idk why realsense makes below the center positive for y lol
+                x, y, z = point_3d
+                y = -y 
+
+                # Display the 3D coordinates
+                cv2.putText(color_image, f"ID {ids[i][0]} XYZ: {x:.2f}, {y:.2f}, {z:.2f}", 
+                            (x_pixel - 100, y_pixel - 15), cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 2)
+
+        # Display rbg frames
+        cv2.imshow('RealSense Color', color_image)
+
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            break
+
+finally:
+    pipeline.stop()
+    cv2.destroyAllWindows()

d435i-aruco/controller.py

@@ -0,0 +1,23 @@
+# the size of my aruco tags is 1.97inch, or 0.05m
+from aruco_module import ArucoDetector
+import cv2
+
+aruco = ArucoDetector()
+aruco.start_stream()
+
+try:
+    while True:
+        aligned_depth_frame, color_image = aruco.process_frames()
+        if color_image is not None and aligned_depth_frame is not None:
+            color_image = aruco.update_marker_positions(color_image, aligned_depth_frame)
+            cv2.imshow('RealSense Color', color_image)
+
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            break
+finally:
+    aruco.stop_stream()
+    cv2.destroyAllWindows()
+
+# To get the position of a specific marker by ID, for example, ID 2:
+position = aruco.get_marker_position(2)
+print(position)

d435i-aruco/generate_aruco.py

@@ -0,0 +1,24 @@
+import cv2
+import cv2.aruco as aruco
+import numpy as np
+
+# Define the ArUco dictionary (Change as needed)
+aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
+
+# Specify the IDs of ArUco tags to generate
+ids_to_generate = [0, 1, 2, 3, 4, 5]
+
+# Size of the ArUco marker images (in pixels)
+marker_size = 200
+
+for id in ids_to_generate:
+    # Generate the marker
+    img = aruco.drawMarker(aruco_dict, id, marker_size)
+
+    # Save the marker to a file
+    cv2.imwrite(f"aruco_marker_{id}.png", img)
+
+    # Optionally display the generated marker
+    cv2.imshow(f"Marker ID: {id}", img)
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()