00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
00040 #define PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
00041
00042 #if defined __GNUC__
00043 # pragma GCC system_header
00044 #endif
00045
00046 #include <pcl/tracking/hsv_color_coherence.h>
00047 #include <Eigen/Dense>
00048
00049 namespace pcl
00050 {
00051 namespace tracking
00052 {
00053
00054 typedef union
00055 {
00056 struct
00057 {
00058 unsigned char Blue;
00059 unsigned char Green;
00060 unsigned char Red;
00061 };
00062 float float_value;
00063 int int_value;
00064 } RGBValue;
00065
00074 void
00075 RGB2HSV (int r, int g, int b, float& fh, float& fs, float& fv)
00076 {
00077
00078
00079 const int hsv_shift = 12;
00080
00081 static const int div_table[] =
00082 {
00083 0, 1044480, 522240, 348160, 261120, 208896, 174080, 149211,
00084 130560, 116053, 104448, 94953, 87040, 80345, 74606, 69632,
00085 65280, 61440, 58027, 54973, 52224, 49737, 47476, 45412,
00086 43520, 41779, 40172, 38684, 37303, 36017, 34816, 33693,
00087 32640, 31651, 30720, 29842, 29013, 28229, 27486, 26782,
00088 26112, 25475, 24869, 24290, 23738, 23211, 22706, 22223,
00089 21760, 21316, 20890, 20480, 20086, 19707, 19342, 18991,
00090 18651, 18324, 18008, 17703, 17408, 17123, 16846, 16579,
00091 16320, 16069, 15825, 15589, 15360, 15137, 14921, 14711,
00092 14507, 14308, 14115, 13926, 13743, 13565, 13391, 13221,
00093 13056, 12895, 12738, 12584, 12434, 12288, 12145, 12006,
00094 11869, 11736, 11605, 11478, 11353, 11231, 11111, 10995,
00095 10880, 10768, 10658, 10550, 10445, 10341, 10240, 10141,
00096 10043, 9947, 9854, 9761, 9671, 9582, 9495, 9410,
00097 9326, 9243, 9162, 9082, 9004, 8927, 8852, 8777,
00098 8704, 8632, 8561, 8492, 8423, 8356, 8290, 8224,
00099 8160, 8097, 8034, 7973, 7913, 7853, 7795, 7737,
00100 7680, 7624, 7569, 7514, 7461, 7408, 7355, 7304,
00101 7253, 7203, 7154, 7105, 7057, 7010, 6963, 6917,
00102 6872, 6827, 6782, 6739, 6695, 6653, 6611, 6569,
00103 6528, 6487, 6447, 6408, 6369, 6330, 6292, 6254,
00104 6217, 6180, 6144, 6108, 6073, 6037, 6003, 5968,
00105 5935, 5901, 5868, 5835, 5803, 5771, 5739, 5708,
00106 5677, 5646, 5615, 5585, 5556, 5526, 5497, 5468,
00107 5440, 5412, 5384, 5356, 5329, 5302, 5275, 5249,
00108 5222, 5196, 5171, 5145, 5120, 5095, 5070, 5046,
00109 5022, 4998, 4974, 4950, 4927, 4904, 4881, 4858,
00110 4836, 4813, 4791, 4769, 4748, 4726, 4705, 4684,
00111 4663, 4642, 4622, 4601, 4581, 4561, 4541, 4522,
00112 4502, 4483, 4464, 4445, 4426, 4407, 4389, 4370,
00113 4352, 4334, 4316, 4298, 4281, 4263, 4246, 4229,
00114 4212, 4195, 4178, 4161, 4145, 4128, 4112, 4096
00115 };
00116 int hr = 180, hscale = 15;
00117 int h, s, v = b;
00118 int vmin = b, diff;
00119 int vr, vg;
00120
00121 v = std::max<int> (v, g);
00122 v = std::max<int> (v, r);
00123 vmin = std::min<int> (vmin, g);
00124 vmin = std::min<int> (vmin, r);
00125
00126 diff = v - vmin;
00127 vr = v == r ? -1 : 0;
00128 vg = v == g ? -1 : 0;
00129
00130 s = diff * div_table[v] >> hsv_shift;
00131 h = (vr & (g - b)) +
00132 (~vr & ((vg & (b - r + 2 * diff))
00133 + ((~vg) & (r - g + 4 * diff))));
00134 h = (h * div_table[diff] * hscale +
00135 (1 << (hsv_shift + 6))) >> (7 + hsv_shift);
00136
00137 h += h < 0 ? hr : 0;
00138 fh = static_cast<float> (h) / 180.0f;
00139 fs = static_cast<float> (s) / 255.0f;
00140 fv = static_cast<float> (v) / 255.0f;
00141 }
00142
00144 template <typename PointInT> double
00145 HSVColorCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
00146 {
00147
00148 RGBValue source_rgb, target_rgb;
00149 source_rgb.int_value = source.rgba;
00150 target_rgb.int_value = target.rgba;
00151
00152 float source_h, source_s, source_v, target_h, target_s, target_v;
00153 RGB2HSV (source_rgb.Red, source_rgb.Blue, source_rgb.Green,
00154 source_h, source_s, source_v);
00155 RGB2HSV (target_rgb.Red, target_rgb.Blue, target_rgb.Green,
00156 target_h, target_s, target_v);
00157
00158 const float _h_diff = fabsf (source_h - target_h);
00159 float h_diff;
00160 if (_h_diff > 0.5f)
00161 h_diff = static_cast<float> (h_weight_) * (_h_diff - 0.5f) * (_h_diff - 0.5f);
00162 else
00163 h_diff = static_cast<float> (h_weight_) * _h_diff * _h_diff;
00164
00165 const float s_diff = static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s);
00166 const float v_diff = static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v);
00167 const float diff2 = h_diff + s_diff + v_diff;
00168
00169 return (1.0 / (1.0 + weight_ * diff2));
00170 }
00171 }
00172 }
00173
00174 #define PCL_INSTANTIATE_HSVColorCoherence(T) template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
00175
00176 #endif