From cbd04604b577a7b2ccce5f476de1510e5484ecfd Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Tue, 1 May 2018 00:24:47 +0200
Subject: [PATCH] refactored HSV color utils
---
.../visualization/DebugDrawerInterface.ice | 10 +-
.../RobotAPI/libraries/core/math/ColorUtils.h | 151 ++++++++++--------
.../libraries/core/test/MathUtilsTest.cpp | 14 ++
3 files changed, 108 insertions(+), 67 deletions(-)
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 629d152d9..1a342947e 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -52,11 +52,15 @@ module armarx
byte b;
};
+
struct HsvColor
{
- byte h;
- byte s;
- byte v;
+ //! 0-360
+ float h;
+ //! 0 - 1
+ float s;
+ //! 0 - 1
+ float v;
};
diff --git a/source/RobotAPI/libraries/core/math/ColorUtils.h b/source/RobotAPI/libraries/core/math/ColorUtils.h
index b28b8c8eb..fb79987b2 100644
--- a/source/RobotAPI/libraries/core/math/ColorUtils.h
+++ b/source/RobotAPI/libraries/core/math/ColorUtils.h
@@ -30,103 +30,126 @@ namespace armarx
namespace colorutils
{
-
-
-
- DrawColor24Bit HsvToRgb(const HsvColor& hsv)
+ DrawColor24Bit HsvToRgb(const HsvColor& in)
{
- DrawColor24Bit rgb;
- unsigned char region, remainder, p, q, t;
-
- if (hsv.s == 0)
+ double hh, p, q, t, ff;
+ long i;
+ double r, g, b;
+ if (in.s <= 0.0) // < is bogus, just shuts up warnings
{
- rgb.r = hsv.v;
- rgb.g = hsv.v;
- rgb.b = hsv.v;
- return rgb;
+ r = in.v;
+ g = in.v;
+ b = in.v;
+ return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
}
- constexpr float by43 = 1.0f / 43.0f;
- region = hsv.h * by43;
- remainder = (hsv.h - (region * 43)) * 6;
-
- p = (hsv.v * (255 - hsv.s)) >> 8;
- q = (hsv.v * (255 - ((hsv.s * remainder) >> 8))) >> 8;
- t = (hsv.v * (255 - ((hsv.s * (255 - remainder)) >> 8))) >> 8;
-
- switch (region)
+ hh = in.h;
+ if (hh >= 360.0)
+ {
+ hh = 0.0;
+ }
+ hh /= 60.0;
+ i = (long)hh;
+ ff = hh - i;
+ p = in.v * (1.0 - in.s);
+ q = in.v * (1.0 - (in.s * ff));
+ t = in.v * (1.0 - (in.s * (1.0 - ff)));
+
+ switch (i)
{
case 0:
- rgb.r = hsv.v;
- rgb.g = t;
- rgb.b = p;
+ r = in.v;
+ g = t;
+ b = p;
break;
case 1:
- rgb.r = q;
- rgb.g = hsv.v;
- rgb.b = p;
+ r = q;
+ g = in.v;
+ b = p;
break;
case 2:
- rgb.r = p;
- rgb.g = hsv.v;
- rgb.b = t;
+ r = p;
+ g = in.v;
+ b = t;
break;
+
case 3:
- rgb.r = p;
- rgb.g = q;
- rgb.b = hsv.v;
+ r = p;
+ g = q;
+ b = in.v;
break;
case 4:
- rgb.r = t;
- rgb.g = p;
- rgb.b = hsv.v;
+ r = t;
+ g = p;
+ b = in.v;
break;
+ case 5:
default:
- rgb.r = hsv.v;
- rgb.g = p;
- rgb.b = q;
+ r = in.v;
+ g = p;
+ b = q;
break;
}
- return rgb;
+ return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
}
- HsvColor RgbToHsv(const DrawColor24Bit& rgb)
+ HsvColor RgbToHsv(const DrawColor24Bit& in)
{
- HsvColor hsv;
- unsigned char rgbMin, rgbMax;
+ double r = in.r * 0.00392156862;
+ double g = in.g * 0.00392156862;
+ double b = in.b * 0.00392156862;
+ HsvColor out;
+ double min, max, delta;
- rgbMin = rgb.r < rgb.g ? (rgb.r < rgb.b ? rgb.r : rgb.b) : (rgb.g < rgb.b ? rgb.g : rgb.b);
- rgbMax = rgb.r > rgb.g ? (rgb.r > rgb.b ? rgb.r : rgb.b) : (rgb.g > rgb.b ? rgb.g : rgb.b);
+ min = r < g ? r : g;
+ min = min < b ? min : b;
- hsv.v = rgbMax;
- if (hsv.v == 0)
+ max = r > g ? r : g;
+ max = max > b ? max : b;
+
+ out.v = max; // v
+ delta = max - min;
+ if (delta < 0.00001)
{
- hsv.h = 0;
- hsv.s = 0;
- return hsv;
+ out.s = 0;
+ out.h = 0; // undefined, maybe nan?
+ return out;
}
-
- hsv.s = 255 * long(rgbMax - rgbMin) / hsv.v;
- if (hsv.s == 0)
+ if (max > 0.0) // NOTE: if Max is == 0, this divide would cause a crash
{
- hsv.h = 0;
- return hsv;
+ out.s = (delta / max); // s
}
-
- if (rgbMax == rgb.r)
+ else
+ {
+ // if max is 0, then r = g = b = 0
+ // s = 0, h is undefined
+ out.s = 0.0;
+ out.h = 0; // its now undefined
+ return out;
+ }
+ if (r >= max) // > is bogus, just keeps compilor happy
{
- hsv.h = 0 + 43 * (rgb.g - rgb.b) / (rgbMax - rgbMin);
+ out.h = (g - b) / delta; // between yellow & magenta
}
- else if (rgbMax == rgb.g)
+ else if (g >= max)
{
- hsv.h = 85 + 43 * (rgb.b - rgb.r) / (rgbMax - rgbMin);
+ out.h = 2.0 + (b - r) / delta; // between cyan & yellow
}
else
{
- hsv.h = 171 + 43 * (rgb.r - rgb.g) / (rgbMax - rgbMin);
+ out.h = 4.0 + (r - g) / delta; // between magenta & cyan
}
- return hsv;
+ out.h *= 60.0; // degrees
+
+ if (out.h < 0.0)
+ {
+ out.h += 360.0;
+ }
+
+ return out;
+
+
}
/**
@@ -137,8 +160,8 @@ namespace armarx
HsvColor HeatMapColor(float percentage)
{
percentage = math::MathUtils::LimitMinMax(0.0f, 1.0f, percentage);
- constexpr float factor = 240.0 * (255.0 / 360.0);
- return HsvColor {(unsigned char)((1.0 - percentage) * factor), 255, 255};
+
+ return HsvColor {((1.0f - percentage) * 240.f), 1.0f, 1.0f};
}
DrawColor24Bit HeatMapRGBColor(float percentage)
diff --git a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
index dd7592f4b..73c932696 100644
--- a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
+++ b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
@@ -25,6 +25,7 @@
#include <RobotAPI/Test.h>
#include <ArmarXCore/core/test/IceTestHelper.h>
#include "../math/MathUtils.h"
+#include "../math/ColorUtils.h"
using namespace armarx;
void check_close(float a, float b, float tolerance)
@@ -60,3 +61,16 @@ BOOST_AUTO_TEST_CASE(fmodTest)
}
}
+
+BOOST_AUTO_TEST_CASE(ColorUtilTest)
+{
+ auto testColor = [](DrawColor24Bit rgb)
+ {
+ auto hsv = colorutils::RgbToHsv(rgb);
+ ARMARX_INFO << VAROUT(rgb.r) << VAROUT(rgb.g) << VAROUT(rgb.b) << " --> " << VAROUT(hsv.h) << VAROUT(hsv.s) << VAROUT(hsv.v);
+ };
+ testColor({255, 0, 0});
+ testColor({0, 255, 0});
+ testColor({0, 0, 255});
+ testColor({0, 20, 0});
+}
--
GitLab