From 0445ee93bef21670bc40014ea4dca22da7b23792 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 4 Jul 2023 15:49:45 +0200 Subject: [PATCH 1/6] pbio/light: Avoid hardcoded sentinel values. --- lib/pbio/include/pbio/light.h | 3 ++- lib/pbio/src/light/color_light.c | 4 ++-- lib/pbio/test/src/test_color_light.c | 2 +- pybricks/common/pb_type_colorlight_internal.c | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/lib/pbio/include/pbio/light.h b/lib/pbio/include/pbio/light.h index b1eea8d7c..a088ae6c6 100644 --- a/lib/pbio/include/pbio/light.h +++ b/lib/pbio/include/pbio/light.h @@ -29,7 +29,8 @@ typedef struct _pbio_color_light_t pbio_color_light_t; { .h = (hue), .s = (saturation), .v = (value) } /** Sentinel value for a color light animation array. */ -#define PBIO_COLOR_LIGHT_ANIMATION_END { .v = UINT8_MAX } +#define PBIO_COLOR_LIGHT_ANIMATION_END_V (UINT8_MAX) +#define PBIO_COLOR_LIGHT_ANIMATION_END_HSV { .v = PBIO_COLOR_LIGHT_ANIMATION_END_V } #if PBIO_CONFIG_LIGHT diff --git a/lib/pbio/src/light/color_light.c b/lib/pbio/src/light/color_light.c index dca8de476..50cc29a16 100644 --- a/lib/pbio/src/light/color_light.c +++ b/lib/pbio/src/light/color_light.c @@ -147,7 +147,7 @@ static uint32_t pbio_color_light_animate_next(pbio_light_animation_t *animation) const pbio_color_compressed_hsv_t *cell = &cells[light->current_cell++]; // if we have reached the array terminator, start back at the beginning - if (cell->v == UINT8_MAX) { + if (cell->v == PBIO_COLOR_LIGHT_ANIMATION_END_V) { cell = &cells[0]; light->current_cell = 1; } @@ -169,7 +169,7 @@ static uint32_t pbio_color_light_animate_next(pbio_light_animation_t *animation) * * @param [in] light The light instance * @param [in] interval The the time intervale between animation cells in milliseconds - * @param [in] cells Array of up to 65536 animation cells ending with ::PBIO_COLOR_LIGHT_ANIMATION_END + * @param [in] cells Array of up to 65536 animation cells ending with ::PBIO_COLOR_LIGHT_ANIMATION_END_HSV */ void pbio_color_light_start_animation(pbio_color_light_t *light, uint16_t interval, const pbio_color_compressed_hsv_t *cells) { pbio_color_light_stop_animation(light); diff --git a/lib/pbio/test/src/test_color_light.c b/lib/pbio/test/src/test_color_light.c index d4f70a1e9..9b0c33ecb 100644 --- a/lib/pbio/test/src/test_color_light.c +++ b/lib/pbio/test/src/test_color_light.c @@ -31,7 +31,7 @@ static const uint16_t test_blink[] = { static const pbio_color_compressed_hsv_t test_animation[] = { PBIO_COLOR_LIGHT_ANIMATION_CELL(PBIO_COLOR_HUE_CYAN, 100, 100), PBIO_COLOR_LIGHT_ANIMATION_CELL(PBIO_COLOR_HUE_MAGENTA, 100, 100), - PBIO_COLOR_LIGHT_ANIMATION_END + PBIO_COLOR_LIGHT_ANIMATION_END_HSV }; static pbio_error_t test_light_set_hsv(pbio_color_light_t *light, const pbio_color_hsv_t *hsv) { diff --git a/pybricks/common/pb_type_colorlight_internal.c b/pybricks/common/pb_type_colorlight_internal.c index d6a0e573a..6d93a7664 100644 --- a/pybricks/common/pb_type_colorlight_internal.c +++ b/pybricks/common/pb_type_colorlight_internal.c @@ -116,7 +116,7 @@ STATIC mp_obj_t common_ColorLight_internal_animate(size_t n_args, const mp_obj_t } // sentinel value - cells[colors_len].v = UINT8_MAX; + cells[colors_len].v = PBIO_COLOR_LIGHT_ANIMATION_END_V; mp_int_t interval = pb_obj_get_int(interval_in); From b0ad0c5226bf344c49faf0134503dca52f9919f8 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 4 Jul 2023 16:09:29 +0200 Subject: [PATCH 2/6] pbio/color: Allow negative v. For color detection applications, negative h can be used to increase the contrast between "no color" and any other color. For most other application, the value has a lower bound of 0, which is what this commit enforces. --- lib/pbio/drv/led/led_pwm.c | 2 +- lib/pbio/drv/led/led_virtual.c | 2 +- lib/pbio/include/pbio/color.h | 14 ++++++++++---- lib/pbio/include/pbio/light.h | 2 +- lib/pbio/src/color/conversion.c | 2 +- lib/pbio/test/src/test_color.c | 9 +++++++++ lib/pbio/test/src/test_color_light.c | 2 +- pybricks/parameters/pb_type_color.c | 11 +++++------ .../pb_type_pupdevices_colorlightmatrix.c | 2 +- pybricks/util_mp/pb_obj_helper.c | 3 ++- 10 files changed, 32 insertions(+), 17 deletions(-) diff --git a/lib/pbio/drv/led/led_pwm.c b/lib/pbio/drv/led/led_pwm.c index 6f557db4e..e093f31a5 100644 --- a/lib/pbio/drv/led/led_pwm.c +++ b/lib/pbio/drv/led/led_pwm.c @@ -48,7 +48,7 @@ static pbio_error_t pbdrv_led_pwm_set_hsv(pbdrv_led_dev_t *dev, const pbio_color uint32_t Y = ((color->r_brightness * r + color->g_brightness * g + color->b_brightness * b) >> 12) + 1; // Reapply V from HSV for brightness. V is squared for gamma correction. - uint32_t scale_factor = hsv->v * hsv->v * pdata->scale_factor / 128; + uint32_t scale_factor = pbio_color_hsv_get_v(hsv) * pbio_color_hsv_get_v(hsv) * pdata->scale_factor / 128; r = r * scale_factor / Y; g = g * scale_factor / Y; diff --git a/lib/pbio/drv/led/led_virtual.c b/lib/pbio/drv/led/led_virtual.c index 74b256a81..6a597a459 100644 --- a/lib/pbio/drv/led/led_virtual.c +++ b/lib/pbio/drv/led/led_virtual.c @@ -26,7 +26,7 @@ static pbio_error_t pbdrv_led_virtual_set_hsv(pbdrv_led_dev_t *dev, const pbio_color_hsv_t *hsv) { uint8_t id = (intptr_t)dev->pdata; - return pbdrv_virtual_call_method("led", id, "on_set_hsv", "IBBB", pbdrv_clock_get_us(), hsv->h, hsv->s, hsv->v); + return pbdrv_virtual_call_method("led", id, "on_set_hsv", "IBBB", pbdrv_clock_get_us(), hsv->h, hsv->s, pbio_color_hsv_get_v(hsv)); } static const pbdrv_led_funcs_t pbdrv_led_virtual_funcs = { diff --git a/lib/pbio/include/pbio/color.h b/lib/pbio/include/pbio/color.h index 0294af8e6..bc0e7f262 100644 --- a/lib/pbio/include/pbio/color.h +++ b/lib/pbio/include/pbio/color.h @@ -92,18 +92,24 @@ typedef struct { uint16_t h; /** The saturation component. 0 to 100 percent. */ uint8_t s; - /** The value component. 0 to 100 percent. */ - uint8_t v; + /** The value component. Normally 0 to 100 percent but allowed to be + * negative to provide higher contrast in color scanning applications. */ + int8_t v; } pbio_color_hsv_t; +static inline uint8_t pbio_color_hsv_get_v(const pbio_color_hsv_t *hsv) { + return hsv->v < 0 ? 0 : hsv->v; +} + /** Compressed HSV color. Stores data in 24 bytes instead of 32. */ typedef struct __attribute__((__packed__)) { /** The hue component. 0 to 359 degrees. */ uint16_t h : 9; /** The saturation component. 0 to 100 percent. */ uint8_t s : 7; - /** The value component. 0 to 100 percent. */ - uint8_t v; + /** The value component. Normally 0 to 100 percent but allowed to be + * negative to provide higher contrast in color scanning applications. */ + int8_t v; } pbio_color_compressed_hsv_t; void pbio_color_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv); diff --git a/lib/pbio/include/pbio/light.h b/lib/pbio/include/pbio/light.h index a088ae6c6..3586632bf 100644 --- a/lib/pbio/include/pbio/light.h +++ b/lib/pbio/include/pbio/light.h @@ -29,7 +29,7 @@ typedef struct _pbio_color_light_t pbio_color_light_t; { .h = (hue), .s = (saturation), .v = (value) } /** Sentinel value for a color light animation array. */ -#define PBIO_COLOR_LIGHT_ANIMATION_END_V (UINT8_MAX) +#define PBIO_COLOR_LIGHT_ANIMATION_END_V (INT8_MAX) #define PBIO_COLOR_LIGHT_ANIMATION_END_HSV { .v = PBIO_COLOR_LIGHT_ANIMATION_END_V } #if PBIO_CONFIG_LIGHT diff --git a/lib/pbio/src/color/conversion.c b/lib/pbio/src/color/conversion.c index e7d52e6b2..4888f6548 100644 --- a/lib/pbio/src/color/conversion.c +++ b/lib/pbio/src/color/conversion.c @@ -105,7 +105,7 @@ void pbio_color_hsv_to_rgb(const pbio_color_hsv_t *hsv, pbio_color_rgb_t *rgb) { // the output more visually linear. // Scale 0..100 percent to 0..255 - uint8_t value = 327 * hsv->v / 128; + uint8_t value = 327 * pbio_color_hsv_get_v(hsv) / 128; uint8_t saturation = 327 * hsv->s / 128; // The brightness floor is minimum number that all of diff --git a/lib/pbio/test/src/test_color.c b/lib/pbio/test/src/test_color.c index f5aa7b067..0c4cf3af0 100644 --- a/lib/pbio/test/src/test_color.c +++ b/lib/pbio/test/src/test_color.c @@ -108,6 +108,15 @@ static void test_hsv_to_rgb(void *env) { pbio_color_hsv_t hsv; pbio_color_rgb_t rgb; + // no-color (negative brightness) + hsv.h = 0; + hsv.s = 0; + hsv.v = -50; + pbio_color_hsv_to_rgb(&hsv, &rgb); + tt_want_int_op(rgb.r, ==, 0); + tt_want_int_op(rgb.g, ==, 0); + tt_want_int_op(rgb.b, ==, 0); + // black hsv.h = 0; hsv.s = 0; diff --git a/lib/pbio/test/src/test_color_light.c b/lib/pbio/test/src/test_color_light.c index 9b0c33ecb..346599b68 100644 --- a/lib/pbio/test/src/test_color_light.c +++ b/lib/pbio/test/src/test_color_light.c @@ -37,7 +37,7 @@ static const pbio_color_compressed_hsv_t test_animation[] = { static pbio_error_t test_light_set_hsv(pbio_color_light_t *light, const pbio_color_hsv_t *hsv) { test_light_set_hsv_call_count++; test_light_set_hsv_last_hue = hsv->h; - test_light_set_hsv_last_brightness = hsv->v; + test_light_set_hsv_last_brightness = pbio_color_hsv_get_v(hsv); return PBIO_SUCCESS; } diff --git a/pybricks/parameters/pb_type_color.c b/pybricks/parameters/pb_type_color.c index 3bf9dd2e5..da3ed8be6 100644 --- a/pybricks/parameters/pb_type_color.c +++ b/pybricks/parameters/pb_type_color.c @@ -12,6 +12,7 @@ #endif #include +#include #include "py/objstr.h" @@ -112,12 +113,10 @@ static mp_obj_t pb_type_Color_make_new_helper(mp_int_t h, mp_int_t s, mp_int_t v self->hsv.h = h < 0 ? h + 360 : h; // Bind s to 0--100 - s = s < 0 ? 0 : s; - self->hsv.s = s > 100 ? 100 : s; + self->hsv.s = pbio_int_math_bind(s, 0, 100); - // Bind v to 0--100 - v = v < 0 ? 0 : v; - self->hsv.v = v > 100 ? 100 : v; + // Bind v to -100 to 100 + self->hsv.v = pbio_int_math_clamp(v, 100); return MP_OBJ_FROM_PTR(self); } @@ -161,7 +160,7 @@ void pb_type_Color_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kin // Otherwise, print hsv representation that can be evaluated pb_type_Color_obj_t *self = MP_OBJ_TO_PTR(self_in); - mp_printf(print, "Color(h=%u, s=%u, v=%u)", self->hsv.h, self->hsv.s, self->hsv.v); + mp_printf(print, "Color(h=%u, s=%u, v=%d)", self->hsv.h, self->hsv.s, self->hsv.v); } STATIC mp_obj_t pb_type_Color_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value) { diff --git a/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c b/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c index 15d0219d8..93595d4c5 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c @@ -37,7 +37,7 @@ STATIC uint8_t get_color_id(mp_obj_t color_in) { const pbio_color_hsv_t *hsv = pb_type_Color_get_hsv(color_in); // Brightness is defined in 10 increments. - uint8_t brightness = hsv->v / 10; + uint8_t brightness = pbio_color_hsv_get_v(hsv) / 10; pb_powered_up_color_id_t color; // For low saturation, assume grayscale. diff --git a/pybricks/util_mp/pb_obj_helper.c b/pybricks/util_mp/pb_obj_helper.c index fb386e8a3..fa70edf33 100644 --- a/pybricks/util_mp/pb_obj_helper.c +++ b/pybricks/util_mp/pb_obj_helper.c @@ -3,6 +3,7 @@ #include #include +#include #include "py/builtin.h" #include "py/mpconfig.h" @@ -104,7 +105,7 @@ mp_int_t pb_obj_get_hue(mp_obj_t arg) { void pb_obj_get_hsv(mp_obj_t arg, pbio_color_hsv_t *hsv) { hsv->h = pb_obj_get_hue(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(0), MP_OBJ_SENTINEL)); hsv->s = pb_obj_get_pct(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(1), MP_OBJ_SENTINEL)); - hsv->v = pb_obj_get_pct(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(2), MP_OBJ_SENTINEL)); + hsv->v = pbio_int_math_clamp(pb_obj_get_int(mp_obj_subscr(arg, MP_OBJ_NEW_SMALL_INT(2), MP_OBJ_SENTINEL)), 100); } mp_obj_t pb_obj_new_fraction(int32_t numerator, int32_t denominator) { From cd1d7b792c0cce742b957fefed839518285357d7 Mon Sep 17 00:00:00 2001 From: Johannes Ringler Date: Mon, 5 Sep 2022 18:49:02 +0200 Subject: [PATCH 3/6] pbio/color: Add new hsv bicone distance function. This new distance function estimates the similarity of two hsv colors by calculating their euclidean distance when mapped into a Hue-Chroma-Lightness bicone. This is much more robust for realistic colors, especially when low saturation or low value is involved. --- CHANGELOG.md | 3 + bricks/_common/sources.mk | 1 + lib/pbio/include/pbio/color.h | 1 + lib/pbio/src/color/util.c | 65 ++++++++ lib/pbio/test/src/test_color.c | 253 ++++++++++++++++++++++++++++++++ pybricks/util_pb/pb_color_map.c | 31 +--- 6 files changed, 325 insertions(+), 29 deletions(-) create mode 100644 lib/pbio/src/color/util.c diff --git a/CHANGELOG.md b/CHANGELOG.md index c939c8ead..ceed3149e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,11 +8,14 @@ - The `use_gyro` method is added to the normal `DriveBase` class instead of having a separate `GyroDriveBase` class. Since the latter was only released in beta versions, this is not a breaking change ([support#1054]). +- New color distance function used by the color sensors that is more + consistent when distinguishing user-provided colors ([pybricks-micropython#104]). ### Fixed - Improved external device detection speed ([support#1140]). - Fixed Powered Up Tilt Sensor not working ([support#1189]). +[pybricks-micropython#104]: https://github.com/pybricks/pybricks-micropython/pull/104 [support#1054]: https://github.com/pybricks/support/issues/1054 [support#1140]: https://github.com/pybricks/support/issues/1140 [support#1189]: https://github.com/pybricks/support/issues/1189 diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index e66367bd2..d6e0e066f 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -190,6 +190,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/angle.c \ src/battery.c \ src/color/conversion.c \ + src/color/util.c \ src/control.c \ src/control_settings.c \ src/dcmotor.c \ diff --git a/lib/pbio/include/pbio/color.h b/lib/pbio/include/pbio/color.h index bc0e7f262..8dc464b2b 100644 --- a/lib/pbio/include/pbio/color.h +++ b/lib/pbio/include/pbio/color.h @@ -118,6 +118,7 @@ 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); #endif // _PBIO_COLOR_H_ diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c new file mode 100644 index 000000000..0d6b5756d --- /dev/null +++ b/lib/pbio/src/color/util.c @@ -0,0 +1,65 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2022 The Pybricks Authors + +#include + +// parabola approximating the first 90 degrees of sine. (0,90) to (0, 10000) +static int32_t sin_deg_branch0(int32_t x) { + return (201 - x) * x; +} + +// integer sine approximation from degrees to (-10000, 10000) +static int32_t sin_deg(int32_t x) { + x = x % 360; + if (x < 90) { + return sin_deg_branch0(x); + } + if (x < 180) { + return sin_deg_branch0(180 - x); + } + if (x < 270) { + return -sin_deg_branch0(x - 180); + } + return -sin_deg_branch0(360 - x); +} + +static int32_t cos_deg(int32_t x) { + return sin_deg(x + 90); +} + +/** + * Gets squared Euclidean distance between HSV colors mapped into a chroma-lightness-bicone. + * The bicone is 20000 units tall and 20000 units in diameter. + * @param [in] hsv_a The first HSV color. + * @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 a_h = hsv_a->h; + int32_t a_s = hsv_a->s; + int32_t a_v = hsv_a->v; + + int32_t b_h = hsv_b->h; + int32_t b_s = hsv_b->s; + int32_t b_v = hsv_b->v; + + // chroma (= radial coordinate in bicone) of a and b (0-10000) + int32_t radius_a = a_v * a_s; + int32_t radius_b = b_v * b_s; + + // lightness (= z-coordinate in bicone) of a and b (0-20000) + int32_t lightness_a = (200 * a_v - a_s * a_v); + int32_t lightness_b = (200 * b_v - b_s * b_v); + + // x and y deltas of a and b in HSV bicone (-20000, 20000) + int32_t delx = (radius_b * cos_deg(b_h) - radius_a * cos_deg(a_h)) / 10000; + int32_t dely = (radius_b * sin_deg(b_h) - radius_a * sin_deg(a_h)) / 10000; + // z delta of a and b in HSV bicone (-20000, 20000) + int32_t delz = (lightness_b - lightness_a); + + // Squared Euclidean distance (0, 400000000) + int32_t cdist = delx * delx + dely * dely + delz * delz; + + return cdist; +} diff --git a/lib/pbio/test/src/test_color.c b/lib/pbio/test/src/test_color.c index 0c4cf3af0..3ee5df701 100644 --- a/lib/pbio/test/src/test_color.c +++ b/lib/pbio/test/src/test_color.c @@ -357,11 +357,264 @@ static void test_color_hsv_compression(void *env) { tt_want_int_op(hsv.v, ==, expanded.v); } +static void test_color_hsv_cost(void *env) { + pbio_color_hsv_t color_a; + pbio_color_hsv_t color_b; + int32_t dist; + + // color compared to itself should give 0 + 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); + + // blacks with different saturations/hues should be the same + color_a.h = 230; + color_a.s = 23; + color_a.v = 0; + + 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); + + // colors with different hues should be different when value>0 and saturation>0 + color_a.h = 230; + color_a.s = 99; + color_a.v = 100; + + 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); + + // grays with different hues should be the same + color_a.h = 230; + color_a.s = 0; + color_a.v = 50; + + 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); + + // distance should be greater when saturation is greater + color_a.h = 30; + color_a.s = 20; + color_a.v = 70; + + color_b.h = 60; + color_b.s = 20; + color_b.v = 70; + + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + + color_a.h = 30; + color_a.s = 40; + color_a.v = 70; + + color_b.h = 60; + color_b.s = 40; + color_b.v = 70; + + tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist); + + // resolve colors that are close + color_a.h = 30; + color_a.s = 20; + color_a.v = 70; + + color_b.h = 35; + color_b.s = 20; + color_b.v = 70; + + tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + + color_a.h = 30; + color_a.s = 20; + color_a.v = 70; + + color_b.h = 30; + color_b.s = 25; + color_b.v = 70; + + tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + + color_a.h = 30; + color_a.s = 20; + color_a.v = 70; + + color_b.h = 30; + color_b.s = 20; + color_b.v = 75; + + tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0); + + // hues 360 and 0 should be the same + color_a.h = 360; + color_a.s = 100; + color_a.v = 100; + + 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); + + // distance between hues 359 and 1 should be smaller than hues 1 and 5 + color_a.h = 359; + color_a.s = 100; + color_a.v = 100; + + color_b.h = 1; + color_b.s = 100; + color_b.v = 100; + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + + color_a.h = 1; + color_a.s = 100; + color_a.v = 100; + + color_b.h = 5; + color_b.s = 100; + color_b.v = 100; + + tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist); + + // check distance is monotonous along several color paths. This should catch potential int overflows + int prev_dist = 0; + bool monotone = true; + + // along saturation + color_a.h = 180; + color_a.s = 0; + color_a.v = 100; + + color_b.h = 180; + color_b.s = 0; + color_b.v = 100; + + while (color_a.s < 100) { + color_a.s += 5; + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + + if (dist <= prev_dist) { + monotone = false; + break; + } + prev_dist = dist; + } + tt_want(monotone); + + // along value + + prev_dist = 0; + monotone = true; + + color_a.h = 180; + color_a.s = 100; + color_a.v = 0; + + color_b.h = 180; + color_b.s = 100; + color_b.v = 0; + + while (color_a.v < 100) { + color_a.v += 5; + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + + if (dist <= prev_dist) { + monotone = false; + break; + } + prev_dist = dist; + } + tt_want(monotone); + + // along value, saturation 0 + + prev_dist = 0; + monotone = true; + + color_a.h = 180; + color_a.s = 0; + color_a.v = 0; + + color_b.h = 180; + color_b.s = 0; + color_b.v = 0; + + while (color_a.v < 100) { + color_a.v += 5; + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + + if (dist <= prev_dist) { + monotone = false; + break; + } + prev_dist = dist; + } + tt_want(monotone); + + // along chroma + + prev_dist = 0; + monotone = true; + + color_a.h = 180; + color_a.s = 100; + color_a.v = 100; + + color_b.h = 180; + color_b.s = 100; + color_b.v = 100; + + for (int i = -19; i < 21; i++) { + color_a.s = i < 0 ? -i * 5 : i * 5; + 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); + + if (dist <= prev_dist) { + monotone = false; + } + prev_dist = dist; + } + tt_want(monotone); + + // check max distances + + color_a.h = 0; + color_a.s = 100; + color_a.v = 100; + + color_b.h = 180; + color_b.s = 100; + color_b.v = 100; + + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + tt_want_int_op(dist, >, 390000000); + tt_want_int_op(dist, <, 410000000); + + color_a.h = 0; + color_a.s = 0; + color_a.v = 0; + + color_b.h = 0; + color_b.s = 0; + color_b.v = 100; + + dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b); + tt_want_int_op(dist, >, 390000000); + tt_want_int_op(dist, <, 410000000); +} + struct testcase_t pbio_color_tests[] = { PBIO_TEST(test_rgb_to_hsv), PBIO_TEST(test_hsv_to_rgb), PBIO_TEST(test_color_to_hsv), PBIO_TEST(test_color_to_rgb), PBIO_TEST(test_color_hsv_compression), + PBIO_TEST(test_color_hsv_cost), END_OF_TESTCASES }; diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index 23cb3d9b4..c2caa9936 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/pybricks/util_pb/pb_color_map.c @@ -13,6 +13,7 @@ #include #include +#include #include "py/obj.h" @@ -71,34 +72,6 @@ void pb_color_map_save_default(mp_obj_t *color_map) { *color_map = MP_OBJ_FROM_PTR(&pb_color_map_default); } -// Cost function between two colors a and b. The lower, the closer they are. -static int32_t get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c) { - - // Calculate the hue error - int32_t hue_error; - - if (c->s <= 5 || x->s <= 5) { - // When comparing against unsaturated colors, - // the hue error is not so relevant. - hue_error = 0; - } else { - hue_error = c->h > x->h ? c->h - x->h : x->h - c->h; - if (hue_error > 180) { - hue_error = 360 - hue_error; - } - } - - // Calculate the value error: - int32_t value_error = x->v > c->v ? x->v - c->v : c->v - x->v; - - // Calculate the saturation error, with extra penalty for low saturation - int32_t saturation_error = x->s > c->s ? x->s - c->s : c->s - x->s; - saturation_error += (100 - c->s) / 2; - - // Total error - return hue_error * hue_error + 5 * saturation_error * saturation_error + 2 * value_error * value_error; -} - // Get a discrete color that matches the given hsv values most closely mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv) { @@ -116,7 +89,7 @@ mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv) { for (size_t i = 0; i < n; i++) { // Evaluate the cost function - cost_now = get_hsv_cost(hsv, pb_type_Color_get_hsv(colors[i])); + cost_now = pbio_color_get_bicone_squared_distance(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 3346936b47a864347b054a21f4f5310dbda9a761 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 4 Jul 2023 16:15:09 +0200 Subject: [PATCH 4/6] pybricks/util_pb: Drop s and v adjustment hacks. The newly introduced color cost function intrinsically deals with low s and v, so we don't need to artificially suppress them here. --- pybricks/util_pb/pb_color_map.c | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index c2caa9936..6e8b506d2 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/pybricks/util_pb/pb_color_map.c @@ -32,16 +32,6 @@ void pb_color_map_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv) // Standard conversion pbio_color_rgb_to_hsv(rgb, hsv); - // For very low values, saturation is not reliable - if (hsv->v <= 3) { - hsv->s = 0; - } - - // For very low values, hue is not reliable - if (hsv->s <= 3) { - hsv->h = 0; - } - // Slight shift for lower hues to make yellow somewhat more accurate if (hsv->h < 40) { uint8_t offset = ((hsv->h - 20) << 8) / 20; From d183834b066c2eab00bbe30c637c6377dd937169 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 4 Jul 2023 16:17:49 +0200 Subject: [PATCH 5/6] pybricks.parameters.Color.NONE: Set v=-40. With the newly introduced color cost function, any measured value less than 50 results in Color.NONE, which makes it the predominant result. This reduces the distance range for the default colors. Instead of adjusting all colors (which are also used to emit colors), we can make the value of Color.NONE negative to achieve a similar result. --- pybricks/parameters/pb_type_color.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pybricks/parameters/pb_type_color.c b/pybricks/parameters/pb_type_color.c index da3ed8be6..cc1204aed 100644 --- a/pybricks/parameters/pb_type_color.c +++ b/pybricks/parameters/pb_type_color.c @@ -68,7 +68,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, 0} + .hsv = {0, 0, -40} }; const pb_type_Color_obj_t pb_Color_BLACK_obj = { From 7d400e51f9d81804c48a2d6738d0f822dd1061d2 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 5 Jul 2023 11:55:22 +0200 Subject: [PATCH 6/6] pbio/color/util: Move math to int_math. Also ensure we pick the proper sign for v in radial computations. --- lib/pbio/include/pbio/int_math.h | 2 + lib/pbio/src/color/util.c | 65 ++++++++++---------------------- lib/pbio/src/int_math.c | 43 +++++++++++++++++++++ 3 files changed, 64 insertions(+), 46 deletions(-) diff --git a/lib/pbio/include/pbio/int_math.h b/lib/pbio/include/pbio/int_math.h index c6d355003..430cbf300 100644 --- a/lib/pbio/include/pbio/int_math.h +++ b/lib/pbio/include/pbio/int_math.h @@ -35,6 +35,8 @@ int32_t pbio_int_math_sign(int32_t a); int32_t pbio_int_math_atan2(int32_t y, int32_t x); int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c); int32_t pbio_int_math_sqrt(int32_t n); +int32_t pbio_int_math_sin_deg(int32_t x); +int32_t pbio_int_math_cos_deg(int32_t x); #endif // _PBIO_INT_MATH_H_ diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c index 0d6b5756d..b2a2eb00a 100644 --- a/lib/pbio/src/color/util.c +++ b/lib/pbio/src/color/util.c @@ -2,64 +2,37 @@ // Copyright (c) 2018-2022 The Pybricks Authors #include - -// parabola approximating the first 90 degrees of sine. (0,90) to (0, 10000) -static int32_t sin_deg_branch0(int32_t x) { - return (201 - x) * x; -} - -// integer sine approximation from degrees to (-10000, 10000) -static int32_t sin_deg(int32_t x) { - x = x % 360; - if (x < 90) { - return sin_deg_branch0(x); - } - if (x < 180) { - return sin_deg_branch0(180 - x); - } - if (x < 270) { - return -sin_deg_branch0(x - 180); - } - return -sin_deg_branch0(360 - x); -} - -static int32_t cos_deg(int32_t x) { - return sin_deg(x + 90); -} +#include /** - * Gets squared Euclidean distance between HSV colors mapped into a chroma-lightness-bicone. - * The bicone is 20000 units tall and 20000 units in diameter. + * Gets squared Euclidean distance between HSV colors mapped into a + * chroma-lightness-bicone. The bicone is 20000 units tall and 20000 units in + * diameter. + * * @param [in] hsv_a The first HSV color. * @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 a_h = hsv_a->h; - int32_t a_s = hsv_a->s; - int32_t a_v = hsv_a->v; + // 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; + int32_t radius_b = pbio_color_hsv_get_v(hsv_b) * hsv_b->s; - int32_t b_h = hsv_b->h; - int32_t b_s = hsv_b->s; - int32_t b_v = hsv_b->v; + // Lightness (= z-coordinate in bicone) of a and b (0-20000). + // v is allowed to be negative, resulting in negative lightness. + // This can be used to create a higher contrast between "none-color" and + // normal colors. + int32_t lightness_a = (200 - hsv_a->s) * hsv_a->v; + int32_t lightness_b = (200 - hsv_b->s) * hsv_b->v; - // chroma (= radial coordinate in bicone) of a and b (0-10000) - int32_t radius_a = a_v * a_s; - int32_t radius_b = b_v * b_s; - - // lightness (= z-coordinate in bicone) of a and b (0-20000) - int32_t lightness_a = (200 * a_v - a_s * a_v); - int32_t lightness_b = (200 * b_v - b_s * b_v); + // z delta of a and b in HSV bicone (-20000, 20000). + int32_t delta_z = (lightness_b - lightness_a); // x and y deltas of a and b in HSV bicone (-20000, 20000) - int32_t delx = (radius_b * cos_deg(b_h) - radius_a * cos_deg(a_h)) / 10000; - int32_t dely = (radius_b * sin_deg(b_h) - radius_a * sin_deg(a_h)) / 10000; - // z delta of a and b in HSV bicone (-20000, 20000) - int32_t delz = (lightness_b - lightness_a); + int32_t delta_x = (radius_b * pbio_int_math_cos_deg(hsv_b->h) - radius_a * pbio_int_math_cos_deg(hsv_a->h)) / 10000; + int32_t delta_y = (radius_b * pbio_int_math_sin_deg(hsv_b->h) - radius_a * pbio_int_math_sin_deg(hsv_a->h)) / 10000; // Squared Euclidean distance (0, 400000000) - int32_t cdist = delx * delx + dely * dely + delz * delz; - - return cdist; + return delta_x * delta_x + delta_y * delta_y + delta_z * delta_z; } diff --git a/lib/pbio/src/int_math.c b/lib/pbio/src/int_math.c index e525b187c..dc89112bf 100644 --- a/lib/pbio/src/int_math.c +++ b/lib/pbio/src/int_math.c @@ -294,3 +294,46 @@ int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c) { assert(result == (int64_t)a * (int64_t)b / (int64_t)c); return result; } + +/** + * Approximates first 90-degree segment of a sine in degrees, output + * upscaled by 10000. + * + * @param [in] x Angle in degrees (0-90). + * @returns Approximately sin(x) * 10000. + */ +static int32_t pbio_int_math_sin_deg_branch0(int32_t x) { + return (201 - x) * x; +} + +// integer sine approximation from degrees to (-10000, 10000) + +/** + * Approximates sine of an angle in degrees, output upscaled by 10000. + * + * @param [in] x Angle in degrees. + * @returns Approximately sin(x) * 10000. + */ +int32_t pbio_int_math_sin_deg(int32_t x) { + x = x % 360; + if (x < 90) { + return pbio_int_math_sin_deg_branch0(x); + } + if (x < 180) { + return pbio_int_math_sin_deg_branch0(180 - x); + } + if (x < 270) { + return -pbio_int_math_sin_deg_branch0(x - 180); + } + return -pbio_int_math_sin_deg_branch0(360 - x); +} + +/** + * Approximates cosine of an angle in degrees, output upscaled by 10000. + * + * @param [in] x Angle in degrees. + * @returns Approximately cos(x) * 10000. + */ +int32_t pbio_int_math_cos_deg(int32_t x) { + return pbio_int_math_sin_deg(x + 90); +}