def detector_im(self):
frametmp = self.frames[self.imageImdex]
detections = self.apriltag.detect(frametmp[3])
detections1 = self.apriltag.detect(frametmp[2])
detections2 = self.apriltag.detect(frametmp[1])
detections3 = self.apriltag.detect(frametmp[0])
if (len(detections) < 1 or len(detections1) < 1 or len(detections2) < 1
or len(detections3) < 1):
return np.array([0, 0, 0, 0])
tmp = 121938.0923
add = 0
dis = tud.get_min_distance(detections, tmp) + add
dis1 = tud.get_min_distance(detections1, tmp) + add
dis2 = tud.get_min_distance(detections2, tmp) + add
dis3 = tud.get_min_distance(detections3, tmp) + add
dege = 1050
x, y, z = tud.sovle_coord(dis, dis1, dis3, dege)
x2, y2, z2 = tud.sovle_coord(dis2, dis3, dis1, dege)
nz = tud.verify_z(x, y, dis2, dege)
nz2 = tud.verify_z(x2, y2, dis, dege)
x2, y2, nz2 = [dege - x2, dege - y2, nz2]
point = np.array([x, y, nz, z])
point2 = np.array([x2, y2, nz2, z2])
print(point)
print(point2)
print((point + point2) / 2)
print()
return (point + point2) / 2
def detectormult_four_pic(self,num):
self.__create_fileRead()
ax = plt.subplot(111, projection='3d')
ax.set_zlabel('z')
ax.set_ylabel('y')
ax.set_xlabel('x')
ax.set_ylim(-20, 120)
ax.set_zlim(-20, 120)
ax.set_xlim(-20, 120)
for index in range(num):
self.__get_picture(index)
result = self.__detector_id_dis()
if len(result) == 4:
x, y, z = tud.sovle_coord(result[0, 2], result[1, 2], result[3, 2], edge=1100)
R4 = result[2, 2]
z = tud.verify_z(x, y, R4)
ax.scatter(x, y, z)
print(x, y, z)
def detectormult_four(self):
self.__create_videocapture()
while (self.__get_grad()):
self.__get_frame()
result = self.__detector_id_dis()
ax = plt.subplot(111, projection='3d')
#fig.canvas.mpl_connect("key_press_event",self.on_key_press)
ax.set_zlabel('z')
ax.set_ylabel('y')
ax.set_xlabel('x')
ax.set_ylim(-200,1200)
ax.set_zlim(-200, 1200)
ax.set_xlim(-200, 1200)
if len(result)==4:
x,y,z = tud.sovle_coord(result[0, 2], result[1, 2], result[3, 2],edge=1100)
R4 = result[2,2]
z = tud.verify_z(x,y,R4)
ax.scatter(x, y, z)
print(x,y,z)
plt.pause(0.001)
frame3 = cv2.imread(filename3)
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
gray1 = cv2.cvtColor(frame1, cv2.COLOR_RGB2GRAY)
gray2 = cv2.cvtColor(frame2, cv2.COLOR_RGB2GRAY)
gray3 = cv2.cvtColor(frame3, cv2.COLOR_RGB2GRAY)
window = 'Camera'
cv2.namedWindow(window)
detector = apriltag.Detector()
detections = detector.detect(gray, return_image=False)
detections1 = detector.detect(gray1, return_image=False)
detections2 = detector.detect(gray2, return_image=False)
detections3 = detector.detect(gray3, return_image=False)
show = None
if (len(detections) + len(detections1) + len(detections2) +
len(detections3) < 4):
continue
else:
dis = tud.get_distance(detections[0].homography, 122274.9)
dis1 = tud.get_distance(detections1[0].homography, 122274.9)
dis2 = tud.get_distance(detections2[0].homography, 122274.9)
dis3 = tud.get_distance(detections3[0].homography, 122274.9)
x, y, z = tud.sovle_coord(dis, dis1, dis3)
z = tud.verify_z(x, y, dis2)
print(x, y, z)
if (len(detections) < 1 or len(detections1) < 1 or len(detections2) < 1
or len(detections3) < 1):
continue
else:
tmp = 121938.0923
add = 0
dis = tud.get_min_distance(detections, tmp) + add
dis1 = tud.get_min_distance(detections1, tmp) + add
dis2 = tud.get_min_distance(detections2, tmp)
dis3 = tud.get_min_distance(detections3, tmp) + add
dege = 1000
x, y, z = tud.sovle_coord(dis, dis1, dis3, dege)
x1, y1, z1 = tud.sovle_coord(dis3, dis, dis2, dege)
x2, y2, z2 = tud.sovle_coord(dis2, dis3, dis1, dege)
nz = tud.verify_z(x, y, dis2, dege)
nz1 = tud.verify_z(x1, y1, dis1, dege)
nz2 = tud.verify_z(x2, y2, dis, dege)
x1, y1, nz1 = [y1, dege - x1, nz1]
x2, y2, nz2 = [dege - x2, dege - y2, nz2]
point = np.array([x, y, nz])
point1 = np.array([x1, y1, nz1])
point2 = np.array([x2, y2, nz2])
print(point)
print(point1)
print(point2)
print((point + point2 + point1) / 3)
print()