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.
pybricks · Sep 5, 2022 · c09c955 · c09c955
1 parent 4a3e7e5
commit c09c955
Show file tree

Hide file tree

Showing 9 changed files with 328 additions and 34 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -13,6 +13,8 @@
 - Implemented iterator protocol on `geometry.Matrix` class.
 - Added experimental support for multi-file
   projects ([pybricks-micropython#115]).
+- Added new calibrated `BRICK_*` colors used in the default
+  colormap ([pybricks-micropython#104]).
 
 ### Changed
 - Battery full indication (green light) comes on earlier ([support#647]).
@@ -27,6 +29,9 @@
 - On Prime Hub and Essential Hub, the user program is now stored in a section
   of the external flash that is not used by any file system of other known
   firmwares, in order to avoid compatibility issues when changing firmware.
+- New color distance function used by the color sensors that is more
+  consistent when distinguishing user-provided
+  colors ([pybricks-micropython#104]).
 
 ### Fixed
 - Fixed motors going out of sync when starting program ([support#679]).
@@ -37,6 +42,7 @@
   there is no program yet ([support#599]).
 
 [pybricks-micropython#115]: https://github.com/pybricks/pybricks-micropython/pull/115
+[pybricks-micropython#103]: https://github.com/pybricks/pybricks-micropython/pull/104
 [support#232]: https://github.com/pybricks/support/issues/232
 [support#232]: https://github.com/pybricks/support/issues/599
 [support#647]: https://github.com/pybricks/support/issues/647

diff --git a/bricks/ev3dev/Makefile b/bricks/ev3dev/Makefile
@@ -307,6 +307,7 @@ PYBRICKS_LIB_SRC_C = $(addprefix lib/,\
 	pbio/src/angle.c \
 	pbio/src/battery.c \
 	pbio/src/color/conversion.c \
+	pbio/src/color/util.c \
 	pbio/src/control.c \
 	pbio/src/control_settings.c \
 	pbio/src/dcmotor.c \

diff --git a/bricks/nxt/Makefile b/bricks/nxt/Makefile
@@ -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/bricks/stm32/stm32.mk b/bricks/stm32/stm32.mk
@@ -427,6 +427,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/bricks/virtualhub/mpconfigvariant.mk b/bricks/virtualhub/mpconfigvariant.mk
@@ -148,6 +148,7 @@ LIB_SRC_C += $(addprefix lib/,\
 	pbio/src/angle.c \
 	pbio/src/battery.c \
 	pbio/src/color/conversion.c \
+	pbio/src/color/util.c \
 	pbio/src/control.c \
 	pbio/src/control_settings.c \
 	pbio/src/dcmotor.c \

diff --git 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_get_bicone_cost(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
@@ -0,0 +1,57 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2018-2022 The Pybricks Authors
+
+#include <pbio/color.h>
+
+int32_t sin_deg_branch0(int32_t x) {
+    return 201 * x - x * x;
+}
+
+// integer sine approximation from degrees to (-10000, 10000)
+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);
+}
+
+int32_t cos_deg(int32_t x) {
+    return sin_deg(x + 90);
+}
+
+// gets squared cartesian distance between hsv colors mapped into a chroma-lightness-bicone
+int32_t pbio_get_bicone_cost(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);
+
+    int32_t cdist = delx * delx + dely * dely + delz * delz;
+
+    return cdist;
+}
diff --git a/lib/pbio/test/src/color.c b/lib/pbio/test/src/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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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_get_bicone_cost(&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
 };