Point Cloud Library (PCL)  1.7.0
hsv_color_coherence.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 #ifndef PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
40 #define PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
41 
42 #if defined __GNUC__
43 # pragma GCC system_header
44 #endif
45 
46 #include <pcl/tracking/hsv_color_coherence.h>
47 #include <Eigen/Dense>
48 
49 namespace pcl
50 {
51  namespace tracking
52  {
53  // utility
54  typedef union
55  {
56  struct /*anonymous*/
57  {
58  unsigned char Blue; // Blue channel
59  unsigned char Green; // Green channel
60  unsigned char Red; // Red channel
61  };
62  float float_value;
63  int int_value;
64  } RGBValue;
65 
66  /** \brief Convert a RGB tuple to an HSV one.
67  * \param[in] r the input Red component
68  * \param[in] g the input Green component
69  * \param[in] b the input Blue component
70  * \param[out] fh the output Hue component
71  * \param[out] fs the output Saturation component
72  * \param[out] fv the output Value component
73  */
74  void
75  RGB2HSV (int r, int g, int b, float& fh, float& fs, float& fv)
76  {
77  // mostly copied from opencv-svn/modules/imgproc/src/color.cpp
78  // revision is 4351
79  const int hsv_shift = 12;
80 
81  static const int div_table[] =
82  {
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
115  };
116  int hr = 180, hscale = 15;
117  int h, s, v = b;
118  int vmin = b, diff;
119  int vr, vg;
120 
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);
125 
126  diff = v - vmin;
127  vr = v == r ? -1 : 0;
128  vg = v == g ? -1 : 0;
129 
130  s = diff * div_table[v] >> hsv_shift;
131  h = (vr & (g - b)) +
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);
136 
137  h += h < 0 ? hr : 0;
138  fh = static_cast<float> (h) / 180.0f;
139  fs = static_cast<float> (s) / 255.0f;
140  fv = static_cast<float> (v) / 255.0f;
141  }
142 
143  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
144  template <typename PointInT> double
145  HSVColorCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
146  {
147  // convert color space from RGB to HSV
148  RGBValue source_rgb, target_rgb;
149  source_rgb.int_value = source.rgba;
150  target_rgb.int_value = target.rgba;
151 
152  float source_h, source_s, source_v, target_h, target_s, target_v;
153  RGB2HSV (source_rgb.Red, source_rgb.Blue, source_rgb.Green,
154  source_h, source_s, source_v);
155  RGB2HSV (target_rgb.Red, target_rgb.Blue, target_rgb.Green,
156  target_h, target_s, target_v);
157  // hue value is in 0 ~ 2pi, but circulated.
158  const float _h_diff = fabsf (source_h - target_h);
159  float h_diff;
160  if (_h_diff > 0.5f)
161  h_diff = static_cast<float> (h_weight_) * (_h_diff - 0.5f) * (_h_diff - 0.5f);
162  else
163  h_diff = static_cast<float> (h_weight_) * _h_diff * _h_diff;
164 
165  const float s_diff = static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s);
166  const float v_diff = static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v);
167  const float diff2 = h_diff + s_diff + v_diff;
168 
169  return (1.0 / (1.0 + weight_ * diff2));
170  }
171  }
172 }
173 
174 #define PCL_INSTANTIATE_HSVColorCoherence(T) template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
175 
176 #endif
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.