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

26
remote_control/aruco_estimator.py
View File

@ -413,7 +413,7 @@ class ArucoEstimator:
# checks if all robot markers have been detected at least once # 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()]) 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 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 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 :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 if orientation was specified the array also contains the matching angle for the orientation
""" """
assert 0 <= x < self.grid_columns if not self.all_corners_detected():
assert 0 <= y < self.grid_rows #raise RuntimeError("not all corner markers have been detected yet")
assert self.all_corners_detected() 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']])
# compute column line x_frac = (x + 0.5) / dimx
a = np.array([self.corner_estimates['a']['x'], self.corner_estimates['a']['y']]) y_frac = (y + 0.5) / dimy
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
vab = b - a vab = b - a
vdc = c - d vdc = c - d