Telos4
/
RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

playing around with different markers and sizes

simple_control
Simon Pirkelmann 2020-10-21 21:10:04 +02:00
parent e93ae65e0f
commit 0bc6b68a20
1 changed files with 16 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
remote_control/opencv_viewer_example.py
View File

@ -38,7 +38,7 @@ class ArucoEstimator:
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, 1920, 1080, rs.format.bgr8, 30)
#config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
@ -107,36 +107,30 @@ class ArucoEstimator:
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
# aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
#aruco_dict = aruco.Dictionary_get(aruco.DICT_APRILTAG_16H5)
#aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_50)
#aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000)
#aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) # fast
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids)
if ids is not None:
for id, corner in zip(ids, corners):
if id in self.corner_marker_ids.values():
marker_size = 0.1
else:
marker_size = 0.07
corner_pixel_coord = np.mean(corner[0], axis=0)
res = aruco.estimatePoseSingleMarkers(corner, 0.10, self.camera_matrix, self.dist_coeffs)
rvecs = res[0][0][0]
tvecs = res[1][0][0]
self.update_estimate(id[0], corner_pixel_coord, rvecs, tvecs)
# res = aruco.estimatePoseSingleMarkers(corner, marker_size, self.camera_matrix, self.dist_coeffs)
# rvecs = res[0][0][0]
# tvecs = res[1][0][0]
#
# self.update_estimate(id[0], corner_pixel_coord, rvecs, tvecs)
frame = self.draw_grid_lines(frame, corners, ids)
frame = self.draw_robot_pos(frame, corners, ids)
else:
# pretent we detected some markers
pass
self.update_estimate(0, rvec=np.array([0, 0, 0]), tvec=np.array([-1, 1, 0]))
self.update_estimate(1, rvec=np.array([0, 0, 0]), tvec=np.array([1, 1.5, 0]))
self.update_estimate(2, rvec=np.array([0, 0, 0]), tvec=np.array([1, -1.5, 0]))
self.update_estimate(3, rvec=np.array([0, 0, 0]), tvec=np.array([-1, -1, 0]))
#import random
#self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]), tvec=np.array([-1.0 + random.random() * 2, -1.0 + random.random() * 2, 0]))
self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]),
tvec=np.array([1.2, 0.42, 0]))
self.update_estimate(13, rvec=np.array([0.0, 0.0, 0.0]),
tvec=np.array([-1.2, -0.42, 0]))
# Show images
cv2.imshow('RoboRally', frame)
@ -344,6 +338,6 @@ class ArucoEstimator:
if __name__ == "__main__":
estimator = ArucoEstimator()
estimator = ArucoEstimator(use_realsense=False)
estimator.run_tracking()