Point Cloud Library (PCL)  1.14.1-dev
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 
48 namespace pcl {
49 namespace tracking {
50 // utility
51 union RGBValue {
52  struct /*anonymous*/
53  {
54  unsigned char Blue; // Blue channel
55  unsigned char Green; // Green channel
56  unsigned char Red; // Red channel
57  };
58  float float_value;
59  int int_value;
60 };
61 
62 /** \brief Convert a RGB tuple to an HSV one.
63  * \param[in] r the input Red component
64  * \param[in] g the input Green component
65  * \param[in] b the input Blue component
66  * \param[out] fh the output Hue component
67  * \param[out] fs the output Saturation component
68  * \param[out] fv the output Value component
69  */
70 void
71 RGB2HSV(int r, int g, int b, float& fh, float& fs, float& fv)
72 {
73  // mostly copied from opencv-svn/modules/imgproc/src/color.cpp
74  // revision is 4351
75  const int hsv_shift = 12;
76 
77  static const int div_table[] = {
78  0, 1044480, 522240, 348160, 261120, 208896, 174080, 149211, 130560, 116053,
79  104448, 94953, 87040, 80345, 74606, 69632, 65280, 61440, 58027, 54973,
80  52224, 49737, 47476, 45412, 43520, 41779, 40172, 38684, 37303, 36017,
81  34816, 33693, 32640, 31651, 30720, 29842, 29013, 28229, 27486, 26782,
82  26112, 25475, 24869, 24290, 23738, 23211, 22706, 22223, 21760, 21316,
83  20890, 20480, 20086, 19707, 19342, 18991, 18651, 18324, 18008, 17703,
84  17408, 17123, 16846, 16579, 16320, 16069, 15825, 15589, 15360, 15137,
85  14921, 14711, 14507, 14308, 14115, 13926, 13743, 13565, 13391, 13221,
86  13056, 12895, 12738, 12584, 12434, 12288, 12145, 12006, 11869, 11736,
87  11605, 11478, 11353, 11231, 11111, 10995, 10880, 10768, 10658, 10550,
88  10445, 10341, 10240, 10141, 10043, 9947, 9854, 9761, 9671, 9582,
89  9495, 9410, 9326, 9243, 9162, 9082, 9004, 8927, 8852, 8777,
90  8704, 8632, 8561, 8492, 8423, 8356, 8290, 8224, 8160, 8097,
91  8034, 7973, 7913, 7853, 7795, 7737, 7680, 7624, 7569, 7514,
92  7461, 7408, 7355, 7304, 7253, 7203, 7154, 7105, 7057, 7010,
93  6963, 6917, 6872, 6827, 6782, 6739, 6695, 6653, 6611, 6569,
94  6528, 6487, 6447, 6408, 6369, 6330, 6292, 6254, 6217, 6180,
95  6144, 6108, 6073, 6037, 6003, 5968, 5935, 5901, 5868, 5835,
96  5803, 5771, 5739, 5708, 5677, 5646, 5615, 5585, 5556, 5526,
97  5497, 5468, 5440, 5412, 5384, 5356, 5329, 5302, 5275, 5249,
98  5222, 5196, 5171, 5145, 5120, 5095, 5070, 5046, 5022, 4998,
99  4974, 4950, 4927, 4904, 4881, 4858, 4836, 4813, 4791, 4769,
100  4748, 4726, 4705, 4684, 4663, 4642, 4622, 4601, 4581, 4561,
101  4541, 4522, 4502, 4483, 4464, 4445, 4426, 4407, 4389, 4370,
102  4352, 4334, 4316, 4298, 4281, 4263, 4246, 4229, 4212, 4195,
103  4178, 4161, 4145, 4128, 4112, 4096};
104  int hr = 180, hscale = 15;
105  int h, s, v = b;
106  int vmin = b, diff;
107  int vr, vg;
108 
109  v = std::max<int>(v, g);
110  v = std::max<int>(v, r);
111  vmin = std::min<int>(vmin, g);
112  vmin = std::min<int>(vmin, r);
113 
114  diff = v - vmin;
115  vr = v == r ? -1 : 0;
116  vg = v == g ? -1 : 0;
117 
118  s = diff * div_table[v] >> hsv_shift;
119  h = (vr & (g - b)) +
120  (~vr & ((vg & (b - r + 2 * diff)) + ((~vg) & (r - g + 4 * diff))));
121  h = (h * div_table[diff] * hscale + (1 << (hsv_shift + 6))) >> (7 + hsv_shift);
122 
123  h += h < 0 ? hr : 0;
124  fh = static_cast<float>(h) / 180.0f;
125  fs = static_cast<float>(s) / 255.0f;
126  fv = static_cast<float>(v) / 255.0f;
127 }
128 
129 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
130 template <typename PointInT>
131 double
132 HSVColorCoherence<PointInT>::computeCoherence(PointInT& source, PointInT& target)
133 {
134  // convert color space from RGB to HSV
135  RGBValue source_rgb, target_rgb;
136  source_rgb.int_value = source.rgba;
137  target_rgb.int_value = target.rgba;
138 
139  float source_h, source_s, source_v, target_h, target_s, target_v;
140  RGB2HSV(
141  source_rgb.Red, source_rgb.Blue, source_rgb.Green, source_h, source_s, source_v);
142  RGB2HSV(
143  target_rgb.Red, target_rgb.Blue, target_rgb.Green, target_h, target_s, target_v);
144  // hue value is in 0 ~ 2pi, but circulated.
145  const float _h_diff = std::abs(source_h - target_h);
146  // Also need to compute distance other way around circle - but need to check which is
147  // closer to 0
148  float _h_diff2;
149  if (source_h < target_h)
150  _h_diff2 =
151  std::abs(1.0f + source_h - target_h); // Add 2pi to source, subtract target
152  else
153  _h_diff2 =
154  std::abs(1.0f + target_h - source_h); // Add 2pi to target, subtract source
155 
156  float h_diff;
157  // Now we need to choose the smaller distance
158  if (_h_diff < _h_diff2)
159  h_diff = static_cast<float>(h_weight_) * _h_diff * _h_diff;
160  else
161  h_diff = static_cast<float>(h_weight_) * _h_diff2 * _h_diff2;
162 
163  const float s_diff =
164  static_cast<float>(s_weight_) * (source_s - target_s) * (source_s - target_s);
165  const float v_diff =
166  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 } // namespace tracking
172 } // namespace pcl
173 
174 #define PCL_INSTANTIATE_HSVColorCoherence(T) \
175  template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
176 
177 #endif
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.