From e58b07f21c7b797631fe59371e61306a1f33af58 Mon Sep 17 00:00:00 2001 From: Johannes Ringler Date: Mon, 5 Sep 2022 18:49:02 +0200 Subject: [PATCH] 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. The default colormap now uses the calibrated brick colors. --- CHANGELOG.md | 6 + 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 | 41 +----- 6 files changed, 333 insertions(+), 34 deletions(-) create mode 100644 lib/pbio/src/color/util.c diff --git a/CHANGELOG.md b/CHANGELOG.md index 7dfbb160a..9caa18da8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,12 @@ - Fixed Technic (Extra) Large motors not working ([support#1131]) on all hubs. - Fixed Powered Up Light not working ([support#1131]) on all hubs. +### Changed + +- New color distance function used by the color sensors that is more + consistent when distinguishing user-provided colors ([pybricks-micropython#104]). + +[pybricks-micropython#104]: https://github.com/pybricks/pybricks-micropython/pull/104 [support#1064]: https://github.com/pybricks/support/issues/1131 ## [3.3.0b7] - 2023-06-30 diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index 62343f91f..55f39e554 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -191,6 +191,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 0294af8e6..ba515e358 100644 --- a/lib/pbio/include/pbio/color.h +++ b/lib/pbio/include/pbio/color.h @@ -112,6 +112,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 f5aa7b067..ed7dd94f3 100644 --- a/lib/pbio/test/src/test_color.c +++ b/lib/pbio/test/src/test_color.c @@ -348,11 +348,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..f505b052c 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" @@ -57,11 +58,11 @@ STATIC const mp_rom_obj_tuple_t pb_color_map_default = { {&mp_type_tuple}, 6, { - MP_OBJ_FROM_PTR(&pb_Color_RED_obj), - MP_OBJ_FROM_PTR(&pb_Color_YELLOW_obj), - MP_OBJ_FROM_PTR(&pb_Color_GREEN_obj), - MP_OBJ_FROM_PTR(&pb_Color_BLUE_obj), - MP_OBJ_FROM_PTR(&pb_Color_WHITE_obj), + MP_OBJ_FROM_PTR(&pb_Color_BRICK_RED_obj), + MP_OBJ_FROM_PTR(&pb_Color_BRICK_YELLOW_obj), + MP_OBJ_FROM_PTR(&pb_Color_BRICK_GREEN_obj), + MP_OBJ_FROM_PTR(&pb_Color_BRICK_BLUE_obj), + MP_OBJ_FROM_PTR(&pb_Color_BRICK_WHITE_obj), MP_OBJ_FROM_PTR(&pb_Color_NONE_obj), } }; @@ -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) {