hsv_color_coherence.hpp
Go to the documentation of this file.
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 
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    
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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:47