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