From 32892e5dcf17ce830a021a0b4e15d84c47d039e1 Mon Sep 17 00:00:00 2001
From: Simon Pirkelmann <simon.pirkelmann@gmail.com>
Date: Wed, 14 Oct 2020 23:08:19 +0200
Subject: [PATCH] added option to compute angle for marker based grid

---
 remote_control/opencv_viewer_example.py | 45 +++++++++++++++++++++----
 1 file changed, 38 insertions(+), 7 deletions(-)

diff --git a/remote_control/opencv_viewer_example.py b/remote_control/opencv_viewer_example.py
index 09a07c7..cf310da 100644
--- a/remote_control/opencv_viewer_example.py
+++ b/remote_control/opencv_viewer_example.py
@@ -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