HCL bicone color distance function (without libfixmath)

[Novakasa]

This replaces #93, opting not to use libfixmath.

Uses the euclidean distance between colors when mapped into this HCL-bicone:

I've tested this quite a bit and found it to be robust. It can distinguish between black and dark bluish gray and NONE very reliably.

I also added calibrated default colors with the BRICK_ prefix for a few standard lego colors and put them as new defaults. For now, I've chosen to use colors measured at a distance of 2 LEGO plates (=6.4 mm), but I have collected a lot of data so we can use other distance as well. If we chose colors at a larger distance, only their value will be lower, which might make the distance window larger to detect these colors rather than a darker one.

The sensor.detectable_colors() function now has a new kwarg chroma_weight which controls the relationship of relative height to width of the HCL-bicone used to estimate the color distance. Setting it to 0 would be equivalent seeing only black & white.

I consider this ready to review, with the following notes:

• I am not experienced with low level optimization. I've assumed that the compiler inlines my integer trigonometry functions and optimizes away some variables used mostly for readability.
• I personally have not seen the need for customizing the chroma_weight parameter so it might not be strictly necessary. Though to most users, it will have a useful default (50) and is entirely optional. EDIT: I removed it.
• I have only tested it with the ColorDistanceSensor, since I don't own other sensors and only PUP hubs. However the dependence of different sensors should only come in through the raw-to-hsv conversion which is out of scope of this PR, though I plan on looking at it further after this ([Feature] Calibrate HSV measurements support#116). If the raw-to-hsv calibration is done, most likely only the calibrated BRICK_ colors need to be changed. As of now, if the hsv output of the other sensors is at least somewhat consistent with the ColorDistanceSensor, the results should be quite robust for the other sensors as well.
• There is an issue when measuring a white lego brick at the optimal distance of 2 plates, however that seems to be a bug somewhere in the conversion from raw to hsv, as I see the raw RGB values at correct values whereas the hsv values have some suspicious artifacts (saturation jumps from ~0 to ~100 with little change of the raw values). This should probably be addressed when looking at [Feature] Calibrate HSV measurements support#116

[dlech]

FYI, this currently comes in at just under +500 bytes on the move hub firmware. About 1/2 of the size is the new color constants and 1/2 is the new implementation. I don't seen any obvious ways to save a few more bytes (other than leaving out the chroma_weight feature).

[dlech]

I have confirmed the improved discrimination of black vs. dark gray. What other measurable benefits does the bicone method have over the existing code?

[Novakasa]

This also helps with consistency in distinguishing bright colors from background colors. If we want to distinguish red from dark bluish gray, and add both colors to detectable_colors it works most of the time. However, due to the low saturation and value of DBG, small fluctuations in the raw RGB-values can produce a large variation in the hue ( See also some of my plots in pybricks/support#627 (comment)).

Since the weight of the hue-delta in the original cost function largely value- and saturation- independent (save for a cut-off), dark bluish gray would still sometimes be classified as red. We could change the cutoff, but any value will be a bit arbitrary and have some blind spots, so the most robust solution would be to weigh the hue linearly in chroma, which by itself is effectively the same as mapping the colors to a "prism" shape. If we then make the hue influence periodic, we get the HSV-cone.

I have no intuition for the size cost on move hub, but I am guessing that 500 bytes is quite large. Do we have some tools to measure size cost or do I just have to look at the resulting binary? I can try to optimize it more. I could go back to a previous approach where I morphed the bicone to make it match the previous default colors. Will not be as robust with varying distance though and sacrifices some of the simplicity of the distance function.

Would it be feasible to put this feature behind a #ifndef MOVE_HUB?

[dlech]

Do we have some tools to measure size cost or do I just have to look at the resulting binary?

Yes, it is printed when building the firmware:

BIN creating firmware base file
META creating firmware metadata
100524 bytes

(there is some variance in the size due to other factors, so consider this +/- 20 bytes of the "real" number)

Would it be feasible to put this feature behind a #ifndef MOVE_HUB?

We have recently freed up quite a bit of space on the move hub so 500 bytes isn't out of the question. If we leave out the chroma_weight option, then I think the code size for the improved color discrimination is really quite reasonable. And if we start running out of space again, we could possibly disable the BRICK_XYX color constants on move hub to free up some space.

[Novakasa]

I removed the chroma_weight kwarg. This reduces the size by 152 bytes on move hub. Now this PR adds 322 bytes compared to master.
Thanks to @laurensvalk, I have a Spike sensor now and tested this PR with it. I have noticed no difference in reliability to the ColorDistanceSensor.

This should be ready for review or merge if you guys are fine with it.

[Novakasa]

Should be ready now. Also changed

return 201 * x - x * x;

to

return (201 - x) * x;

[laurensvalk]

Rebased for easier review on latest firmware. Review in progress :)

