Merge branch 'my-beautyful-implementation-with-fucking-celeb-dataset'…

… into ben_gpsr

common/helpers/numpy2message/src/__init__.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+
+def numpy2message(np_array: np.ndarray) -> list[bytes, list[int], str]:
+    data = np_array.tobytes()
+    shape = list(np_array.shape)
+    dtype = str(np_array.dtype)
+    return data, shape, dtype
+
+
+def message2numpy(data:bytes, shape:list[int], dtype:str) -> np.ndarray:
+    array_shape = tuple(shape)
+    array_dtype = np.dtype(dtype)
+
+    deserialized_array = np.frombuffer(data, dtype=array_dtype)
+    deserialized_array = deserialized_array.reshape(array_shape)
+
+    return deserialized_array
+

common/vision/lasr_vision_cropped_detection/src/lasr_vision_cropped_detection/cropped_detection.py

@@ -171,10 +171,10 @@ def _3d_bbox_crop(
         )
         for det in detections
     ]
+
     if crop_method == "closest":
         detections = [det for _, det in sorted(zip(distances, detections))]
         distances.sort()
-
     elif crop_method == "furthest":
         detections = [
             det for _, det in sorted(zip(distances, detections), reverse=True)

common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/__init__.py

@@ -1,6 +1,5 @@
 import json
 from os import path
-
 import cv2
 import numpy as np
 import rospkg
@@ -20,6 +19,25 @@
 )
 
 
+def gaussian_blur(image, kernel_size, rep=3):
+    """
+    Apply Gaussian blur to an RGB image.
+
+    Parameters:
+    image (numpy.ndarray): The input RGB image.
+    kernel_size (int): The size of the Gaussian kernel. If an even number is provided, it will be incremented to the next odd number.
+
+    Returns:
+    numpy.ndarray: The blurred RGB image.
+    """
+    if kernel_size % 2 == 0:
+        kernel_size += 1  # Increment kernel size to make it odd if it's even
+
+    for _ in range(rep):
+        image = cv2.GaussianBlur(image, (kernel_size, kernel_size), 0)
+    return image
+
+
 def X2conv(in_channels, out_channels, inner_channels=None):
     inner_channels = out_channels // 2 if inner_channels is None else inner_channels
     down_conv = nn.Sequential(

common/vision/lasr_vision_feature_extraction/src/lasr_vision_feature_extraction/image_with_masks_and_attributes.py

@@ -3,6 +3,120 @@
     CategoriesAndAttributes,
 )
 
