-
Notifications
You must be signed in to change notification settings - Fork 16
/
rsAruco.py
137 lines (112 loc) · 5.16 KB
/
rsAruco.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
# coding: utf-8
# Author: Francis (Github @heretic1993)
# License: MIT
import import_ipynb
import threading
import time
import pyrealsense2 as rs
import numpy as np
import cv2
import matplotlib.pyplot as plt
import cv2.aruco as aruco
get_ipython().run_line_magic('matplotlib', 'inline')
center=np.array([1, 2, 3], dtype='float64')
class cameraDetection (threading.Thread):
def __init__(self, threadID, name):
threading.Thread.__init__(self)
self.threadID = threadID
self.name = name
self.__flag = threading.Event() # 用于暂停线程的标识
self.__flag.set() # 设置为True
self.__running = threading.Event() # 用于停止线程的标识
self.__running.set() # 将running设置为True
def run(self):
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 streaming
print("Start streaming")
pipeline.start(config)
##########################
# stream = pipeline.get_stream(rs.stream.depth)
# intrinsics = stream.get_intrinsics()
##########################
while cv2.waitKey(1)<0 and self.__running.isSet():
global color_image
# global color_image
self.__flag.wait() # 为True时立即返回, 为False时阻塞直到内部的标识位为True后返回
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
# get intrinsic of color image
color_intrinsics = color_frame.profile.as_video_stream_profile().intrinsics
# Convert images to numpy arrays
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
# print(depth_image.shape)
# print(color_image.shape)
# Apply colormap on depth image (image must be converted to 8-bit per pixel first)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
# Stack both images horizontally
# images = np.hstack((color_image, depth_colormap))
# Show images
# cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
# cv2.imshow("RealSense", images)
# plt.figure(1)
# plt.subplot(121)
# plt.imshow(color_image)
# plt.subplot(122)
# plt.imshow(depth_image)
# cv2.waitKey(1)
# Our operations on the frame come here
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
parameters = aruco.DetectorParameters_create()
#lists of ids and the corners beloning to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
# print(corners)
if len(corners)!=0:
point = np.average(corners[0][0], axis=0)
depth = depth_frame.get_distance(point[0], point[1])
point = np.append(point,depth)
if depth!=0:
global center
# print("center:%f %f, depth:%f m" %(point[0], point[1], point[2]))
x=point[0]
y=point[1]
z=point[2]
## see rs2 document:
## https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#point-coordinates
## and example: https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0#point-coordinates
x,y,z=rs.rs2_deproject_pixel_to_point(color_intrinsics, [x, y], z)
center=[x,y,z]
# print(center)
color_image = aruco.drawDetectedMarkers(color_image, corners)
# cv2.imwrite('./color.jpg',color_image)
#print(rejectedImgPoints)
# Display the resulting frame
# print("about to show!")
# cv2.startWindowThread()
## if uncommented, crash!!!
# cv2.namedWindow('Detection', cv2.WINDOW_AUTOSIZE)
# cv2.imshow("Detection", color_image)
# cv2.waitKey(1)
# Stop streaming
cv2.destroyAllWindows()
pipeline.stop()
time.sleep(1)
def pause(self):
self.__flag.clear() # 设置为False, 让线程阻塞
def resume(self):
self.__flag.set() # 设置为True, 让线程停止阻塞
def stop(self):
self.__flag.set() # 将线程从暂停状态恢复, 如何已经暂停的话
self.__running.clear() # 设置为False
# In[ ]:
def main():
thread = cameraDetection(1, "rsArucoDetection")
thread.start()
if __name__ == '__main__':
main()