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.robot_marker_estimates = dict([(id, None) for id in self.robot_marker_ids])
self.event_queue = Queue() self.event_queue = Queue()
self.draw_marker_coordinate_system = draw_marker_coordinate_system self.draw_marker_coordinate_system = draw_marker_coordinate_system
if use_realsense: # check if realsense camera is connected 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")) self.camparam.readFromXMLFile(os.path.join(os.path.dirname(__file__), "dfk72_6mm_param2.yml"))
def mouse_callback(self, event, px, py, flags, param): 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(): if self.all_corners_detected():
# inter/extrapolate from clicked point to marker position # inter/extrapolate from clicked point to marker position
px1 = self.corner_estimates['a']['pixel_coordinate'][0] px1 = self.corner_estimates['a']['pixel_coordinate'][0]
@ -113,6 +120,9 @@ class ArucoEstimator:
print("not all markers have been detected!") print("not all markers have been detected!")
def run_tracking(self): def run_tracking(self):
"""
Run the marker tracking in a loop
"""
cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE) cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE)
cv2.setMouseCallback('RoboRally', self.mouse_callback) cv2.setMouseCallback('RoboRally', self.mouse_callback)
@ -139,7 +149,7 @@ class ArucoEstimator:
# extract data for detected markers # extract data for detected markers
detected_marker_data = {} detected_marker_data = {}
for marker in detected_markers: 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 > 0:
if marker.id in self.corner_marker_ids.values(): if marker.id in self.corner_marker_ids.values():
marker.calculateExtrinsics(0.1, self.camparam) marker.calculateExtrinsics(0.1, self.camparam)
@ -156,7 +166,7 @@ class ArucoEstimator:
# store data # store data
for id, data in detected_marker_data.items(): 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 # draw grid
color_image = self.draw_grid_lines(color_image, detected_marker_data) color_image = self.draw_grid_lines(color_image, detected_marker_data)
@ -176,12 +186,14 @@ class ArucoEstimator:
# Stop streaming # Stop streaming
self.pipeline.stop() 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 # update the marker estimate with new data
if id in self.corner_marker_ids.values(): 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 # for corner markers we compute the mean of all measurements s.t. the position stabilizes over time
# (we assume the markers do not move) # (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_x = self.corner_estimates[corner]['x']
old_estimate_y = self.corner_estimates[corner]['y'] old_estimate_y = self.corner_estimates[corner]['y']
n_estimates = self.corner_estimates[corner]['n_estimates'] 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]['x'] = new_estimate_x
self.corner_estimates[corner]['y'] = new_estimate_y self.corner_estimates[corner]['y'] = new_estimate_y
self.corner_estimates[corner]['n_estimates'] = n_estimates + 1 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: elif id in self.robot_marker_ids:
# for robot markers we extract x and y position as well as the angle # 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: if value['n_estimates'] != 0:
print(f"corner marker {key} at pos ({value['x']},{value['y']})") 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 # 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: corner_1_center = self.corner_estimates[corner_1]['pixel_coordinate']
frame = cv2.line(frame, corner_coords_dict[corner_1], corner_coords_dict[corner_2], color=(0, 0, 255), corner_2_center = self.corner_estimates[corner_2]['pixel_coordinate']
thickness=2) 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 return frame
def draw_grid_lines(self, frame, detected_marker_data): def draw_grid_lines(self, frame, detected_marker_data):
# draws a grid onto the given frame # draws a grid onto the given frame
board_corners_pixel_coords = {} frame = self.draw_corner_line(frame, 'a', 'b')
for corner, id in self.corner_marker_ids.items(): frame = self.draw_corner_line(frame, 'b', 'c')
if id in detected_marker_data.keys(): frame = self.draw_corner_line(frame, 'c', 'd')
board_corners_pixel_coords[corner] = tuple(detected_marker_data[id]['center']) frame = self.draw_corner_line(frame, 'd', 'a')
frame = self.draw_corner_line(frame, 'a', 'b', board_corners_pixel_coords) if not any([estimate['pixel_coordinate'] is None for estimate in self.corner_estimates.values()]):
frame = self.draw_corner_line(frame, 'b', 'c', board_corners_pixel_coords) # compute outlines of board
frame = self.draw_corner_line(frame, 'c', 'd', board_corners_pixel_coords) a = self.corner_estimates['a']['pixel_coordinate']
frame = self.draw_corner_line(frame, 'd', 'a', board_corners_pixel_coords) b = self.corner_estimates['b']['pixel_coordinate']
c = self.corner_estimates['c']['pixel_coordinate']
if set(board_corners_pixel_coords.keys()) == set(self.corner_marker_ids.keys()): # all markers have been detected d = self.corner_estimates['d']['pixel_coordinate']
# 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'])
# draw vertical lines
vab = b - a vab = b - a
vdc = c - d vdc = c - d
for x in range(1,self.grid_columns): 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), frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(0, 255, 0),
thickness=1) thickness=1)
# draw horizontal lines
vad = d - a vad = d - a
vbc = c - b vbc = c - b
for y in range(1, self.grid_rows): for y in range(1, self.grid_rows):
@ -359,10 +369,11 @@ class ArucoEstimator:
x = self.robot_marker_estimates[id]['x'] x = self.robot_marker_estimates[id]['x']
y = self.robot_marker_estimates[id]['y'] y = self.robot_marker_estimates[id]['y']
angle = self.robot_marker_estimates[id]['angle'] 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 return frame
if __name__ == "__main__": 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() estimator.run_tracking()