From 2c54e56f95cdc23d0a3748599ea47aa4d54d49af Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Tue, 7 Sep 2021 22:29:27 +0200 Subject: [PATCH] also enable computation of position outside of marker rectangle and for arbitrary grid sizes --- remote_control/aruco_estimator.py | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/remote_control/aruco_estimator.py b/remote_control/aruco_estimator.py index edd7471..26c2478 100644 --- a/remote_control/aruco_estimator.py +++ b/remote_control/aruco_estimator.py @@ -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 < 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']]) - # 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