[laurensvalk]

This looks nice, thanks for submitting this!

How do you feel about merging the new cost function, but leave it up to the user to add custom brick colors?

Since the Color type supports adding extra attributes, that's really easy to do in an end-user script like this.

from pybricks.parameters import Color

Color.BRICK_YELLOW = Color(140, 89, 48)

We can add an example in the docs with all available colors for users to pick from, including Color.BRICK_MEDIUM_AZURE etc.

This saves space and we wouldn't be limited to only a small set of brick colors --- users would have to add color objects anyway.

---

If that sounds OK to you I will go ahead and merge this.

[Novakasa]

If you mean that the default colormap would be empty and the user would always provide his own detectable colors, that sounds good to me.

However, if we went back to the default colormap with default Colors, as discussed in the previous PR (#93 (comment)), they are not reliably detected with realistic lego brick HSV values (Since e.g. realistic green is closer to Color.None or Color.WHITE than Color.GREEN in most metrics other than the previous specialized cost function). It is possible to modify this bicone metric so that it can detect the previous colors to an extent, but that would sacrifice some simplicity and would be a bit arbitrary (see my graphs in the previous PR)

[laurensvalk]

A couple of other notes:

• It might be good make the height and width units set with a #define. The build size would stay the same but the code may be easier to follow. It's hard to see now how to modify the various hardcoded numbers.
• Is there a rationale behind the current dimensions of the bicone? Or similarly, the weight in the cost function?

One issue seems to be that the euclidean distance hard-codes the relative weight of chroma and hue.

[laurensvalk]

This [radial coordinate non-linear in s and v] 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 [...] Personally, I would probably go ahead with the cone or bicone approach with nonlinear radial coordinate.

Originally posted by @Novakasa in #93 (comment)

Do you still have the formulas for this approach somewhere? Complexity in the cost function shouldn't be a major contributor to build size, so I'm curious about this one.

[laurensvalk]

Between the various problems addressed and tested here, is the following still your setup?

[Novakasa]

Do you still have the formulas for this approach somewhere? Complexity in the cost function shouldn't be a major contributor to build size, so I'm curious about this one.

For normalized S and V, the radial coordinate for each color would be V*(2-V)*S*(2-S) instead of just S*V. In practice it would look like v*(200-v)*s*(200-v)/10000 for values between 0 and 100.

• Is there a rationale behind the current dimensions of the bicone? Or similarly, the weight in the cost function?

The Lightness (z-coordinate in the cone) can be calculated without division or additional multiplication only if it ranges from 0 to 10000.
Since the x-/y- coordinates have to be rescaled anyway to prevent int overflow for the squared distance, I chose to rescale to the same range as the z-coordinate.
Thus, all the magic numbers in the current implementation are due to the range of the hsv-values (0-100) and factors of 2.

If we added an #define for an arbitrary scale, that would introduce some additional multiplications/divisions, but I could do it if that's not that important.
We could probably do a custom scale for the ratio of width/height without additional operations.

Between the various problems addressed and tested here, is the following still your setup?

Yes, though I have since moved on to processing the hsv values instead of using the sensor.color() method

[laurensvalk]

Thanks for the additional info.

Don't worry about updating the PR right now though, I'm still reading through all the issues and trying to set up something to compare. Especially after realizing that

One issue seems to be that the euclidean distance hard-codes the relative weight of chroma and hue.

I don't think scaling the cone would make much of a difference.

If I understand correctly, the original problem was mainly over-sensitivity to hue on low saturations/values. That's definitely something we can improve but based on some experiments I'm not sure that hard-coding the weight of hue relative to chroma is the best way to do it.

Yes, though I have since moved on to processing the hsv values instead of using the sensor.color() method

What does your new approach look like? Would it be better to just make the color map configurable?

[laurensvalk]

I'd also be curious to hear about specific problematic cases, so I can test them on my end too.

E.g. I think you mentioned green bricks on a gray track with a black background. Is it also a problem stationary or just at speed?

[Novakasa]

So my initial issue was like this: I had custom colors for the black background, the DBG tracks, and red and blue plates. I wanted to detect the red and blue plates and ignore the black and DBG colors. However, the color() method would sometimes return the red and blue color when it actually measured the DBG tracks. The reason for that is that the saturation fluctuates quite a bit for DBG, for the same reason that the saturation does not encode any real information for pure black.

That's the reason that to me it would make the most sense to rather take into account the chroma value than the saturation. The chroma inherently encodes the fact that the darker a color is, the less perceptually important its saturation is. However, that way we can't control the saturation/value weight individually anymore, which is a problem when trying to associate colors with their S=V=100 counterpart, but for any cost function that actually wants to measure the perceptual distance of colors, this is a good approach.

Once again, the core difficulty here is that we want a cost function that works well for custom calibrated colors as well as the default ones, which is why to me it seemed best to replace the default colors with realistic ones, but I am biased here since I only ever want to compare realistic colors.

What does your new approach look like? Would it be better to just make the color map configurable?

My new approach is a quite simple filter based on the chroma value, and then checking the hue only for the filtered colors. It works very well so I doubt I would replace it with a colormap approach again, but if there would have been the possibility to provide custom color distance function I would have used that with success as well.

[coveralls]

coverage: 56.278% (+0.3%) from 55.99% when pulling 7d400e5 on Novakasa:hsv-bicone-int into 04c7304 on pybricks:master.

[github-actions]

Download the artifacts for this pull request:

• cityhub-firmware-build-3068-git71703dfb.zip
• essentialhub-firmware-build-3068-git71703dfb.zip
• movehub-firmware-build-3068-git71703dfb.zip
• mpy-cross.zip
• nxt-firmware-build-3068-git71703dfb.zip
• pr_number.zip
• primehub-firmware-build-3068-git71703dfb.zip
• pybricks-micropython-build-3068-git71703dfb.zip
• technichub-firmware-build-3068-git71703dfb.zip

[laurensvalk]

Here is the extended PR: https://github.com/pybricks/pybricks-micropython/commits/hsv-bicone-int-2

Note that this keeps your main contribution unchanged, so this should remain a 1:1 to your tests and graphs.

The only difference are a few cleanups in separate commits, as an alternate way to address the default colors.

Curious to get your thoughts; happy to add my changes to your PR if you like.

[Novakasa]

Thanks for giving this another chance! The timing is quite neat, as I have found some situations in my project where my previous approach for detecting markers needs some more sophistication. To have full control though, I might need access to the hsv cost function in a user program. Would this be too costly to implement? In either case, if this PR gets merged I can just do a draft PR to expose the cost function to see how expensive it is.

Interesting idea to make None have negative value! If this makes the color detection robust with all default colors (Including green and white, which was problematic for me), this seems to be the best approach if we want to avoid adding calibrated colors.

Unfortunately, I might have difficulty testing this currently, as I don't have all the colors available to me currently (my collection has been moved). Maybe I can find some LEGO around the house in a few hours.

[laurensvalk]

That could certainly be an interesting addition. I can have a look. Has your program ever needed any kind of hysteresis or other internal state or has instantaneous detection usually worked well enough?

Also if you have found a better cost function since your original contribution, it would be easy enough to swap it in.

[Novakasa]

My previous approach was to record all hues above a certain chroma threshold. Once chroma is below the threshold I would average over the hues to match against the "marker hues". This worked flawlessly in my experience. Recently, I had to relax this to match already the first hue after the chroma threshold is met (to reject certain colors), and now I seem to get color detection issues from time to time. I did not have time to investigate this in full detail yet, but it seems like the detection worked better when I averaged over multiple hue samples. The first sample might capture the "transition" between two colors, which is a possible reason for the degraded reliability in this approach.

Writing this, I am honestly not sure why I now only take the first sample now, I might have had a good reason for this but maybe not, I need to check. (maybe writing this just solved my issues)

[Novakasa]

I think the HCL bicone is the best function that is a good compromise between simplicity and perceptual consistency, the only alternative to me is the regular HSV cone, but I find it unintuitive that white is exactly between two colors with opposite hues and max chroma. I think that in the HSV cone, Green would also be closer to white than None, so then the None.v=-30 trick would not work. (#93 (comment))

[laurensvalk]

Thanks for the input! Perhaps to achieve a similar effect, there could be some form of hysteresis. Here are two partially overlapping variants/ideas:

• When a new color is detected, don't switch until it has reached, say, 55% of the way there or after a timeout was reached. This may help avoid frequent jumps between adjacent hyperspaces in the map. The 55% may be defined as the cost difference between the last detected color and the new best match.
• Or, keep a count and update the color on some number of consecutive matches. For best performance, the map would need to be tied to the incoming sensor data (I think 100 Hz), but this would not be safe with a user-defined cost function.

But since this PR changes (almost) nothing for the user, we could always start by getting this merged and building on the more advanced features later.

Since you're positive about the suggestions, I'll update this PR with those changes so we can review and test it in more detail.

[laurensvalk]

When the pull request finishes building in half an hour or so, you can find the ready-made firmware files here: #104 (comment)

[Novakasa]

OK, I just did some testing using this build. I was using variations of this script:

from pybricks.pupdevices import ColorDistanceSensor
from pybricks.parameters import Color, Port, Button
from pybricks.hubs import ThisHub

sensor = ColorDistanceSensor(Port.B)

last_color = None
counter = 0

hub = ThisHub()

LBG = Color(h=217, s=46, v=60)
DBG = Color(h=207, s=48, v=39)

Color.DBG = DBG
Color.LBG = LBG

sensor.detectable_colors([Color.RED, Color.GREEN, Color.BLACK, Color.BLUE, Color.YELLOW, Color.DBG, Color.WHITE, Color.LBG])

hub.system.set_stop_button(None)

while True:
    color = sensor.color()
    counter += 1
    if color != last_color:
        print(counter, color)
        last_color = color
        counter = 0
    if hub.button.pressed():
        print(sensor.hsv())

And everything seems to work as expected. I had to calibrate DBG and LBG carefully (measure at the same distance), because they mostly differ in the brightness, and the brightness is very sensitive to distance. (seems to be inverse square law, see also pybricks/support#116 (comment))

(uncalibrated) Green seems to be detected reliably, and None still works when pointing into the air. So, to me, this looks good!

[laurensvalk]

Thanks for the quick test! Glad to hear the basics are working for you.

In the tutorials, when people change the detectable colors, I think we’ll want to recommend that they measure all their colors instead of combining a few measurements with some of the builtin colors, for optimal performance.

With the updated map, it was able to distinguish between the dbg sleepers, my desk underneath, a red tile, and a blue tile, which is pretty neat. Thanks for this, and sorry it took so long!

[Novakasa]

Btw, I was testing with city hub + ColorDistanceSensor

[laurensvalk]

Added a commit to move some of the math, and clean up some of the steps.

The only algorithmic change is that I clamped the lower v bound at 0 for radius computations. It probably makes sense for the negative V to only affect the delta z. (Ran out of time for testing today.)

[laurensvalk]

It would be nice to also do a 1:1 test with the default settings on this firmware and master on a different hub side by side.

If we can get ballpark the same performance, that would be nice. We can adjust the -30 as needed to get a similar threshold as before.

[laurensvalk]

It's merged! 🎉

[Novakasa]

Thank you for seeing this through to the end!

[laurensvalk]

And here is what it looks like in blocks. It gets very easy to use this way 😄