pbio/color/util: Add heuristic distance for saturated colors.

The bicone mapping is highly distant dependent, which makes it suitable in
limited cases, and it doesn't work great with the default colors.

If only saturated or grayscale colors are in the mapping, we can generally get
a much better result by looking at hue only for saturated colors and looking at
value only for unsaturated colors, and use the saturation to decide which to
pick. This also means we won't need the workaround of having negative V for None.

The logic here is that if you do specify fine-grained, calibrated colors, then
it will use the original bicone distance mapping.

Author: laurensvalk

lib/pbio/include/pbio/color.h

@@ -122,6 +122,7 @@ void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_c
 typedef int32_t (*pbio_color_distance_func_t)(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b);
 
 int32_t pbio_color_get_distance_bicone_squared(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b);
+int32_t pbio_color_get_distance_saturation_heuristic(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b);
 
 #endif // _PBIO_COLOR_H_
 

lib/pbio/src/color/util.c

@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: MIT
 // Copyright (c) 2018-2022 The Pybricks Authors
 
+#include <assert.h>
 #include <pbio/color.h>
 #include <pbio/int_math.h>
 
@@ -36,3 +37,47 @@ int32_t pbio_color_get_distance_bicone_squared(const pbio_color_hsv_t *hsv_a, co
     // Squared Euclidean distance (0, 400000000)
     return delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
 }
+
+/**
+ * Gets distance measure between a HSV color (a) and a fully or zero saturated
+ * candidate color.
+ *
+ * @param [in]  measurement    The measured HSV color.
+ * @param [in]  candidate      The candidate HSV color (an idealized color or grayscale).
+ * @returns                    Heuristic distance.
+ */
+int32_t pbio_color_get_distance_saturation_heuristic(const pbio_color_hsv_t *measurement, const pbio_color_hsv_t *candidate) {
+
+    bool idealized_grayscale = candidate->s == 0 && candidate->h == 0;
+    bool idealized_color = candidate->s == 100 && candidate->v == 100;
+
+    // Calling code needs to ensure this.
+    assert(idealized_grayscale || idealized_color);
+
+    uint32_t hue_dist = pbio_int_math_abs(candidate->h - measurement->h);
+    if (hue_dist > 180) {
+        hue_dist = 360 - hue_dist;
+    }
+
+    uint32_t value_dist = pbio_int_math_abs(candidate->v - measurement->v);
+
+    const uint32_t penalty = 1000;
+
+    if (measurement->s <= 40 || measurement->v <= 1) {
+        // Measurement is unsaturated, so match to nearest grayscale; penalize color.
+        if (idealized_grayscale) {
+            // Match to nearest value.
+            return value_dist;
+        }
+        // Looking for grayscale, so disqualify color candidate.
+        return penalty + hue_dist;
+    } else {
+        // Measurement is saturated, so match to nearest full color; penalize grayscale.
+        if (idealized_color) {
+            // Match to nearest hue.
+            return hue_dist;
+        }
+        // Looking for color, so disqualify grayscale candidate.
+        return penalty + value_dist;
+    }
+}

pybricks/parameters/pb_type_color.c

@@ -62,7 +62,7 @@ const pb_type_Color_obj_t pb_Color_MAGENTA_obj = {
 
 const pb_type_Color_obj_t pb_Color_NONE_obj = {
     {&pb_type_Color},
-    .hsv = {0, 0, -40}
+    .hsv = {0, 0, 0}
 };
 
 const pb_type_Color_obj_t pb_Color_BLACK_obj = {

pybricks/util_pb/pb_color_map.c

@@ -73,7 +73,23 @@ mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv) {
     int32_t cost_now = INT32_MAX;
     int32_t cost_min = INT32_MAX;
 
-    pbio_color_distance_func_t distance_func = pbio_color_get_distance_bicone_squared;
+    pbio_color_distance_func_t distance_func = pbio_color_get_distance_saturation_heuristic;
+
+    // If user only provides fully saturated colors (hue, 100, 100) and/or fully
+    // desaturated colors (0, 0, value), use a simplified heuristic matcher for
+    // better default results that are distance independent. Otherwise use a
+    // bicone color distance measure.
+    for (size_t i = 0; i < n; i++) {
+        const pbio_color_hsv_t *candidate = pb_type_Color_get_hsv(colors[i]);
+
+        // Use bicone mapping if custom (realistic) colors provided.
+        bool idealized_grayscale = candidate->s == 0 && candidate->h == 0;
+        bool idealized_color = candidate->s == 100 && candidate->v == 100;
+        if (!idealized_grayscale && !idealized_color) {
+            distance_func = pbio_color_get_distance_bicone_squared;
+            break;
+        }
+    }
 
     // Compute cost for each candidate
     for (size_t i = 0; i < n; i++) {