+reference_colours = {
+    "blue_very_light": np.array([240, 248, 255]),  # Alice blue
+    "blue_light": np.array([173, 216, 230]),  # Light blue
+    "blue_sky": np.array([135, 206, 235]),  # Sky blue
+    "blue_powder": np.array([176, 224, 230]),  # Powder blue
+    "blue_celeste": np.array([178, 255, 255]),  # Celeste, very pale blue shade
+    "blue_periwinkle": np.array(
+        [204, 204, 255]
+    ),  # Periwinkle, a mix of light blue and lavender
+    "blue_cadet": np.array([95, 158, 160]),  # Cadet blue, a muted blue-green
+    "blue": np.array([0, 0, 255]),  # Standard blue
+    "blue_royal": np.array([65, 105, 225]),  # Royal blue
+    "blue_deep": np.array([0, 0, 139]),  # Deep blue
+    "blue_dark": np.array([0, 0, 128]),  # Dark blue
+    # "blue_navy": np.array([0, 0, 80]),             # Navy blue
+    "yellow_very_light": np.array([255, 255, 204]),  # Very light yellow
+    "yellow_light": np.array([255, 255, 224]),  # Light yellow
+    "yellow": np.array([255, 255, 0]),  # Standard yellow
+    "yellow_gold": np.array([255, 215, 0]),  # Gold yellow
+    "yellow_dark": np.array([204, 204, 0]),  # Dark yellow
+    "yellow_mustard": np.array([255, 219, 88]),  # Mustard yellow
+    "red_very_light": np.array([255, 204, 204]),  # Very light red
+    "red_light": np.array([255, 102, 102]),  # Light red
+    "red": np.array([255, 0, 0]),  # Standard red
+    "red_dark": np.array([139, 0, 0]),  # Dark red
+    "red_maroon": np.array([128, 0, 0]),  # Maroon
+    "orange_very_light": np.array([255, 229, 180]),  # Very light orange
+    "orange_light": np.array([255, 179, 71]),  # Light orange
+    "orange": np.array([255, 165, 0]),  # Standard orange
+    "orange_dark": np.array([255, 140, 0]),  # Dark orange
+    "orange_burnt": np.array([204, 85, 0]),  # Burnt orange
+    "black": np.array([30, 30, 30]),  # Black
+    "white": np.array([255, 255, 255]),  # White
+    "gray": np.array([160, 160, 160]),  # Gray
+}
+
+colour_group_map = {
+    "blue_very_light": "blue",
+    "blue_light": "blue",
+    "blue_sky": "blue",
+    "blue_powder": "blue",
+    "blue_celeste": "blue",
+    "blue_periwinkle": "blue",
+    "blue_cadet": "blue",
+    "blue": "blue",
+    "blue_royal": "blue",
+    "blue_deep": "blue",
+    "blue_dark": "blue",
+    "yellow_very_light": "yellow",
+    "yellow_light": "yellow",
+    "yellow": "yellow",
+    "yellow_gold": "yellow",
+    "yellow_dark": "yellow",
+    "yellow_mustard": "yellow",
+    "red_very_light": "red",
+    "red_light": "red",
+    "red": "red",
+    "red_dark": "red",
+    "red_maroon": "red",
+    "orange_very_light": "orange",
+    "orange_light": "orange",
+    "orange": "orange",
+    "orange_dark": "orange",
+    "orange_burnt": "orange",
+    "black": "black",
+    "white": "white",
+    "gray": "gray",
+}
+
+possible_colours = ["blue", "yellow", "red", "orange", "black", "white", "gray"]
+
+
+def estimate_colour(rgb_array):
+    # Calculate distances to each reference colour
+    # distances = {colour: cie76_distance(rgb_array, ref_rgb) for colour, ref_rgb in reference_colours.items()}
+    distances = {
+        colour: np.linalg.norm(rgb_array - ref_rgb)
+        for colour, ref_rgb in reference_colours.items()
+    }
+
+    # Find the colour with the smallest distance
+    estimated_colour = min(distances, key=distances.get)
+
+    return estimated_colour
+
+
+def split_and_sample_colours(image, mask, square_size):
+    height, width, _ = image.shape
+    squares_colours = {}
+    valid_squares = set()
+
+    square_index = 0
+
+    for y in range(0, height, square_size):
+        for x in range(0, width, square_size):
+            square = image[y : y + square_size, x : x + square_size]
+            mask_square = mask[y : y + square_size, x : x + square_size]
+
+            # Calculate the average colour
+            average_colour = square.mean(axis=(0, 1))
+
+            # Save the average colour in the dictionary
+            squares_colours[square_index] = estimate_colour(average_colour)
+            # squares_colours[square_index] = average_colour  # estimate_colour(average_colour)
+
+            # Check the mask condition
+            a = np.sum(mask_square)
+            if np.sum(mask_square) > 0.5 * square_size * square_size:
+                valid_squares.add(square_index)
+
+            square_index += 1
+
+    return squares_colours, valid_squares
+
 
 def _softmax(x: list[float]) -> list[float]:
     """Compute softmax values for each set of scores in x without using NumPy."""
@@ -98,6 +212,11 @@ def describe(self) -> dict:
         glasses = self.attributes["Eyeglasses"] - 0.5
         hat = self.attributes["Wearing_Hat"] - 0.5
 
