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

added option to compute angle for marker based grid

simple_control
Simon Pirkelmann 2020-10-14 23:08:19 +02:00
parent 149e9b3536
commit 32892e5dcf
1 changed files with 38 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
remote_control/opencv_viewer_example.py
View File

@ -161,8 +161,15 @@ class ArucoEstimator:
# checks if all corner markers have been detected at least once
return not any([estimate[0] is None for estimate in self.corner_estimates.values()])
def get_pos_from_grid_point(self, x, y):
# returns the position for the given grid point based on the current corner estimates
def get_pos_from_grid_point(self, x, y, orientation=None):
"""
returns the position for the given grid point based on the current corner estimates
:param x: x position on the grid ( 0 &le x &lt number of grid columns)
:param y: y position on the grid ( 0 &le x &lt number of grid rows)
:param orientation: (optional) orientation in the given grid cell (one of ^, >, v, &lt )
:return: numpy array with corresponding real world x- and y-position
if orientation was specified the array also contains the matching angle for the orientation
"""
assert x >= 0 and x < self.grid_columns
assert y >= 0 and y < self.grid_rows
assert self.all_corners_detected()
@ -172,15 +179,18 @@ class ArucoEstimator:
b = self.corner_estimates['b'][0]
c = self.corner_estimates['c'][0]
d = self.corner_estimates['d'][0]
x_frac = (x + 0.5) / self.grid_columns
y_frac = (y + 0.5) / self.grid_rows
vab = b - a
vdc = c - d
column_line_top = a + (x + 0.5)/self.grid_columns * vab
column_line_bottom = d + (x + 0.5) / self.grid_columns * vdc
column_line_top = a + x_frac * vab
column_line_bottom = d + x_frac * vdc
vad = d - a
vbc = c - b
row_line_top = a + (y + 0.5)/self.grid_rows * vad
row_line_bottom = b + (y + 0.5) / self.grid_rows * vbc
row_line_top = a + y_frac * vad
row_line_bottom = b + y_frac * vbc
column_line = LineString([column_line_top, column_line_bottom])
row_line = LineString([row_line_top, row_line_bottom])
@ -188,7 +198,28 @@ class ArucoEstimator:
int_pt = column_line.intersection(row_line)
point_of_intersection = np.array([int_pt.x, int_pt.y])
return point_of_intersection
if orientation is not None:
# compute angle corresponding to the orientation w.r.t. the grid
# TODO: test this code
angle_ab = np.arctan2(vab[1], vab[0])
angle_dc = np.arctan2(vdc[1], vdc[0])
angle_ad = np.arctan2(vad[1], vad[0])
angle_bc = np.arctan2(vbc[1], vbc[0])
angle = 0.0
if orientation == '>':
angle = y_frac * angle_ab + (1 - y_frac) * angle_dc
elif orientation == '<':
angle = - (y_frac * angle_ab + (1 - y_frac) * angle_dc)
elif orientation == 'v':
angle = x_frac * angle_ad + (1 - x_frac) * angle_bc
elif orientation == '^':
angle = - (x_frac * angle_ad + (1 - x_frac) * angle_bc)
return np.array((point_of_intersection[0], point_of_intersection[1], angle))
else:
return point_of_intersection
def get_grid_point_from_pos(self):
# return the nearest grid point for the given position estimate