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

added option to draw marker coordinate system and modified dict for corner marker estimates

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-10-24 19:55:04 +02:00
parent ec6b2bbf4a
commit 02a83f405f
1 changed files with 42 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
remote_control/aruco_estimator.py
View File

@ -2,6 +2,7 @@ import pyrealsense2 as rs
import numpy as np import numpy as np
import cv2 import cv2
import os import os
import time
from shapely.geometry import LineString from shapely.geometry import LineString
from queue import Queue from queue import Queue
@ -10,7 +11,7 @@ import aruco
class ArucoEstimator: class ArucoEstimator:
corner_marker_ids = { corner_marker_ids = {
'a': 15, 'a': 0,
'b': 1, 'b': 1,
'c': 2, 'c': 2,
'd': 3 'd': 3
@ -19,13 +20,13 @@ class ArucoEstimator:
angles = [] angles = []
corner_estimates = { corner_estimates = {
'a': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, 'a': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 },
'b': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, 'b': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 },
'c': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, 'c': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 },
'd': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, 'd': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 },
} }
def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8): def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8, draw_marker_coordinate_system=False):
self.grid_columns = grid_columns self.grid_columns = grid_columns
self.grid_rows = grid_rows self.grid_rows = grid_rows
@ -36,6 +37,8 @@ class ArucoEstimator:
self.event_queue = Queue() self.event_queue = Queue()
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
# Configure depth and color streams # Configure depth and color streams
self.pipeline = rs.pipeline() self.pipeline = rs.pipeline()
@ -90,10 +93,10 @@ class ArucoEstimator:
py1 = self.corner_estimates['a']['pixel_coordinate'][1] py1 = self.corner_estimates['a']['pixel_coordinate'][1]
py3 = self.corner_estimates['c']['pixel_coordinate'][1] py3 = self.corner_estimates['c']['pixel_coordinate'][1]
x1 = self.corner_estimates['a']['real_world_estimate'][0] x1 = self.corner_estimates['a']['x']
x3 = self.corner_estimates['c']['real_world_estimate'][0] x3 = self.corner_estimates['c']['x']
y1 = self.corner_estimates['a']['real_world_estimate'][1] y1 = self.corner_estimates['a']['y']
y3 = self.corner_estimates['c']['real_world_estimate'][1] y3 = self.corner_estimates['c']['y']
alpha = (px - px1) / (px3 - px1) alpha = (px - px1) / (px3 - px1)
beta = (py - py1) / (py3 - py1) beta = (py - py1) / (py3 - py1)
@ -126,6 +129,7 @@ class ArucoEstimator:
else: else:
# Capture frame-by-frame # Capture frame-by-frame
ret, color_image = self.cv_camera.read() ret, color_image = self.cv_camera.read()
t_image = time.time()
gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
@ -146,11 +150,13 @@ class ArucoEstimator:
if marker.id > 0: # draw markers onto the image if marker.id > 0: # draw markers onto the image
marker.draw(color_image, np.array([255, 255, 255]), 2, True) marker.draw(color_image, np.array([255, 255, 255]), 2, True)
if self.draw_marker_coordinate_system:
aruco.CvDrawingUtils.draw3dAxis(color_image, self.camparam, marker.Rvec, marker.Tvec, .1) aruco.CvDrawingUtils.draw3dAxis(color_image, self.camparam, marker.Rvec, marker.Tvec, .1)
# 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']) self.update_estimate(id, data['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)
@ -170,23 +176,27 @@ class ArucoEstimator:
# Stop streaming # Stop streaming
self.pipeline.stop() self.pipeline.stop()
def update_estimate(self, id, pixel_coord, rvec, tvec): def update_estimate(self, id, pixel_coord, 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 corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys())) # get corresponding corner to the detected marker
old_estimate = self.corner_estimates[corner]['real_world_estimate'] old_estimate_x = self.corner_estimates[corner]['x']
old_estimate_y = self.corner_estimates[corner]['y']
n_estimates = self.corner_estimates[corner]['n_estimates'] n_estimates = self.corner_estimates[corner]['n_estimates']
x = tvec[0][0] x = tvec[0][0]
y = -tvec[1][0] # flip y coordinate y = -tvec[1][0] # flip y coordinate
tvec_proj = np.array([x, y]) if not any([old_estimate_x is None, old_estimate_y is None]):
if old_estimate is not None: new_estimate_x = (n_estimates * old_estimate_x + x) / (n_estimates + 1) # weighted update
new_estimate = (n_estimates * old_estimate + tvec_proj) / (n_estimates + 1) # weighted update new_estimate_y = (n_estimates * old_estimate_y + y) / (n_estimates + 1)
else: else:
new_estimate = tvec_proj # first estimate new_estimate_x = x # first estimate
self.corner_estimates[corner]['real_world_estimate'] = new_estimate new_estimate_y = y
self.corner_estimates[corner]['t'] = t_image
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]['n_estimates'] = n_estimates + 1
self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord
@ -203,7 +213,7 @@ class ArucoEstimator:
angle = -euler_angles[2][0] * np.pi / 180.0 angle = -euler_angles[2][0] * np.pi / 180.0
self.angles.append(angle) self.angles.append(angle)
self.robot_marker_estimates[id] = (x, y, angle) self.robot_marker_estimates[id] = {'t': t_image, 'x': x, 'y': y, 'angle': angle}
def all_corners_detected(self): def all_corners_detected(self):
# checks if all corner markers have been detected at least once # checks if all corner markers have been detected at least once
@ -227,10 +237,10 @@ class ArucoEstimator:
assert self.all_corners_detected() assert self.all_corners_detected()
# compute column line # compute column line
a = self.corner_estimates['a']['real_world_estimate'] a = np.array([self.corner_estimates['a']['x'], self.corner_estimates['a']['y']])
b = self.corner_estimates['b']['real_world_estimate'] b = np.array([self.corner_estimates['b']['x'], self.corner_estimates['b']['y']])
c = self.corner_estimates['c']['real_world_estimate'] c = np.array([self.corner_estimates['c']['x'], self.corner_estimates['c']['y']])
d = self.corner_estimates['d']['real_world_estimate'] d = np.array([self.corner_estimates['d']['x'], self.corner_estimates['d']['y']])
x_frac = (x + 0.5) / self.grid_columns x_frac = (x + 0.5) / self.grid_columns
y_frac = (y + 0.5) / self.grid_rows y_frac = (y + 0.5) / self.grid_rows
@ -274,14 +284,13 @@ class ArucoEstimator:
return point_of_intersection return point_of_intersection
def get_grid_point_from_pos(self): def get_grid_point_from_pos(self):
# return the nearest grid point for the given position estimate # TODO return the nearest grid point for the given position estimate
pass pass
def print_corner_estimates(self): def print_corner_estimates(self):
for key, value in self.corner_estimates.items(): for key, value in self.corner_estimates.items():
if value['n_estimates'] != 0: if value['n_estimates'] != 0:
print("corner marker {} at pos {}".format(key, value['real_world_estimate'])) print(f"corner marker {key} at pos ({value['x']},{value['y']})")
print()
def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict): def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict):
# draws a line between the given markers onto the given frame # draws a line between the given markers onto the given frame
@ -330,7 +339,8 @@ class ArucoEstimator:
def get_robot_state_estimate(self, id): def get_robot_state_estimate(self, id):
if id in self.robot_marker_estimates: if id in self.robot_marker_estimates:
if self.robot_marker_estimates[id] is not None: if self.robot_marker_estimates[id] is not None:
return np.array(self.robot_marker_estimates[id]) return np.array([self.robot_marker_estimates[id]['x'], self.robot_marker_estimates[id]['y'],
self.robot_marker_estimates[id]['angle']])
else: else:
print(f"error: no estimate available for robot {id}") print(f"error: no estimate available for robot {id}")
return None return None
@ -346,13 +356,13 @@ class ArucoEstimator:
robot_corners_pixel_coords[id] = tuple(detected_marker_data[id]['center']) robot_corners_pixel_coords[id] = tuple(detected_marker_data[id]['center'])
for id, coord in robot_corners_pixel_coords.items(): for id, coord in robot_corners_pixel_coords.items():
x = self.robot_marker_estimates[id][0] x = self.robot_marker_estimates[id]['x']
y = self.robot_marker_estimates[id][1] y = self.robot_marker_estimates[id]['y']
angle = self.robot_marker_estimates[id][2] 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) estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[15], draw_marker_coordinate_system=True)
estimator.run_tracking() estimator.run_tracking()