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
| import pyrealsense2 as rs import numpy as np import cv2 import sys import math
pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
profile = pipeline.start(config)
mouse_config_p = 0
S0 = 4*math.tan(34.7/180*math.pi)*math.tan(21.25/180*math.pi)/1280/720
x_min = 0 y_min = 0 x_max = 0 y_max = 0
def onmouse(event, x, y, flags, param): global mouse_config_p,x_min,y_min,x_max,y_max if event==cv2.EVENT_LBUTTONDOWN: if mouse_config_p == 0 : x_min = x y_min = y mouse_config_p += 1 elif mouse_config_p == 1: x_max = x y_max = y mouse_config_p += 1 with open("./config.ini","w") as f: f.write(str(x_min)) f.write(" ") f.write(str(y_min)) f.write(" ") f.write(str(x_max)) f.write(" ") f.write(str(y_max))
def Region_config(): finish = False while finish == False: cv2.namedWindow('FreshCompanion') cv2.setMouseCallback('FreshCompanion', onmouse) frames = pipeline.wait_for_frames() color_frame = frames.get_color_frame() if not color_frame : continue color_image = np.asanyarray(color_frame.get_data()) cv2.imshow("FreshCompanion", color_image) cv2.waitKey(1) if x_min != 0 and y_min != 0 and x_max != 0 and y_max != 0: finish = True
def get_V(): while True:
for i in range(5): pipeline.wait_for_frames()
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
depth_image = np.asanyarray(depth_frame.get_data()) color_image = np.asanyarray(color_frame.get_data())
depth = np.asanyarray(depth_frame.get_data()) depth = depth[y_min:y_max,x_min:x_max].astype(float)
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale() depth = depth[depth!=0] depth = depth * depth_scale V0 = depth*depth*depth*S0*1000 V = np.sum(V0) dist, _, _, _ = cv2.mean(depth) cv2.rectangle(color_image, (x_min, y_min),(x_max, y_max), (255, 255, 255), 2) cv2.putText(color_image, "V = " + str(V) + "L", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) cv2.putText(color_image, "Dis = " + str(dist) + "m", (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) cv2.imshow("FreshCompanion", color_image) cv2.waitKey(1)
Region_config() get_V()
|