/
lib_aruco_pose.py
executable file
·264 lines (175 loc) · 9.04 KB
/
lib_aruco_pose.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
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
"""
This demo calculates multiple things for different scenarios.
IF RUNNING ON A PI, BE SURE TO sudo modprobe bcm2835-v4l2
Here are the defined reference frames:
TAG:
A y
|
|
|tag center
O---------> x
CAMERA:
X--------> x
| frame center
|
|
V y
F1: Flipped (180 deg) tag frame around x axis
F2: Flipped (180 deg) camera frame around x axis
The attitude of a generic frame 2 respect to a frame 1 can obtained by calculating euler(R_21.T)
We are going to obtain the following quantities:
> from aruco library we obtain tvec and Rct, position of the tag in camera frame and attitude of the tag
> position of the Camera in Tag axis: -R_ct.T*tvec
> Transformation of the camera, respect to f1 (the tag flipped frame): R_cf1 = R_ct*R_tf1 = R_cf*R_f
> Transformation of the tag, respect to f2 (the camera flipped frame): R_tf2 = Rtc*R_cf2 = R_tc*R_f
> R_tf1 = R_cf2 an symmetric = R_f
"""
import numpy as np
import cv2
import cv2.aruco as aruco
import sys, time, math
class ArucoSingleTracker():
def __init__(self,
id_to_find,
marker_size,
camera_matrix,
camera_distortion,
camera_size=[640,480],
show_video=False
):
self.id_to_find = id_to_find
self.marker_size = marker_size
self._show_video = show_video
self._camera_matrix = camera_matrix
self._camera_distortion = camera_distortion
self.is_detected = False
self._kill = False
#--- 180 deg rotation matrix around the x axis
self._R_flip = np.zeros((3,3), dtype=np.float32)
self._R_flip[0,0] = 1.0
self._R_flip[1,1] =-1.0
self._R_flip[2,2] =-1.0
#--- Define the aruco dictionary
self._aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
self._parameters = aruco.DetectorParameters_create()
#--- Capture the videocamera (this may also be a video or a picture)
self._cap = cv2.VideoCapture(0)
#-- Set the camera size as the one it was calibrated with
self._cap.set(cv2.CAP_PROP_FRAME_WIDTH, camera_size[0])
self._cap.set(cv2.CAP_PROP_FRAME_HEIGHT, camera_size[1])
#-- Font for the text in the image
self.font = cv2.FONT_HERSHEY_PLAIN
self._t_read = time.time()
self._t_detect = self._t_read
self.fps_read = 0.0
self.fps_detect = 0.0
def _rotationMatrixToEulerAngles(self,R):
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def isRotationMatrix(R):
Rt = np.transpose(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype=R.dtype)
n = np.linalg.norm(I - shouldBeIdentity)
return n < 1e-6
assert (isRotationMatrix(R))
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2, 1], R[2, 2])
y = math.atan2(-R[2, 0], sy)
z = math.atan2(R[1, 0], R[0, 0])
else:
x = math.atan2(-R[1, 2], R[1, 1])
y = math.atan2(-R[2, 0], sy)
z = 0
return np.array([x, y, z])
def _update_fps_read(self):
t = time.time()
self.fps_read = 1.0/(t - self._t_read)
self._t_read = t
def _update_fps_detect(self):
t = time.time()
self.fps_detect = 1.0/(t - self._t_detect)
self._t_detect = t
def stop(self):
self._kill = True
def track(self, loop=True, verbose=False, show_video=None):
self._kill = False
if show_video is None: show_video = self._show_video
marker_found = False
x = y = z = 0
while not self._kill:
#-- Read the camera frame
ret, frame = self._cap.read()
self._update_fps_read()
#-- Convert in gray scale
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #-- remember, OpenCV stores color images in Blue, Green, Red
#-- Find all the aruco markers in the image
corners, ids, rejected = aruco.detectMarkers(image=gray, dictionary=self._aruco_dict,
parameters=self._parameters,
cameraMatrix=self._camera_matrix,
distCoeff=self._camera_distortion)
if not ids is None and self.id_to_find in ids[0]:
marker_found = True
self._update_fps_detect()
#-- ret = [rvec, tvec, ?]
#-- array of rotation and position of each marker in camera frame
#-- rvec = [[rvec_1], [rvec_2], ...] attitude of the marker respect to camera frame
#-- tvec = [[tvec_1], [tvec_2], ...] position of the marker in camera frame
ret = aruco.estimatePoseSingleMarkers(corners, self.marker_size, self._camera_matrix, self._camera_distortion)
#-- Unpack the output, get only the first
rvec, tvec = ret[0][0,0,:], ret[1][0,0,:]
x = tvec[0]
y = tvec[1]
z = tvec[2]
#-- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners)
aruco.drawAxis(frame, self._camera_matrix, self._camera_distortion, rvec, tvec, 10)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
#-- Get the attitude in terms of euler 321 (Needs to be flipped first)
roll_marker, pitch_marker, yaw_marker = self._rotationMatrixToEulerAngles(self._R_flip*R_tc)
#-- Now get Position and attitude f the camera respect to the marker
pos_camera = -R_tc*np.matrix(tvec).T
# print "Camera X = %.1f Y = %.1f Z = %.1f - fps = %.0f"%(pos_camera[0], pos_camera[1], pos_camera[2],fps_detect)
if verbose: print "Marker X = %.1f Y = %.1f Z = %.1f - fps = %.0f"%(tvec[0], tvec[1], tvec[2],self.fps_detect)
if show_video:
#-- Print the tag position in camera frame
str_position = "MARKER Position x=%4.0f y=%4.0f z=%4.0f"%(tvec[0], tvec[1], tvec[2])
cv2.putText(frame, str_position, (0, 100), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
#-- Print the marker's attitude respect to camera frame
str_attitude = "MARKER Attitude r=%4.0f p=%4.0f y=%4.0f"%(math.degrees(roll_marker),math.degrees(pitch_marker),
math.degrees(yaw_marker))
cv2.putText(frame, str_attitude, (0, 150), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
str_position = "CAMERA Position x=%4.0f y=%4.0f z=%4.0f"%(pos_camera[0], pos_camera[1], pos_camera[2])
cv2.putText(frame, str_position, (0, 200), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
#-- Get the attitude of the camera respect to the frame
roll_camera, pitch_camera, yaw_camera = rotationMatrixToEulerAngles(R_flip*R_tc)
str_attitude = "CAMERA Attitude r=%4.0f p=%4.0f y=%4.0f"%(math.degrees(roll_camera),math.degrees(pitch_camera),
math.degrees(yaw_camera))
cv2.putText(frame, str_attitude, (0, 250), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
else:
if verbose: print "Nothing detected - fps = %.0f"%self.fps_read
if show_video:
#--- Display the frame
cv2.imshow('frame', frame)
#--- use 'q' to quit
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
self._cap.release()
cv2.destroyAllWindows()
break
if not loop: return(marker_found, x, y, z)
if __name__ == "__main__":
#--- Define Tag
id_to_find = 72
marker_size = 4 #- [cm]
#--- Get the camera calibration path
calib_path = ""
camera_matrix = np.loadtxt(calib_path+'cameraMatrix_raspi.txt', delimiter=',')
camera_distortion = np.loadtxt(calib_path+'cameraDistortion_raspi.txt', delimiter=',')
aruco_tracker = ArucoSingleTracker(id_to_find=72, marker_size=10, show_video=False, camera_matrix=camera_matrix, camera_distortion=camera_distortion)
aruco_tracker.track(verbose=True)