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