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 if (source_h < target_h)
162 _h_diff2 = fabsf (1.0f + source_h - target_h);
164 _h_diff2 = fabsf (1.0f + target_h - source_h);
168 if (_h_diff < _h_diff2)
169 h_diff = static_cast<float> (h_weight_) * _h_diff * _h_diff;
171 h_diff = static_cast<float> (h_weight_) * _h_diff2 * _h_diff2;
173 const float s_diff = static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s);
174 const float v_diff = static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v);
175 const float diff2 = h_diff + s_diff + v_diff;
177 return (1.0 / (1.0 + weight_ * diff2));
182 #define PCL_INSTANTIATE_HSVColorCoherence(T) template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
double computeCoherence(PointInT &source, PointInT &target)
return the color coherence between the two points.
void RGB2HSV(int r, int g, int b, float &fh, float &fs, float &fv)
Convert a RGB tuple to an HSV one.