From 015a604298b592d41ff0ed7ac142d6ee675bc149 Mon Sep 17 00:00:00 2001 From: Johannes Ringler Date: Wed, 9 Feb 2022 17:58:13 +0100 Subject: [PATCH] util_pb/pb_color_map: HSV cone distance as error Implement the color error by calculating the cartesian distance between the colors in the HSV cone. This should be more insensitive to fluctuating hue values on dark and unsaturated colors. This has been discussed further in https://github.com/pybricks/support/issues/627 --- pybricks/util_pb/pb_color_map.c | 74 ++++++++++++++++++++++----------- 1 file changed, 49 insertions(+), 25 deletions(-) diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index c4b07a7f0..206726ac2 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/pybricks/util_pb/pb_color_map.c @@ -8,6 +8,8 @@ #include #include +#include + #include #include "py/obj.h" @@ -68,31 +70,53 @@ void pb_color_map_save_default(mp_obj_t *color_map) { } // 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; +static int32_t get_hsv_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b) { + + // normalize h to radians, s/v to (0,1) + fix16_t a_h = fix16_deg_to_rad(fix16_from_int(a->h)); + fix16_t b_h = fix16_deg_to_rad(fix16_from_int(b->h)); + fix16_t a_s = fix16_div(fix16_from_int(a->s), fix16_from_int(100)); + fix16_t b_s = fix16_div(fix16_from_int(b->s), fix16_from_int(100)); + fix16_t a_v = fix16_div(fix16_from_int(a->v), fix16_from_int(100)); + fix16_t b_v = fix16_div(fix16_from_int(b->v), fix16_from_int(100)); + fix16_t piovertwo = fix16_div(fix16_pi, fix16_from_int(2)); + + // x, y and z deltas between cartesian coordinates of a and b in HSV cone + // delx = b_s*b_v*cos(b_h) - a_s*a_v*cos(a_h) + fix16_t delx = fix16_sub( + fix16_mul( + fix16_mul( + fix16_sin_parabola(fix16_add(b_h, piovertwo)), + b_v), + a_s), + fix16_mul( + fix16_mul( + fix16_sin_parabola(fix16_add(a_h, piovertwo)), + a_v), + b_s)); + + // dely = b_s*b_v*sin(b_h) - a_s*a_v*sin(a_h) + fix16_t dely = fix16_sub( + fix16_mul( + fix16_mul( + fix16_sin_parabola(b_h), + b_v), + a_s), + fix16_mul( + fix16_mul( + fix16_sin_parabola(a_h), + a_v), + b_s)); + // delz = b_v - a_v + fix16_t delz = fix16_sub(b_v, a_v); + + // cdist = delx*delx + dely*dely + delz*delz + fix16_t cdist = fix16_add( + fix16_add( + fix16_sq(delx), + fix16_sq(dely)), + fix16_sq(delz)); + return fix16_to_int(cdist); } // Get a discrete color that matches the given hsv values most closely