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

also enable computation of position outside of marker rectangle and for arbitrary grid sizes

Browse Source
This commit is contained in:
Simon Pirkelmann 2021-09-07 22:29:27 +02:00
parent 1db24bc573
commit 2c54e56f95
1 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
remote_control/aruco_estimator.py
View File

@ -413,7 +413,7 @@ class ArucoEstimator:
# checks if all robot markers have been detected at least once
return not any([estimate['t'] is None for estimate in self.robot_marker_estimates.values()])
def get_pos_from_grid_point(self, x, y, orientation=None):
def get_pos_from_grid_point(self, x, y, dimx, dimy, 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)
@ -422,17 +422,21 @@ class ArucoEstimator:
: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 0 <= x < self.grid_columns
assert 0 <= y < self.grid_rows
assert self.all_corners_detected()
if not self.all_corners_detected():
#raise RuntimeError("not all corner markers have been detected yet")
a = np.array([0,1])
b = np.array([1,1])
c = np.array([1,0])
d = np.array([0,0])
else:
# compute column line
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
x_frac = (x + 0.5) / dimx
y_frac = (y + 0.5) / dimy
vab = b - a
vdc = c - d