Merge pull request #378 from JdeRobot/issue-377

Updated opencv brain displaying predicted values

Author: sergiopaniego

behavior_metrics/brains/f1/brain_f1_keras_opencv_dataset.py

@@ -9,17 +9,17 @@
 
 """
 
-import tensorflow as tf
-import numpy as np
 import cv2
-import time
+import math
+import numpy as np
 import os
-
-from utils.constants import PRETRAINED_MODELS_DIR, ROOT_PATH
-from os import path
+import tensorflow as tf
+import time
 from albumentations import (
     Compose, Normalize
 )
+from os import path
+from utils.constants import PRETRAINED_MODELS_DIR, ROOT_PATH
 from utils.gradcam.gradcam import GradCAM
 
 PRETRAINED_MODELS = ROOT_PATH + '/' + PRETRAINED_MODELS_DIR + 'tf_models/'
@@ -49,6 +49,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
         self.suddenness_distance = []
         self.previous_v = None
         self.previous_w = None
+        self.previous_w_normalized = None
 
         if self.config['GPU'] is False:
             os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
@@ -67,23 +68,35 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
             print("- Models path: " + PRETRAINED_MODELS)
             print("- Model: " + str(model))
 
-    def update_frame(self, frame_id, data, angular_speed=None):
+    def update_frame(self, frame_id, data, current_angular_speed=None, previous_angular_speed=None, distance=None):
         """Update the information to be shown in one of the GUI's frames.
 
         Arguments:
             frame_id {str} -- Id of the frame that will represent the data
             data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
         """
-        if angular_speed:
-            import math
+        if current_angular_speed:
+            data = np.array(data, copy=True)
+
             x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0]  # ancho, alto
             length = 200
-            angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
+            angle = (90 + int(math.degrees(-current_angular_speed))) * 3.14 / 180.0
             x2 = int(x1 - length * math.cos(angle))
             y2 = int(y1 - length * math.sin(angle))
 
-            line_thickness = 2
+            line_thickness = 10
             cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
+            length = 150
+            angle = (90 + int(math.degrees(-previous_angular_speed))) * 3.14 / 180.0
+            x2 = int(x1 - length * math.cos(angle))
+            y2 = int(y1 - length * math.sin(angle))
+
+            cv2.line(data, (x1, y1), (x2, y2), (255, 0, 0), thickness=line_thickness)
+            if float(distance) > 0.01:
+                cv2.putText(data, distance, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA)
+            else:
+                cv2.putText(data, distance, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA)
+
         self.handler.update_frame(frame_id, data)
 
     def execute(self):
@@ -99,7 +112,7 @@ def execute(self):
             self.first_image = image
 
         image = self.handler.transform_image(image, self.config['ImageTranform'])
-        #self.update_frame('frame_0', image)
+        self.update_frame('frame_0', image)
 
         try:
             if self.config['ImageCropped']:
@@ -132,6 +145,7 @@ def execute(self):
                 self.motors.sendV(prediction_v)
                 self.motors.sendW(prediction_w)
 
+            current_w_normalized = prediction_w
             if self.previous_v != None:
                 a = np.array((prediction[0][0], prediction[0][1]))
                 b = np.array((self.previous_v, self.previous_w))
@@ -140,7 +154,9 @@ def execute(self):
             self.previous_v = prediction[0][0]
             self.previous_w = prediction[0][1]
 
-            self.update_frame('frame_0', base_image, prediction_w)
+            if self.previous_w_normalized != None:
+                self.update_frame('frame_2', base_image, current_w_normalized, self.previous_w_normalized, str(round(distance, 4)))
+            self.previous_w_normalized = current_w_normalized
 
             # GradCAM from image
             i = np.argmax(prediction[0])

behavior_metrics/brains/f1/brain_f1_keras_seq_3_opencv_dataset.py

@@ -9,18 +9,17 @@
 
 """
 
-import numpy as np
-
 import cv2
-import time
+import math
+import numpy as np
 import os
 import tensorflow as tf
-
-from utils.constants import PRETRAINED_MODELS_DIR, ROOT_PATH
-from os import path
+import time
 from albumentations import (
     Compose, Normalize
 )
+from os import path
+from utils.constants import PRETRAINED_MODELS_DIR, ROOT_PATH
 from utils.gradcam.gradcam import GradCAM
 
 PRETRAINED_MODELS = ROOT_PATH + '/' + PRETRAINED_MODELS_DIR + 'tf_models/'
@@ -53,6 +52,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
         self.suddenness_distance = []
         self.previous_v = None
         self.previous_w = None
+        self.previous_w_normalized = None
 
         self.third_image = []
 
@@ -72,23 +72,35 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
             print("- Models path: " + PRETRAINED_MODELS)
             print("- Model: " + str(model))
 
-    def update_frame(self, frame_id, data, angular_speed=None):
+    def update_frame(self, frame_id, data, current_angular_speed=None, previous_angular_speed=None, distance=None):
         """Update the information to be shown in one of the GUI's frames.
 
         Arguments:
             frame_id {str} -- Id of the frame that will represent the data
             data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
         """
-        if angular_speed:
-            import math
+        if current_angular_speed:
+            data = np.array(data, copy=True)
+
             x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0]  # ancho, alto
             length = 200
-            angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
+            angle = (90 + int(math.degrees(-current_angular_speed))) * 3.14 / 180.0
             x2 = int(x1 - length * math.cos(angle))
             y2 = int(y1 - length * math.sin(angle))
 
-            line_thickness = 2
+            line_thickness = 10
             cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
