-
Notifications
You must be signed in to change notification settings - Fork 0
/
mp_rs_code_sample.py
144 lines (115 loc) · 5.28 KB
/
mp_rs_code_sample.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
# ====== Sample Code for Smart Design Technology Blog ======
# Intel Realsense D435 cam has RGB camera with 1920×1080 resolution
# Depth camera is 1280x720
# FOV is limited to 69deg x 42deg (H x V) - the RGB camera FOV
# If you run this on a non-Intel CPU, explore other options for rs.align
# On the NVIDIA Jetson AGX we build the pyrealsense lib with CUDA
import pyrealsense2 as rs
import mediapipe as mp
import cv2
import numpy as np
import datetime as dt
font = cv2.FONT_HERSHEY_SIMPLEX
org = (20, 100)
fontScale = .5
color = (0,50,255)
thickness = 1
# ====== Realsense ======
realsense_ctx = rs.context()
connected_devices = [] # List of serial numbers for present cameras
for i in range(len(realsense_ctx.devices)):
detected_camera = realsense_ctx.devices[i].get_info(rs.camera_info.serial_number)
print(f"{detected_camera}")
connected_devices.append(detected_camera)
device = connected_devices[0] # In this example we are only using one camera
pipeline = rs.pipeline()
config = rs.config()
background_removed_color = 153 # Grey
# ====== Mediapipe ======
mpHands = mp.solutions.hands
hands = mpHands.Hands()
mpDraw = mp.solutions.drawing_utils
# ====== Enable Streams ======
config.enable_device(device)
# # For worse FPS, but better resolution:
# stream_res_x = 1280
# stream_res_y = 720
# # For better FPS. but worse resolution:
stream_res_x = 640
stream_res_y = 480
stream_fps = 30
config.enable_stream(rs.stream.depth, stream_res_x, stream_res_y, rs.format.z16, stream_fps)
config.enable_stream(rs.stream.color, stream_res_x, stream_res_y, rs.format.bgr8, stream_fps)
profile = pipeline.start(config)
align_to = rs.stream.color
align = rs.align(align_to)
# ====== Get depth Scale ======
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print(f"\tDepth Scale for Camera SN {device} is: {depth_scale}")
# ====== Set clipping distance ======
clipping_distance_in_meters = 2
clipping_distance = clipping_distance_in_meters / depth_scale
print(f"\tConfiguration Successful for SN {device}")
# ====== Get and process images ======
print(f"Starting to capture images on SN: {device}")
while True:
start_time = dt.datetime.today().timestamp()
# Get and align frames
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
# Process images
depth_image = np.asanyarray(aligned_depth_frame.get_data())
depth_image_flipped = cv2.flip(depth_image,1)
color_image = np.asanyarray(color_frame.get_data())
depth_image_3d = np.dstack((depth_image,depth_image,depth_image)) #Depth image is 1 channel, while color image is 3
background_removed = np.where((depth_image_3d > clipping_distance) | (depth_image_3d <= 0), background_removed_color, color_image)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
images = cv2.flip(background_removed,1)
color_image = cv2.flip(color_image,1)
color_images_rgb = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
# Process hands
results = hands.process(color_images_rgb)
if results.multi_hand_landmarks:
number_of_hands = len(results.multi_hand_landmarks)
i=0
for handLms in results.multi_hand_landmarks:
mpDraw.draw_landmarks(images, handLms, mpHands.HAND_CONNECTIONS)
org2 = (20, org[1]+(20*(i+1)))
hand_side_classification_list = results.multi_handedness[i]
hand_side = hand_side_classification_list.classification[0].label
middle_finger_knuckle = results.multi_hand_landmarks[i].landmark[9]
x = int(middle_finger_knuckle.x*len(depth_image_flipped[0]))
y = int(middle_finger_knuckle.y*len(depth_image_flipped))
if x >= len(depth_image_flipped[0]):
x = len(depth_image_flipped[0]) - 1
if y >= len(depth_image_flipped):
y = len(depth_image_flipped) - 1
mfk_distance = depth_image_flipped[y,x] * depth_scale # meters
mfk_distance_feet = mfk_distance * 3.281 # feet
images = cv2.putText(images, f"{hand_side} Hand Distance: {mfk_distance_feet:0.3} feet ({mfk_distance:0.3} m) away", org2, font, fontScale, color, thickness, cv2.LINE_AA)
i+=1
images = cv2.putText(images, f"Hands: {number_of_hands}", org, font, fontScale, color, thickness, cv2.LINE_AA)
else:
images = cv2.putText(images,"No Hands", org, font, fontScale, color, thickness, cv2.LINE_AA)
# Display FPS
time_diff = dt.datetime.today().timestamp() - start_time
fps = int(1 / time_diff)
org3 = (20, org[1] + 60)
images = cv2.putText(images, f"FPS: {fps}", org3, font, fontScale, color, thickness, cv2.LINE_AA)
name_of_window = 'SN: ' + str(device)
# Display images
cv2.namedWindow(name_of_window, cv2.WINDOW_AUTOSIZE)
cv2.imshow(name_of_window, images)
key = cv2.waitKey(1)
# Press esc or 'q' to close the image window
if key & 0xFF == ord('q') or key == 27:
print(f"User pressed break key for SN: {device}")
break
print(f"Application Closing")
pipeline.stop()
print(f"Application Closed.")