Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
48namespace pcl {
49namespace tracking {
50// utility
51union RGBValue {
52 struct /*anonymous*/
53 {
54 unsigned char Blue; // Blue channel
55 unsigned char Green; // Green channel
56 unsigned char Red; // Red channel
57 };
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 */
70void
71RGB2HSV(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///////////////////////////////////////////////////////////////////////////////////////////////////////////////
130template <typename PointInT>
131double
132HSVColorCoherence<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.