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