util_pb/pb_color_map: use HSV cone distance as error

[Novakasa]

I am trying to 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 pybricks/support#627

This doesn't crash the hub, but doesn't really seem to work yet, still need to figure out how to debug on the hub.

[Novakasa]

I am not an expert on C, but I would have expected to be able to use fix16_sin_parabola after just including:

#include <fixmath.h>

Because the function prototypes seem to be declared in fix16.h, I would expect this to work. Instead, I get the error:

arm-none-eabi-ld.exe: pb_color_map.c:(.text.pb_color_map_get_color+0xc4): undefined reference to `fix16_sin_parabola'

Instead, I have to do:

#include <fix16_trig.c>

which doesn't seem correct. Am I misunderstanding something? Might there be an issue in the build system?

Also, when using fix16_sin instead of fix16_sin_parabola I get the errors:

C:\Program Files (x86)\GNU Arm Embedded Toolchain\10 2021.10\bin\arm-none-eabi-ld.exe: build/firmware-no-checksum.elf section `.bss' will not fit in region `RAM'
C:\Program Files (x86)\GNU Arm Embedded Toolchain\10 2021.10\bin\arm-none-eabi-ld.exe: region `RAM' overflowed by 23156 bytes

Which I guess means these implementations are just too big to be used.

[dlech]

An easier way to debug this instead of running on the hub would be to create a new int32_t pbio_color_hsv_distance(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b) function in lib/pbio/src/color/conversion.c (or start a new file since this isn't really a conversion). Then add tests in lib/pbio/test/src/color.c.

[dlech]

Which I guess means these implementations are just too big to be used.

It is probably due to the cache table. It looks like it can be disabled.

diff --git a/bricks/stm32/stm32.mk b/bricks/stm32/stm32.mk
index 2d002a59..4fdfc386 100644
--- a/bricks/stm32/stm32.mk
+++ b/bricks/stm32/stm32.mk
@@ -406,7 +406,7 @@ LWRB_SRC_C = lib/lwrb/src/lwrb/lwrb.c
 
 # libfixmath
 
-COPT += -DFIXMATH_NO_CTYPE
+COPT += -DFIXMATH_NO_CTYPE -DFIXMATH_NO_CACHE -DFIXMATH_FAST_SIN
 
 LIBFIXMATH_SRC_C = $(addprefix lib/libfixmath/libfixmath/,\
 	fix16_sqrt.c \

[dlech]

which doesn't seem correct. Am I misunderstanding something? Might there be an issue in the build system?

We need to add the source file to the list of source files in the makefile.

diff --git a/bricks/stm32/stm32.mk b/bricks/stm32/stm32.mk
index 2d002a59..72f585cb 100644
--- a/bricks/stm32/stm32.mk
+++ b/bricks/stm32/stm32.mk
@@ -411,6 +411,7 @@ COPT += -DFIXMATH_NO_CTYPE
 LIBFIXMATH_SRC_C = $(addprefix lib/libfixmath/libfixmath/,\
 	fix16_sqrt.c \
 	fix16_str.c \
+	fix16_trig.c \
 	fix16.c \
 	uint32.c \
 	)

[Novakasa]

We need to add the source file to the list of source files in the makefile.

Makes sense, will do!

[Novakasa]

Which I guess means these implementations are just too big to be used.

It is probably due to the cache table. It looks like it can be disabled.

diff --git a/bricks/stm32/stm32.mk b/bricks/stm32/stm32.mk
index 2d002a59..4fdfc386 100644
--- a/bricks/stm32/stm32.mk
+++ b/bricks/stm32/stm32.mk
@@ -406,7 +406,7 @@ LWRB_SRC_C = lib/lwrb/src/lwrb/lwrb.c
 
 # libfixmath
 
-COPT += -DFIXMATH_NO_CTYPE
+COPT += -DFIXMATH_NO_CTYPE -DFIXMATH_NO_CACHE -DFIXMATH_FAST_SIN
 
 LIBFIXMATH_SRC_C = $(addprefix lib/libfixmath/libfixmath/,\
 	fix16_sqrt.c \

This produces the following error:

../../lib/libfixmath/libfixmath/fix16_trig.c: In function 'fix16_sin_parabola':
../../lib/libfixmath/libfixmath/fix16_trig.c:19:23: error: unused variable 'abs_retval' [-Werror=unused-variable]
   19 |  fix16_t abs_inAngle, abs_retval, retval;
      |                       ^~~~~~~~~~
cc1.exe: all warnings being treated as errors

This comes from -DFIXMATH_FAST_SIN specifically.
Would this require an upstream fix? I've checked the upstream repository and it doesn't seem to have been fixed yet.

But already with -DFIXMATH_NO_CACHE it seems we are below size threshold, for cityhub as well as debug config and movehub.

[dlech]

Would this require an upstream fix?

Yes. The maintainer has been quite responsive in the past if you want to do this.

[Novakasa]

OK, I'll give it a shot

[Novakasa]

This seems to work as of now, but I still want to move the function to pbio and add tests for it. Just need to find some time for it.

I also want to do some testing to see if it really improves on the previous method.

[laurensvalk]

Thanks for submitting this. It looks nice!

Which definition of the cone did you implement? It appears that cones are commonly associated with chroma (which we could calculate also). When saturation is used, it seems more common to work with a cylinder.

Is it one of the following, and if so, which one?

I've also been wondering if we may still need two scaling factors to set the relative dimensions of the cylinder or cone, which would amount to relative weights similar to what we have today.

[laurensvalk]

And after this is completed and merged, I think it's worth adding a little bit of (configurable) hysteresis as well. This can be done fairly trivially by requiring a minimal change in the cost function to update the returned color.

That makes it more reliable to reliably detect a change in color without jitter. We've used it here, but only for reflection. By applying this to the cost function, we may be able to get the same benefit for colors.

[Novakasa]

Which definition of the cone did you implement? It appears that cones are commonly associated with chroma (which we could calculate also). When saturation is used, it seems more common to work with a cylinder.

That's a good point. Using the cylinder would not be ideal as it is degenerate for the color black, giving arbitrary distances between different HSV parameters for black. For this reason, I have opted to adopt the cone (Fig 3b). While I was not aware of the distinction between chroma and saturation, I think I intuitively used chroma anyways because that's the intuitive way to map it into the cone (radial coordinate = saturation*value = chroma). Fun fact: the German wikipedia page also misleadingly labels the radial coordinate as saturation for the HSV cone)

Of course, using the bicone (Fig3a) might also be a valid approach, I might experiment with that as well.

I've also been wondering if we may still need two scaling factors to set the relative dimensions of the cylinder or cone, which would amount to relative weights similar to what we have today.

One scaling factor suffices, since only the ratio between HS-plane and V-axis matters. I have this in mind in my implementation, but need to figure out whether to calibrate it somehow or leave it to the user.

[Novakasa]

One other aspect I have been thinking about is the default colors. When testing this cost function with standard colors I found that it is more difficult to detect some standard LEGO colors. Especially for standard LEGO green I found that it has quite a low value and thus is not very close to the pybricks Color.GREEN, whereas the previous cost function was very sensitive to hue in this case and did identify the lego green as Color.GREEN. This is of course also a matter of calibrating the scaling factor in the cone cost function, which I plan to look at more systematically.

But beyond that, would it make sense to provide calibrated colors for standard LEGO colors?

[dlech]

But beyond that, would it make sense to provide calibrated colors for standard LEGO colors?

If we can come up with values that work 80% of the time in a wide variety of lighting conditions, then yes, it could be worth it. But if users will always have to calibrate anyway, then it isn't worth it.

[laurensvalk]

But beyond that, would it make sense to provide calibrated colors for standard LEGO colors?

The problem here isn't so much LEGO colors or the color mapping cost function per se. The main thing here is that we also use the default Color values for emitted light, with 100% saturation and value.

In the current mapping, this provides a nice compromise, with the default colors (None, Red, Green, Blue, Yellow) being detected very well. In my tests, default Pybricks performance was a lot better than with the standard LEGO color mode.

Even if we provided other colors, it’s not really clear what they should be, since S and V are distance dependent. We could provide Color.BRICK_YELLOW_1CM, but this might be the point where users are better off providing their own colors.

Notice also that the current cost function mapping isn't strictly symmetric. In the current version, we assign more meaning to the requested color than the measured color. The reasoning for this should probably be documented a bit better.

Separately from this, especially for green and yellow, see also the hack in pb_color_map_rgb_to_hsv which needs to be fixed (this is a separate issue). With the standard RGB to HSV conversion, the LEGO sensors are very much off in the yellow range due to the way they are calibrated. We currently have an ugly hack to shift it, but this could be a lot better. @dlech may have some equipment for this now :)

[Novakasa]

I might try to run the test at tests/pup/sensors/color_color.py soon but it seems to assume some kind of contraption for testing the sensor. I remember that I have at some point seen a picture of one of theses devices you built for the tests, but can't find it anymore. @laurensvalk @dlech do you still have it documented somewhere?

Edit: found it: pybricks/pybricks-coverage#1 (comment)
Will need to adapt it though, I have only Powered up motors and sensors.

[Novakasa]

So I've been testing my PR to recognize the standard PB colors and still had some difficulty with Color.GREEN. Because the value of measured LEGO green is relatively low (~52), it also has quite low chroma (=sat*val). For that reason, it is closer to Color.WHITE in the cone rather than Color.GREEN. See this image (red point is the measured green, the leftmost dot is Color.GREEN, only points in the green-magenta-plane are displayed):

One thing that does help however, is making the radial coordinate non-linear in the value. By using s*(1-(1-v)*(1-v)) = s*v*(2-v) instead of s*v as the radial coordinate, this suffices in recognizing GREEN as well.

Another approach I took was testing whether the bicone as referenced above would improve this situation.
The naive implementation looks like this:

This now make the distance between the measured color and WHITE much larger. However, we are now getting closer to black (= Color.None), so with chroma_weight=50 we are hovering between GREEN and None. However, using a chroma_weight of 10, actually makes it so that we can now detect Color.GREEN pretty consistently:

I am though not sure about it's reliability, it seems for now it is pretty close to being closer to None still. Another approach could be to also use a radial coordinate non-linear in s and v, which looks like this:

This has the most robust similarity between the measured color and Color.GREEN to my eyes, but also has the most complex computations.

To my eyes, all of these approaches seem better suited than the previous implementation in case the user provides his own calibrated colors, but as seen above, it requires some massaging to properly recognize the default colors. Personally, I would probably go ahead with the cone or bicone approach with nonlinear radial coordinate.

Bonus picture:

[laurensvalk]

Thanks for all the work and extensive analysis so far! I don't have a lot of time at the moment, but reviewing this is definitely on my todo list.

Did you check out pybricks/support#116 and the following path we are currently using?

Maybe you're addressing this already, but if not, let's make sure we're not over-fitting the HSV mapping to this underlying RGB-to-HSV hack.

pybricks-micropython/pybricks/util_pb/pb_color_map.c

Lines 22 to 45 in f69bd69

	// This expands pbio_color_rgb_to_hsv with additional calibration steps that
	// ultimately must be properly done in pbio_color_rgb_to_hsv, just like
	// pbio_color_hsv_to_rgb, by adjusting RGB instead of hacking at the HSV value.
	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;
	int32_t scale = 200 - ((100 * (offset * offset)) >> 16);
	hsv->h = hsv->h * scale / 100;
	}

[Novakasa]

I did take a loot at pybricks/support#116, but somehow assumed it just affected the hue (which I wasn't too concerned about here). I seem to have missed the fact that it also changes the value/saturation. I'll take a look.

I'll squash some of the commits and give better commit messages once this PR approaches merging. Or do you prefer to keep this all as one commit?

[Novakasa]

If I disable these corrections:

pybricks-micropython/pybricks/util_pb/pb_color_map.c

Lines 47 to 49 in f69bd69

	// Value and saturation correction
	hsv->s = hsv->s * (200 - hsv->s) / 100;
	hsv->v = hsv->v * (200 - hsv->v) / 100;

I once again have problems with detecting GREEN rather than None, since the green value is just ~31. These lines are actually the same modification I use when mapping to the bicone radius, so basically the most recent commit applies this transformation twice.

[dlech]

I don't think we should be trying to make "green" LEGO bricks match Color.GREEN since they are clearly not the same colors. How well does it match to Color(141, 71, 47) instead? (I got these numbers from converting the RGB value for LEGO "green" from Rebrickable to HSV.)

[Novakasa]

Yeah, I think specific calibrated colors would work very well, and the color you provided is pretty close to what I get from sensor.hsv() (with pybricks/support#116 enabled). That would mean introducing new default colors, e.g. Color.LEGO_GREEN. In my experience the results are surprisingly independent of ambient lighting conditions as well, though I did not yet do much testing in broad daylight outside.

[laurensvalk]

So perhaps before adjusting the HSV cost matching function (this PR), maybe we could first have a closer look at fixing the RGB to HSV conversion in #pybricks/support#116.

I.e. @dlech has made a carefully crafted pbio_color_hsv_to_rgb function specifically for lights.

But our pbio_color_rgb_to_hsv is just the Wikipedia conversion and doesn't take sensor quirks into account (Other than this hack).

Do you have a SPIKE Color Sensor, @Novakasa? If not, we can send you one.

[Novakasa]

I don't have any SPIKE components, would be great if you have a spare

[laurensvalk]

I don't think we should be trying to make "green" LEGO bricks match Color.GREEN since they are clearly not the same colors. How well does it match to Color(141, 71, 47) instead?

Maybe I'm missing something, but I believe the basic LEGO brick colors (including green) are all detected very well with the default settings. So did this occur specifically with the default (master), or the modified cost function (this PR), or both?

[laurensvalk]

I don't have any SPIKE components, would be great if you have a spare

👍 I will reach out via email.

I'll squash some of the commits and give better commit messages once this PR approaches merging. Or do you prefer to keep this all as one commit?

Small changes/typos/fixes can generally be squashed into the relevant main commit, but it is fine to have more than one commit for different pieces of code. For example, see this branch. Coincidentally, it already has a squashed version of some of your commits :)

[laurensvalk]

I would also recommend trying out the PUPDevice class so you can access the original RGB values.

Then you can test various conversions in Python without having to reflash the firmware every time. You'll also be able to either duplicate our HSV conversions or adjust them as needed.

You could even take it a step further by having the sensor just continuously print the RGB data and do processing in your computer as it comes in.

[Novakasa]

I don't think we should be trying to make "green" LEGO bricks match Color.GREEN since they are clearly not the same colors. How well does it match to Color(141, 71, 47) instead?

Maybe I'm missing something, but I believe the basic LEGO brick colors (including green) are all detected very well with the default settings. So did this occur specifically with the default (master), or the modified cost function (this PR), or both?

In master, the default colors are detected very well in my testing.

The way I see it, the color error function in master is fine-tuned to work quite well with the task of detecting the default colors, but has some issues when trying to distinguish custom colors that can be closer to each other especially with low value and saturation.
With a naive cone/bicone approach, we can distinguish close and well-calibrated colors quite well, but lose the ability to match the "unrealistic" default colors.
Due to the discussion abolve (#93 (comment)) I still wanted to match the current default colors, and with the state of the latest commit, I think it works pretty well for both usecases (with the previous hack enabled). If we modify the hack, I should be able to adapt this PR accordingly.

If we chose to use more realistic colors for matching, that would make things a bit easier and we could probably have more default colors, but in any case I think the latest bicone approach seems quite robust to me even with the "unrealistic colors".

[dlech]

@Novakasa, what is this status on this PR? Is it ready to be merged or do you still want to make changes?

[laurensvalk]

Since we'll no longer be using libfixmath for motors, this might be quite a sizable increase in build size.

I would rather leave it open until we're definitely sure it's a big improvement and for a large number of users.

Here's perhaps a different idea: Instead of changing the cost function in the firmware, how about we generalize it so users can provide their own and write it in MicroPython?

For example:

def my_cost_function(sample: Color, detectable_color: Color):
    return 123456

sensor.detectable_colors(colors, cost_function=my_cost_function)

It seems like this would give all the existing advantages of using this detectable color feature, while giving power users a flexible tool to make it suit their application.

[Novakasa]

@dlech While this PR is close to ready, if libfixmath has been dropped from other locations, I honestly am not sure whether this PR is worth it currently (I am not sure how large the size cost is exactly). @laurensvalk proposal to instead allow for user function sounds quite good to me. If that would be performant enough I think that could be a good approach. We could provide an example to do hsv-cone matching and keep our previous approach as the default cost function (as it is tuned well for the default colors). It would be good to point out in the docs that the default color matching function is not suited well to match against "realistic" colors, especially with low saturations/values.

Another possibility would be to try to implement hsv-cone-cost without libfixmath. I might give this a shot if I find the time. However, I would prefer to not try to make the function match well the default colors and the calibrated colors at the same time, as it does sacrifice some simplicity. We could either change the default colors to match against more realistic ones (and maybe map it back to Color.GREEN etc before returning the color), or we use the original cost function by default, but change to the cone cost function when the user provides their own colors.

[dlech]

Sounds good. I'm in favor of adding a few predefined colors like Color.BRICK_RED, Color.BRICK_BLUE, etc. that would be better suited for detecting common LEGO brick colors. And if we could find a clever way to simplify the math to avoid the trig functions, all the better. Amazing work so far!

[Novakasa]

closing this in favor of #104