Point Cloud Library (PCL)  1.10.1
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  union RGBValue
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  };
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 = std::abs (source_h - target_h);
159  // Also need to compute distance other way around circle - but need to check which is closer to 0
160  float _h_diff2;
161  if (source_h < target_h)
162  _h_diff2 = std::abs (1.0f + source_h - target_h); //Add 2pi to source, subtract target
163  else
164  _h_diff2 = std::abs (1.0f + target_h - source_h); //Add 2pi to target, subtract source
165 
166  float h_diff;
167  //Now we need to choose the smaller distance
168  if (_h_diff < _h_diff2)
169  h_diff = static_cast<float> (h_weight_) * _h_diff * _h_diff;
170  else
171  h_diff = static_cast<float> (h_weight_) * _h_diff2 * _h_diff2;
172 
173  const float s_diff = static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s);
174  const float v_diff = static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v);
175  const float diff2 = h_diff + s_diff + v_diff;
176 
177  return (1.0 / (1.0 + weight_ * diff2));
178  }
179  }
180 }
181 
182 #define PCL_INSTANTIATE_HSVColorCoherence(T) template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
183 
184 #endif
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::tracking::HSVColorCoherence::computeCoherence
double computeCoherence(PointInT &source, PointInT &target) override
return the color coherence between the two points.
Definition: hsv_color_coherence.hpp:145
pcl::tracking::RGBValue::Red
unsigned char Red
Definition: hsv_color_coherence.hpp:60
pcl::tracking::RGB2HSV
void RGB2HSV(int r, int g, int b, float &fh, float &fs, float &fv)
Convert a RGB tuple to an HSV one.
Definition: hsv_color_coherence.hpp:75
pcl::tracking::RGBValue::float_value
float float_value
Definition: hsv_color_coherence.hpp:62
pcl::tracking::RGBValue::int_value
int int_value
Definition: hsv_color_coherence.hpp:63
pcl::tracking::RGBValue::Green
unsigned char Green
Definition: hsv_color_coherence.hpp:59
pcl::tracking::RGBValue
Definition: hsv_color_coherence.hpp:54
pcl::tracking::RGBValue::Blue
unsigned char Blue
Definition: hsv_color_coherence.hpp:58