+            length = 150
+            angle = (90 + int(math.degrees(-previous_angular_speed))) * 3.14 / 180.0
+            x2 = int(x1 - length * math.cos(angle))
+            y2 = int(y1 - length * math.sin(angle))
+
+            cv2.line(data, (x1, y1), (x2, y2), (255, 0, 0), thickness=line_thickness)
+            if float(distance) > 0.01:
+                cv2.putText(data, distance, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA)
+            else:
+                cv2.putText(data, distance, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA)
+
         self.handler.update_frame(frame_id, data)
 
     def check_center(self, position_x):
@@ -136,7 +148,7 @@ def execute(self):
                 img = cv2.resize(image, (self.config['ImageSize'][0], self.config['ImageSize'][1]))
             else:
                 img = image
-            # sself.update_frame('frame_0', img)
+            self.update_frame('frame_0', img)
             if self.config['ImageNormalized']:
                 AUGMENTATIONS_TEST = Compose([
                     Normalize()
@@ -191,8 +203,7 @@ def execute(self):
                     self.motors.sendV(prediction_v)
                     self.motors.sendW(prediction_w)
 
-                self.update_frame('frame_0', base_image, prediction_w)
-
+                current_w_normalized = prediction_w
                 if self.previous_v != None:
                     a = np.array((prediction[0][0], prediction[0][1]))
                     b = np.array((self.previous_v, self.previous_w))
@@ -201,6 +212,10 @@ def execute(self):
                 self.previous_v = prediction[0][0]
                 self.previous_w = prediction[0][1]
 
+                if self.previous_w_normalized != None and distance:
+                    self.update_frame('frame_1', base_image, current_w_normalized, self.previous_w_normalized, str(round(distance, 4)))
+                self.previous_w_normalized = current_w_normalized
+
 
         except Exception as err:
             print(err)

behavior_metrics/brains/f1/brain_f1_opencv.py

@@ -1,18 +1,21 @@
 #!/usr/bin/python
 # -*- coding: utf-8 -*-
-import threading
-import time
-
+import csv
 import cv2
+import math
 import numpy as np
+import threading
+import time
+from albumentations import (
+    Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare
+)
+from utils.constants import DATASETS_DIR, ROOT_PATH
 
-time_cycle = 80
 error = 0
 integral = 0
 v = 0
 w = 0
 current = 'straight'
-time_cycle = 80
 RED = (255, 0, 0)
 GREEN = (0, 255, 0)
 BLUE = (0, 0, 255)
@@ -24,15 +27,8 @@
 V_MULT = 2
 v_mult = V_MULT
 
-import csv
-from utils.constants import DATASETS_DIR, ROOT_PATH
-
 GENERATED_DATASETS_DIR = ROOT_PATH + '/' + DATASETS_DIR
 
-from albumentations import (
-    Compose, Normalize, RandomRain, RandomBrightness, RandomShadow, RandomSnow, RandomFog, RandomSunFlare
-)
-
 
 class Brain:
 
@@ -55,6 +51,7 @@ def __init__(self, sensors, actuators, handler, config=None):
 
         self.previous_v = None
         self.previous_w = None
+        self.previous_w_normalized = None
         self.suddenness_distance = []
 
         # Save dataset
@@ -66,23 +63,35 @@ def __init__(self, sensors, actuators, handler, config=None):
         '''
         time.sleep(2)
 
-    def update_frame(self, frame_id, data, angular_speed=None):
+    def update_frame(self, frame_id, data, current_angular_speed=None, previous_angular_speed=None, distance=None):
         """Update the information to be shown in one of the GUI's frames.
 
         Arguments:
             frame_id {str} -- Id of the frame that will represent the data
             data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
         """
-        if angular_speed:
-            import math
+        if current_angular_speed:
+            data = np.array(data, copy=True)
+
             x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0]  # ancho, alto
             length = 200
-            angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
+            angle = (90 + int(math.degrees(-current_angular_speed))) * 3.14 / 180.0
             x2 = int(x1 - length * math.cos(angle))
             y2 = int(y1 - length * math.sin(angle))
 
-            line_thickness = 2
+            line_thickness = 10
             cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
+            length = 150
+            angle = (90 + int(math.degrees(-previous_angular_speed))) * 3.14 / 180.0
+            x2 = int(x1 - length * math.cos(angle))
+            y2 = int(y1 - length * math.sin(angle))
+
+            cv2.line(data, (x1, y1), (x2, y2), (255, 0, 0), thickness=line_thickness)
+            if float(distance) > 0.01:
+                cv2.putText(data, distance, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA)
+            else:
+                cv2.putText(data, distance, (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA)
+
         self.handler.update_frame(frame_id, data)
 
     def collinear3(self, x1, y1, x2, y2, x3, y3):
@@ -215,7 +224,8 @@ def execute(self):
         self.motors.sendW(w)
         self.motors.sendV(v)
 
-        self.update_frame('frame_0', image, w)
+        self.update_frame('frame_0', image)
+        current_w_normalized = w
 
         v = np.interp(np.array([v]), (6.5, 24), (0, 1))[0]
         w = np.interp(np.array([w]), (-7.1, 7.1), (0, 1))[0]
@@ -227,6 +237,10 @@ def execute(self):
         self.previous_v = v
         self.previous_w = w
 
+        if self.previous_w_normalized != None:
+            self.update_frame('frame_2', image, current_w_normalized, self.previous_w_normalized, str(round(distance, 4)))
+        self.previous_w_normalized = current_w_normalized
+
         '''
         if (save_dataset):
             # Save dataset
@@ -250,4 +264,3 @@ def execute(self):
                     (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, MAGENTA, 2, cv2.LINE_AA)
 
         self.update_frame('frame_1', image_mask)
-