1 import pyrealsense2
as rs
9 pipeline = rs.pipeline()
11 config.enable_stream(rs.stream.depth, W, H, rs.format.z16, 30)
12 config.enable_stream(rs.stream.color, W, H, rs.format.bgr8, 30)
15 print(
"[INFO] start streaming...")
16 pipeline.start(config)
18 aligned_stream = rs.align(rs.stream.color)
19 point_cloud = rs.pointcloud()
21 print(
"[INFO] loading model...")
23 net = cv2.dnn.readNetFromTensorflow(
"frozen_inference_graph.pb",
"faster_rcnn_inception_v2_coco_2018_01_28.pbtxt")
25 frames = pipeline.wait_for_frames()
26 frames = aligned_stream.process(frames)
27 color_frame = frames.get_color_frame()
28 depth_frame = frames.get_depth_frame().as_depth_frame()
30 points = point_cloud.calculate(depth_frame)
31 verts = np.asanyarray(points.get_vertices()).
view(np.float32).
reshape(-1, W, 3)
34 depth_image = np.asanyarray(depth_frame.get_data())
36 if not np.any(depth_image):
38 print(
"[INFO] found a valid depth frame")
39 color_image = np.asanyarray(color_frame.get_data())
41 scaled_size = (int(W), int(H))
42 net.setInput(cv2.dnn.blobFromImage(color_image, size=scaled_size, swapRB=
True, crop=
False))
43 detections = net.forward()
45 print(
"[INFO] drawing bounding box on detected objects...")
47 for detection
in detections[0,0]:
48 score = float(detection[2])
49 idx = int(detection[1])
50 print(
" [DEBUG] classe : ",idx)
52 if score > 0.8
and idx == 0:
53 left = detection[3] * W
54 top = detection[4] * H
55 right = detection[5] * W
56 bottom = detection[6] * H
60 bbox = (int(left), int(top), int(width), int(height))
62 p1 = (int(bbox[0]), int(bbox[1]))
63 p2 = (int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3]))
64 cv2.rectangle(color_image, p1, p2, (255, 0, 0), 2, 1)
67 obj_points = verts[int(bbox[1]):int(bbox[1] + bbox[3]), int(bbox[0]):int(bbox[0] + bbox[2])].
reshape(-1, 3)
73 ys = np.delete(ys, np.where((zs < z - 1) | (zs > z + 1)))
75 my = np.amin(ys, initial=1)
76 My = np.amax(ys, initial=-1)
79 height = float(
"{:.2f}".format(height))
80 print(
"[INFO] object height is: ", height,
"[m]")
81 height_txt =
str(height)+
"[m]" 84 font = cv2.FONT_HERSHEY_SIMPLEX
85 bottomLeftCornerOfText = (p1[0], p1[1]+20)
87 fontColor = (255, 255, 255)
89 cv2.putText(color_image, height_txt,
90 bottomLeftCornerOfText,
97 cv2.namedWindow(
'RealSense', cv2.WINDOW_AUTOSIZE)
98 cv2.imshow(
'RealSense', color_image)
void reshape(GLFWwindow *window, int w, int h)
static std::string print(const transformation &tf)