Loading...
Searching...
No Matches
hsv_color_coherence.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
#define PCL_TRACKING_IMPL_HSV_COLOR_COHERENCE_H_
#if defined __GNUC__
#pragma GCC system_header
#endif
#include <pcl/tracking/hsv_color_coherence.h>
namespace pcl {
namespace tracking {
// utility
union RGBValue {
struct /*anonymous*/
{
unsigned char Blue; // Blue channel
unsigned char Green; // Green channel
unsigned char Red; // Red channel
};
float float_value;
int int_value;
};
/** \brief Convert a RGB tuple to an HSV one.
* \param[in] r the input Red component
* \param[in] g the input Green component
* \param[in] b the input Blue component
* \param[out] fh the output Hue component
* \param[out] fs the output Saturation component
* \param[out] fv the output Value component
*/
void
RGB2HSV(int r, int g, int b, float& fh, float& fs, float& fv)
{
// mostly copied from opencv-svn/modules/imgproc/src/color.cpp
// revision is 4351
const int hsv_shift = 12;
static const int div_table[] = {
0, 1044480, 522240, 348160, 261120, 208896, 174080, 149211, 130560, 116053,
104448, 94953, 87040, 80345, 74606, 69632, 65280, 61440, 58027, 54973,
52224, 49737, 47476, 45412, 43520, 41779, 40172, 38684, 37303, 36017,
34816, 33693, 32640, 31651, 30720, 29842, 29013, 28229, 27486, 26782,
26112, 25475, 24869, 24290, 23738, 23211, 22706, 22223, 21760, 21316,
20890, 20480, 20086, 19707, 19342, 18991, 18651, 18324, 18008, 17703,
17408, 17123, 16846, 16579, 16320, 16069, 15825, 15589, 15360, 15137,
14921, 14711, 14507, 14308, 14115, 13926, 13743, 13565, 13391, 13221,
13056, 12895, 12738, 12584, 12434, 12288, 12145, 12006, 11869, 11736,
11605, 11478, 11353, 11231, 11111, 10995, 10880, 10768, 10658, 10550,
10445, 10341, 10240, 10141, 10043, 9947, 9854, 9761, 9671, 9582,
9495, 9410, 9326, 9243, 9162, 9082, 9004, 8927, 8852, 8777,
8704, 8632, 8561, 8492, 8423, 8356, 8290, 8224, 8160, 8097,
8034, 7973, 7913, 7853, 7795, 7737, 7680, 7624, 7569, 7514,
7461, 7408, 7355, 7304, 7253, 7203, 7154, 7105, 7057, 7010,
6963, 6917, 6872, 6827, 6782, 6739, 6695, 6653, 6611, 6569,
6528, 6487, 6447, 6408, 6369, 6330, 6292, 6254, 6217, 6180,
6144, 6108, 6073, 6037, 6003, 5968, 5935, 5901, 5868, 5835,
5803, 5771, 5739, 5708, 5677, 5646, 5615, 5585, 5556, 5526,
5497, 5468, 5440, 5412, 5384, 5356, 5329, 5302, 5275, 5249,
5222, 5196, 5171, 5145, 5120, 5095, 5070, 5046, 5022, 4998,
4974, 4950, 4927, 4904, 4881, 4858, 4836, 4813, 4791, 4769,
4748, 4726, 4705, 4684, 4663, 4642, 4622, 4601, 4581, 4561,
4541, 4522, 4502, 4483, 4464, 4445, 4426, 4407, 4389, 4370,
4352, 4334, 4316, 4298, 4281, 4263, 4246, 4229, 4212, 4195,
4178, 4161, 4145, 4128, 4112, 4096};
int hr = 180, hscale = 15;
int h, s, v = b;
int vmin = b, diff;
int vr, vg;
v = std::max<int>(v, g);
v = std::max<int>(v, r);
vmin = std::min<int>(vmin, g);
vmin = std::min<int>(vmin, r);
diff = v - vmin;
vr = v == r ? -1 : 0;
vg = v == g ? -1 : 0;
s = diff * div_table[v] >> hsv_shift;
h = (vr & (g - b)) +
(~vr & ((vg & (b - r + 2 * diff)) + ((~vg) & (r - g + 4 * diff))));
h = (h * div_table[diff] * hscale + (1 << (hsv_shift + 6))) >> (7 + hsv_shift);
h += h < 0 ? hr : 0;
fh = static_cast<float>(h) / 180.0f;
fs = static_cast<float>(s) / 255.0f;
fv = static_cast<float>(v) / 255.0f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT>
double
HSVColorCoherence<PointInT>::computeCoherence(PointInT& source, PointInT& target)
{
// convert color space from RGB to HSV
RGBValue source_rgb, target_rgb;
source_rgb.int_value = source.rgba;
target_rgb.int_value = target.rgba;
float source_h, source_s, source_v, target_h, target_s, target_v;
RGB2HSV(
source_rgb.Red, source_rgb.Blue, source_rgb.Green, source_h, source_s, source_v);
RGB2HSV(
target_rgb.Red, target_rgb.Blue, target_rgb.Green, target_h, target_s, target_v);
// hue value is in 0 ~ 2pi, but circulated.
const float _h_diff = std::abs(source_h - target_h);
// Also need to compute distance other way around circle - but need to check which is
// closer to 0
float _h_diff2;
if (source_h < target_h)
_h_diff2 =
std::abs(1.0f + source_h - target_h); // Add 2pi to source, subtract target
else
_h_diff2 =
std::abs(1.0f + target_h - source_h); // Add 2pi to target, subtract source
float h_diff;
// Now we need to choose the smaller distance
if (_h_diff < _h_diff2)
h_diff = static_cast<float>(h_weight_) * _h_diff * _h_diff;
else
h_diff = static_cast<float>(h_weight_) * _h_diff2 * _h_diff2;
const float s_diff =
static_cast<float>(s_weight_) * (source_s - target_s) * (source_s - target_s);
const float v_diff =
static_cast<float>(v_weight_) * (source_v - target_v) * (source_v - target_v);
const float diff2 = h_diff + s_diff + v_diff;
return (1.0 / (1.0 + weight_ * diff2));
}
} // namespace tracking
} // namespace pcl
#define PCL_INSTANTIATE_HSVColorCoherence(T) \
template class PCL_EXPORTS pcl::tracking::HSVColorCoherence<T>;
#endif
pcl::tracking::HSVColorCoherence::computeCoherence
double computeCoherence(PointInT &source, PointInT &target) override
return the color coherence between the two points.
Definition hsv_color_coherence.hpp:132
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:71
pcl
Definition convolution.h:46
pcl::tracking::RGBValue
Definition hsv_color_coherence.hpp:51
pcl::tracking::RGBValue::int_value
int int_value
Definition hsv_color_coherence.hpp:59
pcl::tracking::RGBValue::Green
unsigned char Green
Definition hsv_color_coherence.hpp:55
pcl::tracking::RGBValue::float_value
float float_value
Definition hsv_color_coherence.hpp:58
pcl::tracking::RGBValue::Red
unsigned char Red
Definition hsv_color_coherence.hpp:56
pcl::tracking::RGBValue::Blue
unsigned char Blue
Definition hsv_color_coherence.hpp:54