+        if hat >= 0.25:  # probably wearing hat
+            has_hair *= (
+                (0.5 - hat) / 0.5 * has_hair
+            )  # give a penalty to the hair confidence (hope this helps!)
+
         result = {
             "has_hair": has_hair,
             "hair_colour": hair_colour,
@@ -136,6 +255,8 @@ def from_parent_instance(
         )
 
     def describe(self) -> dict:
+        # Maybe not keep ‘sleeveless’ anymore but might do this in the actual environment.
+
         result = {
             "top": self.attributes["top"] / 2,
             "outwear": self.attributes["outwear"] / 2,
@@ -178,4 +299,38 @@ def describe(self) -> dict:
             max_attribute = "sleeveless dress"
         result["max_dress"] = max_attribute
 
+        # QUICK FIX that won't break the code but not returning dress anymore.
+        result["dress"] = 0.0
+
+        # ========= colour estimation below: =========
+        # blurred_imagge kept here so that we can quickly make it work if we need.
+        # blurred_image = gaussian_blur(self.image, kernel_size=13, rep=3)
+        blurred_image = self.image
+        for cloth in ["top", "outwear", "dress"]:  #  "down",
+            mask = self.masks[cloth]
+            squares_colours, valid_squares = split_and_sample_colours(
+                blurred_image, mask, 20
+            )
+            _squares_colours = {}
+            for k in squares_colours.keys():
+                if k in valid_squares:
+                    _squares_colours[k] = squares_colours[k]
+            squares_colours = {
+                k: colour_group_map[colour] for k, colour in _squares_colours.items()
+            }
+            squares_colours_count = {}
+            for k, colour in squares_colours.items():
+                if colour not in squares_colours_count.keys():
+                    squares_colours_count[colour] = 1
+                else:
+                    squares_colours_count[colour] += 1
+            print(squares_colours_count)
+            tag = cloth + " colour"
+            result[tag] = {}
+            for k in possible_colours:
+                if k in squares_colours_count.keys():
+                    result[tag][k] = squares_colours_count[k] / len(squares_colours)
+                else:
+                    result[tag][k] = 0.0
+
         return result

common/vision/lasr_vision_torch/nodes/service

@@ -0,0 +1,130 @@
+from lasr_vision_msgs.srv import TorchFaceFeatureDetection, TorchFaceFeatureDetectionRequest, TorchFaceFeatureDetectionResponse
+from lasr_vision_msgs.msg import FeatureWithColour, ColourPrediction
+from colour_estimation import closest_colours, RGB_COLOURS, RGB_HAIR_COLOURS
+from cv2_img import msg_to_cv2_img
+from torch_module.helpers import binary_erosion_dilation, median_color_float
+from numpy2message import message2numpy
+
+import numpy as np
+import torch
+import rospy
+import lasr_vision_torch
+
+
+model = lasr_vision_torch.load_face_classifier_model()
+
+
+def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetectionResponse:
+    # decode the image
+    rospy.loginfo('Decoding')
+    frame = msg_to_cv2_img(request.image_raw)
+    torso_mask_data, torso_mask_shape, torso_mask_dtype = request.torso_mask_data, request.torso_mask_shape, request.torso_mask_dtype
+    head_mask_data, head_mask_shape, head_mask_dtype = request.head_mask_data, request.head_mask_shape, request.head_mask_dtype 
+    torsal_mask = message2numpy(torso_mask_data, torso_mask_shape, torso_mask_dtype)
+    head_mask = message2numpy(head_mask_data, head_mask_shape, head_mask_dtype)
+    print(torso_mask_shape)
+    print(head_mask_shape)
+
+    # 'hair', 'hat', 'glasses', 'face'
+    input_image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float()
+    input_image /= 255.0
+    masks_batch_pred, pred_classes = model(input_image)
+
+    thresholds_mask = [
+        0.5, 0.75, 0.25, 0.5,  # 0.5, 0.5, 0.5, 0.5,
+    ]
+    thresholds_pred = [
+        0.6, 0.8, 0.1, 0.5,
+    ]
+    erosion_iterations = 1
+    dilation_iterations = 1
+    categories = ['hair', 'hat', 'glasses', 'face',]
+
+    masks_batch_pred = binary_erosion_dilation(
+        masks_batch_pred, thresholds=thresholds_mask,
+        erosion_iterations=erosion_iterations, dilation_iterations=dilation_iterations
+    )
+
+    median_colours = (median_color_float(
+        input_image, masks_batch_pred).detach().squeeze(0)*255).numpy().astype(np.uint8)
+
+    # discarded: masks = masks_batch_pred.detach().squeeze(0).numpy().astype(np.uint8)
+    # discarded: mask_list = [masks[i,:,:] for i in range(masks.shape[0])]
+
+    pred_classes = pred_classes.detach().squeeze(0).numpy()
+    # discarded: class_list = [categories[i] for i in range(
+    #     pred_classes.shape[0]) if pred_classes[i].item() > thresholds_pred[i]]
+    colour_list = [median_colours[i, :]
+                   for i in range(median_colours.shape[0])]
+
+    response = TorchFaceFeatureDetectionResponse()
+    response.detected_features = [
+        FeatureWithColour(categories[i], [
+            ColourPrediction(colour, distance)
+            for colour, distance
+            in closest_colours(colour_list[i], RGB_HAIR_COLOURS if categories[i] == 'hair' else RGB_COLOURS)
+        ])
+        for i
+        in range(pred_classes.shape[0])
+        if pred_classes[i].item() > thresholds_pred[i]
+    ]
+
+    return response
+
+
+# def detect(request: TorchFaceFeatureDetectionRequest) -> TorchFaceFeatureDetectionResponse:
+#     # decode the image
+#     rospy.loginfo('Decoding')
+#     frame = msg_to_cv2_img(request.image_raw)
+
+#     # 'hair', 'hat', 'glasses', 'face'
+#     input_image = torch.from_numpy(frame).permute(2, 0, 1).unsqueeze(0).float()
+#     input_image /= 255.0
+#     masks_batch_pred, pred_classes = model(input_image)
+
+#     thresholds_mask = [
+#         0.5, 0.75, 0.25, 0.5,  # 0.5, 0.5, 0.5, 0.5,
+#     ]
+#     thresholds_pred = [
+#         0.6, 0.8, 0.1, 0.5,
+#     ]
+#     erosion_iterations = 1
+#     dilation_iterations = 1
+#     categories = ['hair', 'hat', 'glasses', 'face',]
+
+#     masks_batch_pred = binary_erosion_dilation(
+#         masks_batch_pred, thresholds=thresholds_mask,
+#         erosion_iterations=erosion_iterations, dilation_iterations=dilation_iterations
+#     )
+
+#     median_colours = (median_color_float(
+#         input_image, masks_batch_pred).detach().squeeze(0)*255).numpy().astype(np.uint8)
+
+#     # discarded: masks = masks_batch_pred.detach().squeeze(0).numpy().astype(np.uint8)
+#     # discarded: mask_list = [masks[i,:,:] for i in range(masks.shape[0])]
+
+#     pred_classes = pred_classes.detach().squeeze(0).numpy()
+#     # discarded: class_list = [categories[i] for i in range(
+#     #     pred_classes.shape[0]) if pred_classes[i].item() > thresholds_pred[i]]
+#     colour_list = [median_colours[i, :]
+#                    for i in range(median_colours.shape[0])]
+
+#     response = TorchFaceFeatureDetectionResponse()
+#     response.detected_features = [
+#         FeatureWithColour(categories[i], [
+#             ColourPrediction(colour, distance)
+#             for colour, distance
+#             in closest_colours(colour_list[i], RGB_HAIR_COLOURS if categories[i] == 'hair' else RGB_COLOURS)
+#         ])
+#         for i
+#         in range(pred_classes.shape[0])
+#         if pred_classes[i].item() > thresholds_pred[i]
+#     ]
+
+#     return response
+# test test
+
+rospy.init_node('torch_service')
+rospy.Service('/torch/detect/face_features', TorchFaceFeatureDetection, detect)
+rospy.loginfo('Torch service started')
+rospy.spin()

skills/config/motions.yaml

@@ -84,17 +84,17 @@ play_motion:
     u3l:
       joints: [torso_lift_joint, head_1_joint, head_2_joint]
       points:
-      - positions: [0.4, 0.35, 0.05]
+      - positions: [0.4, 0.35, 0.1]
         time_from_start: 0.0
     u3m:
       joints: [torso_lift_joint, head_1_joint, head_2_joint]
       points:
-      - positions: [0.4, 0.0, 0.05]
+      - positions: [0.4, 0.0, 0.1]
         time_from_start: 0.0
     u3r:
       joints: [torso_lift_joint, head_1_joint, head_2_joint]
       points:
-      - positions: [0.4, -0.35, 0.05]
+      - positions: [0.4, -0.35, 0.1]
         time_from_start: 0.0
 
     u2l: