39 #ifndef PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
40 #define PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
43 # pragma GCC system_header
46 #include <pcl/tracking/hsv_color_coherence.h>
47 #include <Eigen/Dense>
75 RGB2HSV (
int r,
int g,
int b,
float& fh,
float& fs,
float& fv)
79 const int hsv_shift = 12;
81 static const int div_table[] =
83 0, 1044480, 522240, 348160, 261120, 208896, 174080, 149211,
84 130560, 116053, 104448, 94953, 87040, 80345, 74606, 69632,
85 65280, 61440, 58027, 54973, 52224, 49737, 47476, 45412,
86 43520, 41779, 40172, 38684, 37303, 36017, 34816, 33693,
87 32640, 31651, 30720, 29842, 29013, 28229, 27486, 26782,
88 26112, 25475, 24869, 24290, 23738, 23211, 22706, 22223,
89 21760, 21316, 20890, 20480, 20086, 19707, 19342, 18991,
90 18651, 18324, 18008, 17703, 17408, 17123, 16846, 16579,
91 16320, 16069, 15825, 15589, 15360, 15137, 14921, 14711,
92 14507, 14308, 14115, 13926, 13743, 13565, 13391, 13221,
93 13056, 12895, 12738, 12584, 12434, 12288, 12145, 12006,
94 11869, 11736, 11605, 11478, 11353, 11231, 11111, 10995,
95 10880, 10768, 10658, 10550, 10445, 10341, 10240, 10141,
96 10043, 9947, 9854, 9761, 9671, 9582, 9495, 9410,
97 9326, 9243, 9162, 9082, 9004, 8927, 8852, 8777,
98 8704, 8632, 8561, 8492, 8423, 8356, 8290, 8224,
99 8160, 8097, 8034, 7973, 7913, 7853, 7795, 7737,
100 7680, 7624, 7569, 7514, 7461, 7408, 7355, 7304,
101 7253, 7203, 7154, 7105, 7057, 7010, 6963, 6917,
102 6872, 6827, 6782, 6739, 6695, 6653, 6611, 6569,
103 6528, 6487, 6447, 6408, 6369, 6330, 6292, 6254,
104 6217, 6180, 6144, 6108, 6073, 6037, 6003, 5968,
105 5935, 5901, 5868, 5835, 5803, 5771, 5739, 5708,
106 5677, 5646, 5615, 5585, 5556, 5526, 5497, 5468,
107 5440, 5412, 5384, 5356, 5329, 5302, 5275, 5249,
108 5222, 5196, 5171, 5145, 5120, 5095, 5070, 5046,
109 5022, 4998, 4974, 4950, 4927, 4904, 4881, 4858,
110 4836, 4813, 4791, 4769, 4748, 4726, 4705, 4684,
111 4663, 4642, 4622, 4601, 4581, 4561, 4541, 4522,
112 4502, 4483, 4464, 4445, 4426, 4407, 4389, 4370,
113 4352, 4334, 4316, 4298, 4281, 4263, 4246, 4229,
114 4212, 4195, 4178, 4161, 4145, 4128, 4112, 4096
116 int hr = 180, hscale = 15;
121 v = std::max<int> (v, g);
122 v = std::max<int> (v, r);
123 vmin = std::min<int> (vmin, g);
124 vmin = std::min<int> (vmin, r);
127 vr = v == r ? -1 : 0;
128 vg = v == g ? -1 : 0;
130 s = diff * div_table[v] >> hsv_shift;
132 (~vr & ((vg & (b - r + 2 * diff))
133 + ((~vg) & (r - g + 4 * diff))));
134 h = (h * div_table[diff] * hscale +
135 (1 << (hsv_shift + 6))) >> (7 + hsv_shift);
138 fh =
static_cast<float> (h) / 180.0f;
139 fs =
static_cast<float> (s) / 255.0f;
140 fv =
static_cast<float> (v) / 255.0f;
144 template <
typename Po
intInT>
double
152 float source_h, source_s, source_v, target_h, target_s, target_v;
154 source_h, source_s, source_v);
156 target_h, target_s, target_v);
158 const float _h_diff = fabsf (source_h - target_h);
161 h_diff =
static_cast<float> (h_weight_) * (_h_diff - 0.5f) * (_h_diff - 0.5f);
163 h_diff =
static_cast<float> (h_weight_) * _h_diff * _h_diff;
165 const float s_diff =
static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s);
166 const float v_diff =
static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v);
167 const float diff2 = h_diff + s_diff + v_diff;
169 return (1.0 / (1.0 + weight_ * diff2));
174 #define PCL_INSTANTIATE_HSVColorCoherence(T) template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
void RGB2HSV(int r, int g, int b, float &fh, float &fs, float &fv)
Convert a RGB tuple to an HSV one.
double computeCoherence(PointInT &source, PointInT &target)
return the color coherence between the two points.