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

improved grid drawing code to also work when markers are occluded and added documentation

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-10-24 20:54:47 +02:00
parent a22a3fdea3
commit dd162018d8
1 changed files with 39 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
remote_control/aruco_estimator.py
View File

@ -36,7 +36,6 @@ class ArucoEstimator:
self.robot_marker_estimates = dict([(id, None) for id in self.robot_marker_ids])
self.event_queue = Queue()
self.draw_marker_coordinate_system = draw_marker_coordinate_system
if use_realsense: # check if realsense camera is connected
@ -85,7 +84,15 @@ class ArucoEstimator:
self.camparam.readFromXMLFile(os.path.join(os.path.dirname(__file__), "dfk72_6mm_param2.yml"))
def mouse_callback(self, event, px, py, flags, param):
if event == 1:
"""
This function is called for each mouse event inside the opencv window.
If there are reference markers available we compute the real world position corresponding to the clicked
position and put it in an event queue.
:param event: type of event (mouse movement, left click, right click, etc.)
:param px: x-position of event
:param py: y-position of event
"""
if event == cv2.EVENT_LBUTTONDOWN:
if self.all_corners_detected():
# inter/extrapolate from clicked point to marker position
px1 = self.corner_estimates['a']['pixel_coordinate'][0]
@ -113,6 +120,9 @@ class ArucoEstimator:
print("not all markers have been detected!")
def run_tracking(self):
"""
Run the marker tracking in a loop
"""
cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE)
cv2.setMouseCallback('RoboRally', self.mouse_callback)
@ -139,7 +149,7 @@ class ArucoEstimator:
# extract data for detected markers
detected_marker_data = {}
for marker in detected_markers:
detected_marker_data[marker.id] = {'center': marker.getCenter()}
detected_marker_data[marker.id] = {'marker_center': marker.getCenter()}
if marker.id > 0:
if marker.id in self.corner_marker_ids.values():
marker.calculateExtrinsics(0.1, self.camparam)
@ -156,7 +166,7 @@ class ArucoEstimator:
# store data
for id, data in detected_marker_data.items():
self.update_estimate(id, data['center'], data['Rvec'], data['Tvec'], t_image)
self.update_estimate(id, data['marker_center'], data['Rvec'], data['Tvec'], t_image)
# draw grid
color_image = self.draw_grid_lines(color_image, detected_marker_data)
@ -176,12 +186,14 @@ class ArucoEstimator:
# Stop streaming
self.pipeline.stop()
def update_estimate(self, id, pixel_coord, rvec, tvec, t_image):
def update_estimate(self, id, pixel_coord_center, rvec, tvec, t_image):
# update the marker estimate with new data
if id in self.corner_marker_ids.values():
# for corner markers we compute the mean of all measurements s.t. the position stabilizes over time
# (we assume the markers do not move)
corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys())) # get corresponding corner to the detected marker
# get corresponding corner to the detected marker
corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys()))
old_estimate_x = self.corner_estimates[corner]['x']
old_estimate_y = self.corner_estimates[corner]['y']
n_estimates = self.corner_estimates[corner]['n_estimates']
@ -198,7 +210,7 @@ class ArucoEstimator:
self.corner_estimates[corner]['x'] = new_estimate_x
self.corner_estimates[corner]['y'] = new_estimate_y
self.corner_estimates[corner]['n_estimates'] = n_estimates + 1
self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord
self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord_center
elif id in self.robot_marker_ids:
# for robot markers we extract x and y position as well as the angle
@ -292,32 +304,29 @@ class ArucoEstimator:
if value['n_estimates'] != 0:
print(f"corner marker {key} at pos ({value['x']},{value['y']})")
def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict):
def draw_corner_line(self, frame, corner_1, corner_2):
# draws a line between the given markers onto the given frame
if corner_1 in corner_coords_dict and corner_2 in corner_coords_dict:
frame = cv2.line(frame, corner_coords_dict[corner_1], corner_coords_dict[corner_2], color=(0, 0, 255),
thickness=2)
corner_1_center = self.corner_estimates[corner_1]['pixel_coordinate']
corner_2_center = self.corner_estimates[corner_2]['pixel_coordinate']
if corner_1_center is not None and corner_2_center is not None:
frame = cv2.line(frame, corner_1_center, corner_2_center, color=(0, 0, 255), thickness=2)
return frame
def draw_grid_lines(self, frame, detected_marker_data):
# draws a grid onto the given frame
board_corners_pixel_coords = {}
for corner, id in self.corner_marker_ids.items():
if id in detected_marker_data.keys():
board_corners_pixel_coords[corner] = tuple(detected_marker_data[id]['center'])
frame = self.draw_corner_line(frame, 'a', 'b')
frame = self.draw_corner_line(frame, 'b', 'c')
frame = self.draw_corner_line(frame, 'c', 'd')
frame = self.draw_corner_line(frame, 'd', 'a')
frame = self.draw_corner_line(frame, 'a', 'b', board_corners_pixel_coords)
frame = self.draw_corner_line(frame, 'b', 'c', board_corners_pixel_coords)
frame = self.draw_corner_line(frame, 'c', 'd', board_corners_pixel_coords)
frame = self.draw_corner_line(frame, 'd', 'a', board_corners_pixel_coords)
if set(board_corners_pixel_coords.keys()) == set(self.corner_marker_ids.keys()): # all markers have been detected
# compute column line
a = np.array(board_corners_pixel_coords['a'])
b = np.array(board_corners_pixel_coords['b'])
c = np.array(board_corners_pixel_coords['c'])
d = np.array(board_corners_pixel_coords['d'])
if not any([estimate['pixel_coordinate'] is None for estimate in self.corner_estimates.values()]):
# compute outlines of board
a = self.corner_estimates['a']['pixel_coordinate']
b = self.corner_estimates['b']['pixel_coordinate']
c = self.corner_estimates['c']['pixel_coordinate']
d = self.corner_estimates['d']['pixel_coordinate']
# draw vertical lines
vab = b - a
vdc = c - d
for x in range(1,self.grid_columns):
@ -326,6 +335,7 @@ class ArucoEstimator:
frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(0, 255, 0),
thickness=1)
# draw horizontal lines
vad = d - a
vbc = c - b
for y in range(1, self.grid_rows):
@ -359,10 +369,11 @@ class ArucoEstimator:
x = self.robot_marker_estimates[id]['x']
y = self.robot_marker_estimates[id]['y']
angle = self.robot_marker_estimates[id]['angle']
frame = cv2.putText(frame, "pos = ({:5.3f}, {:5.3f}), ang = {:5.3f}".format(x, y, angle), coord, cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0,255,0))
frame = cv2.putText(frame, "pos = ({:5.3f}, {:5.3f}), ang = {:5.3f}".format(x, y, angle), coord,
cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0,255,0))
return frame
if __name__ == "__main__":
estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[15], draw_marker_coordinate_system=True)
estimator = ArucoEstimator(use_realsense=False, draw_marker_coordinate_system=True)
estimator.run_tracking()