From c1d934c948c776cdac348b6cf9fad509f761f37a Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 26 Nov 2025 15:10:00 +0100 Subject: [PATCH 1/3] pbio/color: Introduce cost function type. Sets the stage for the next commits. This does not change any color detection code yet. --- lib/pbio/include/pbio/color.h | 5 ++++- lib/pbio/src/color/util.c | 2 +- lib/pbio/test/src/test_color.c | 36 ++++++++++++++++----------------- pybricks/util_pb/pb_color_map.c | 4 +++- 4 files changed, 26 insertions(+), 21 deletions(-) diff --git a/lib/pbio/include/pbio/color.h b/lib/pbio/include/pbio/color.h index 8dc464b2b..97b9f07af 100644 --- a/lib/pbio/include/pbio/color.h +++ b/lib/pbio/include/pbio/color.h @@ -118,7 +118,10 @@ void pbio_color_to_hsv(pbio_color_t color, pbio_color_hsv_t *hsv); void pbio_color_to_rgb(pbio_color_t color, pbio_color_rgb_t *rgb); void pbio_color_hsv_compress(const pbio_color_hsv_t *hsv, pbio_color_compressed_hsv_t *compressed); void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_color_hsv_t *hsv); -int32_t pbio_color_get_bicone_squared_distance(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b); + +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); #endif // _PBIO_COLOR_H_ diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c index b2a2eb00a..78b7fb7c9 100644 --- a/lib/pbio/src/color/util.c +++ b/lib/pbio/src/color/util.c @@ -13,7 +13,7 @@ * @param [in] hsv_b The second HSV color. * @returns Squared distance (0 to 400000000). */ -int32_t pbio_color_get_bicone_squared_distance(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) { // Chroma (= radial coordinate in bicone) of a and b (0-10000). int32_t radius_a = pbio_color_hsv_get_v(hsv_a) * hsv_a->s; diff --git a/lib/pbio/test/src/test_color.c b/lib/pbio/test/src/test_color.c index 3ee5df701..a27fc1d2c 100644 --- a/lib/pbio/test/src/test_color.c +++ b/lib/pbio/test/src/test_color.c @@ -366,7 +366,7 @@ static void test_color_hsv_cost(void *env) { color_a.h = 0; color_a.s = 100; color_a.v = 100; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_a), ==, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_a), ==, 0); // blacks with different saturations/hues should be the same color_a.h = 230; @@ -376,7 +376,7 @@ static void test_color_hsv_cost(void *env) { color_b.h = 23; color_b.s = 99; color_b.v = 0; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), ==, 0); // colors with different hues should be different when value>0 and saturation>0 color_a.h = 230; @@ -386,7 +386,7 @@ static void test_color_hsv_cost(void *env) { color_b.h = 23; color_b.s = 99; color_b.v = 100; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), >, 0); // grays with different hues should be the same color_a.h = 230; @@ -396,7 +396,7 @@ static void test_color_hsv_cost(void *env) { color_b.h = 23; color_b.s = 0; color_b.v = 50; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), ==, 0); // distance should be greater when saturation is greater color_a.h = 30; @@ -407,7 +407,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 20; color_b.v = 70; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); color_a.h = 30; color_a.s = 40; @@ -417,7 +417,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 40; color_b.v = 70; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), >, dist); // resolve colors that are close color_a.h = 30; @@ -428,7 +428,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 20; color_b.v = 70; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), >, 0); color_a.h = 30; color_a.s = 20; @@ -438,7 +438,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 25; color_b.v = 70; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), >, 0); color_a.h = 30; color_a.s = 20; @@ -448,7 +448,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 20; color_b.v = 75; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), >, 0); // hues 360 and 0 should be the same color_a.h = 360; @@ -458,7 +458,7 @@ static void test_color_hsv_cost(void *env) { color_b.h = 0; color_b.s = 100; color_b.v = 100; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), ==, 0); // distance between hues 359 and 1 should be smaller than hues 1 and 5 color_a.h = 359; @@ -468,7 +468,7 @@ static void test_color_hsv_cost(void *env) { color_b.h = 1; color_b.s = 100; color_b.v = 100; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); color_a.h = 1; color_a.s = 100; @@ -478,7 +478,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 100; color_b.v = 100; - tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist); + tt_want_int_op(pbio_color_get_distance_bicone_squared(&color_a, &color_b), >, dist); // check distance is monotonous along several color paths. This should catch potential int overflows int prev_dist = 0; @@ -495,7 +495,7 @@ static void test_color_hsv_cost(void *env) { while (color_a.s < 100) { color_a.s += 5; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); if (dist <= prev_dist) { monotone = false; @@ -520,7 +520,7 @@ static void test_color_hsv_cost(void *env) { while (color_a.v < 100) { color_a.v += 5; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); if (dist <= prev_dist) { monotone = false; @@ -545,7 +545,7 @@ static void test_color_hsv_cost(void *env) { while (color_a.v < 100) { color_a.v += 5; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); if (dist <= prev_dist) { monotone = false; @@ -573,7 +573,7 @@ static void test_color_hsv_cost(void *env) { color_a.h = i < 0 ? 180 : 0; color_a.v = 10000 / (200 - color_a.s); // constant lightness - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); if (dist <= prev_dist) { monotone = false; @@ -592,7 +592,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 100; color_b.v = 100; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); tt_want_int_op(dist, >, 390000000); tt_want_int_op(dist, <, 410000000); @@ -604,7 +604,7 @@ static void test_color_hsv_cost(void *env) { color_b.s = 0; color_b.v = 100; - dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + dist = pbio_color_get_distance_bicone_squared(&color_a, &color_b); tt_want_int_op(dist, >, 390000000); tt_want_int_op(dist, <, 410000000); } diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index 9069caee2..d8551e986 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/pybricks/util_pb/pb_color_map.c @@ -75,11 +75,13 @@ 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; + // Compute cost for each candidate for (size_t i = 0; i < n; i++) { // Evaluate the cost function - cost_now = pbio_color_get_bicone_squared_distance(hsv, pb_type_Color_get_hsv(colors[i])); + cost_now = distance_func(hsv, pb_type_Color_get_hsv(colors[i])); // If cost is less than before, update the minimum and the match if (cost_now < cost_min) { From 209d4632d59d3ee0b03722f9bcb81576d7c2f0b5 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 26 Nov 2025 19:38:56 +0100 Subject: [PATCH 2/3] pybricks.pupdevices.ColorSensor: Split corrections to sensor. This didn't work great for both sensor types, so move them to their own variants. This still needs to be revisited properly, but this is less bad than before. Also make the h adjustment monotonic so it doesn't skip ahead. --- .../pb_type_pupdevices_colordistancesensor.c | 4 ++++ .../pupdevices/pb_type_pupdevices_colorsensor.c | 8 ++++++++ pybricks/util_pb/pb_color_map.c | 14 ++++++-------- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c b/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c index 95b84ea2b..3ff7a3ad2 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c @@ -90,6 +90,10 @@ static void get_hsv_data(pupdevices_ColorDistanceSensor_obj_t *self, pbio_color_ rgb.g = 1187 * raw[1] / 2048; rgb.b = 1187 * raw[2] / 2048; pb_color_map_rgb_to_hsv(&rgb, hsv); + + // Approximately double low values to get similar results + // as with other sensors. + hsv->v = hsv->v * (200 - hsv->v) / 100; } // pybricks.pupdevices.ColorDistanceSensor.color diff --git a/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c b/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c index ae6630bdd..9d4218684 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c @@ -71,6 +71,14 @@ static void get_hsv_reflected(mp_obj_t self_in, pbio_color_hsv_t *hsv) { .b = data[2] == 1024 ? 255 : data[2] >> 2, }; pb_color_map_rgb_to_hsv(&rgb, hsv); + + // Approximately double saturation for low values to get similar results + // as other sensors. + hsv->s = hsv->s * (200 - hsv->s) / 100; + + // Approximately +50% low values to get similar results + // as with other sensors. + hsv->v = hsv->v * (150 - hsv->v / 2) / 100; } // Helper for getting HSV with the light off, scale saturation and value to diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index d8551e986..2601af60b 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/pybricks/util_pb/pb_color_map.c @@ -33,15 +33,13 @@ void pb_color_map_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv) pbio_color_rgb_to_hsv(rgb, hsv); // Slight shift for lower hues to make yellow somewhat more accurate - if (hsv->h < 40) { - uint8_t offset = ((hsv->h - 20) << 8) / 20; - int32_t scale = 200 - ((100 * (offset * offset)) >> 16); - hsv->h = hsv->h * scale / 100; + if (hsv->h >= 350) { + hsv->h = (350 + 2 * (hsv->h - 350)) % 360; + } else if (hsv->h < 40) { + hsv->h += 10; + } else if (hsv->h < 60) { + hsv->h = 50 + (hsv->h - 40) / 2; } - - // Value and saturation correction - hsv->s = hsv->s * (200 - hsv->s) / 100; - hsv->v = hsv->v * (200 - hsv->v) / 100; } static const mp_rom_obj_tuple_t pb_color_map_default = { From e99263c5d8b30877bfc0a69127e91a574a874544 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 26 Nov 2025 20:06:51 +0100 Subject: [PATCH 3/3] 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. --- lib/pbio/include/pbio/color.h | 1 + lib/pbio/src/color/util.c | 45 +++++++++++++++++++++++++++++ pybricks/parameters/pb_type_color.c | 2 +- pybricks/util_pb/pb_color_map.c | 18 +++++++++++- 4 files changed, 64 insertions(+), 2 deletions(-) diff --git a/lib/pbio/include/pbio/color.h b/lib/pbio/include/pbio/color.h index 97b9f07af..05203fbb8 100644 --- a/lib/pbio/include/pbio/color.h +++ b/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_ diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c index 78b7fb7c9..7008be9dd 100644 --- a/lib/pbio/src/color/util.c +++ b/lib/pbio/src/color/util.c @@ -1,6 +1,7 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2018-2022 The Pybricks Authors +#include #include #include @@ -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; + } +} diff --git a/pybricks/parameters/pb_type_color.c b/pybricks/parameters/pb_type_color.c index 8e6ed2d3f..03339b323 100644 --- a/pybricks/parameters/pb_type_color.c +++ b/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 = { diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index 2601af60b..490325b41 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/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++) {