|
@@ -0,0 +1,98 @@
|
|
|
+import pyrealsense2 as rs
|
|
|
+import numpy as np
|
|
|
+import cv2
|
|
|
+import supervision as sv
|
|
|
+from ultralytics import YOLO
|
|
|
+import time
|
|
|
+
|
|
|
+# 啟動 RealSense 相機
|
|
|
+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)
|
|
|
+pipeline.start(config)
|
|
|
+# profile = config.resolve(pipeline)
|
|
|
+# # Declare sensor object and set options
|
|
|
+# depth_sensor = profile.get_device().first_depth_sensor()
|
|
|
+# depth_sensor.set_option(rs.option.visual_preset, 5) # 5 is short range, 3 is low ambient light
|
|
|
+
|
|
|
+# 初始化 YOLO 模型
|
|
|
+model = YOLO('yolov8n-seg.pt')
|
|
|
+byte_tracker = sv.ByteTrack()
|
|
|
+
|
|
|
+align_to = rs.stream.color
|
|
|
+align = rs.align(align_to)
|
|
|
+
|
|
|
+try:
|
|
|
+ while True:
|
|
|
+ # 讀取 RealSense 相機影像
|
|
|
+ frames = pipeline.wait_for_frames()
|
|
|
+ aligned_frames = align.process(frames)
|
|
|
+ depth_frame = aligned_frames.get_depth_frame()
|
|
|
+ color_frame = aligned_frames.get_color_frame()
|
|
|
+
|
|
|
+ if not depth_frame or not color_frame:
|
|
|
+ continue
|
|
|
+
|
|
|
+ depth_image = np.asanyarray(depth_frame.get_data())
|
|
|
+ color_image = np.asanyarray(color_frame.get_data())
|
|
|
+ depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_WINTER)
|
|
|
+
|
|
|
+ # 進行物體檢測
|
|
|
+ result = model(color_image)[0]
|
|
|
+ detections = sv.Detections.from_ultralytics(result)
|
|
|
+ detections_tracker = byte_tracker.update_with_detections(detections=detections)
|
|
|
+
|
|
|
+ polygon_annotator = sv.PolygonAnnotator()
|
|
|
+ annotated_frame = polygon_annotator.annotate(
|
|
|
+ scene=color_image.copy(), detections=detections)
|
|
|
+
|
|
|
+ mask_annotator = sv.MaskAnnotator()
|
|
|
+ annotated_frame = mask_annotator.annotate(
|
|
|
+ scene=annotated_frame, detections=detections
|
|
|
+ )
|
|
|
+
|
|
|
+ labels = [
|
|
|
+ f"#{tracker_id} {result.names[class_id]} {confidence:.2f}"
|
|
|
+ for class_id, tracker_id, confidence
|
|
|
+ in zip(detections_tracker.class_id, detections_tracker.tracker_id, detections_tracker.confidence)
|
|
|
+ ]
|
|
|
+
|
|
|
+ label_annotator = sv.LabelAnnotator()
|
|
|
+ annotated_frame = label_annotator.annotate(
|
|
|
+ scene=annotated_frame,
|
|
|
+ detections=detections_tracker,
|
|
|
+ labels=labels
|
|
|
+ )
|
|
|
+
|
|
|
+ if detections:
|
|
|
+ position = sv.Position.CENTER_OF_MASS
|
|
|
+ xy = detections.get_anchors_coordinates(anchor=position)
|
|
|
+ #print('postion',xy)
|
|
|
+ for detection_idx in range(len(detections)):
|
|
|
+ #print(xy)
|
|
|
+ center = (int(xy[detection_idx,0]), int(xy[detection_idx,1]))
|
|
|
+ cv2.circle(annotated_frame, center,radius=3, color=(255,255,0), thickness=-1)
|
|
|
+ #print(center[0],center[1])
|
|
|
+
|
|
|
+ # 在畫面上顯示距離信息和檢測結果
|
|
|
+ text_depth = "Depth value at ({x},{y}): {:.2f} meters".format(
|
|
|
+ depth_frame.get_distance(int(xy[detection_idx,0]), int(xy[detection_idx,1])),x = int(xy[detection_idx,0]),y=int(xy[detection_idx,1]))
|
|
|
+ annotated_frame = cv2.putText(annotated_frame, text_depth, (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
|
+ (0, 0, 255), 2)
|
|
|
+ print("距離:",depth_frame.get_distance(int(xy[detection_idx,0]), int(xy[detection_idx,1])))
|
|
|
+
|
|
|
+ #annotated_frame = cv2.circle(annotated_frame, (320, 240), 4, (0, 0, 255), -1)
|
|
|
+
|
|
|
+ annotated_frame = np.hstack((annotated_frame, depth_colormap))
|
|
|
+
|
|
|
+ # 顯示畫面
|
|
|
+ cv2.imshow('RealSense', annotated_frame)
|
|
|
+
|
|
|
+ key = cv2.waitKey(1)
|
|
|
+ if key & 0xFF == ord('q') or key == 27:
|
|
|
+ cv2.destroyAllWindows()
|
|
|
+ break
|
|
|
+
|
|
|
+finally:
|
|
|
+ pipeline.stop()
|