Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 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 // utility 00054 typedef union 00055 { 00056 struct /*anonymous*/ 00057 { 00058 unsigned char Blue; // Blue channel 00059 unsigned char Green; // Green channel 00060 unsigned char Red; // Red channel 00061 }; 00062 float float_value; 00063 int int_value; 00064 } RGBValue; 00065 00066 /** \brief Convert a RGB tuple to an HSV one. 00067 * \param[in] r the input Red component 00068 * \param[in] g the input Green component 00069 * \param[in] b the input Blue component 00070 * \param[out] fh the output Hue component 00071 * \param[out] fs the output Saturation component 00072 * \param[out] fv the output Value component 00073 */ 00074 void 00075 RGB2HSV (int r, int g, int b, float& fh, float& fs, float& fv) 00076 { 00077 // mostly copied from opencv-svn/modules/imgproc/src/color.cpp 00078 // revision is 4351 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 00143 /////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00144 template <typename PointInT> double 00145 HSVColorCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target) 00146 { 00147 // convert color space from RGB to HSV 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 // hue value is in 0 ~ 2pi, but circulated. 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