40 #ifndef JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__ 41 #define JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__ 46 #include <pcl/point_cloud.h> 47 #include <pcl/point_types.h> 70 hls.
x = hsv.x; hls.
y = hsv.y; hls.
z = hsv.z;
72 hls.
l = (2.0 - hsv.s) * hsv.v;
73 hls.
s = hsv.s * hsv.v;
74 hls.
s /= (hls.
l <= 1.0) ? hls.
l : 2.0 - hls.
l;
80 const double& min,
const double& max)
82 int idx = std::floor((val - min) / (max - min) * step);
83 if (idx >= step)
return step - 1;
84 else if (idx <= 0)
return 0;
92 for (
size_t i = 0; i < histogram.size(); ++i)
95 for (
size_t i = 0; i < histogram.size(); ++i)
102 std::vector<float>& histogram,
104 const double white_threshold=0.1,
105 const double black_threshold=0.1)
107 histogram.resize(bin_size + 2, 0.0
f);
108 int white_index = bin_size;
109 int black_index = bin_size + 1;
111 for (
size_t i = 0; i < cloud.points.size(); ++i) {
114 if (p.
l > 1.0 - white_threshold)
115 histogram[white_index] += 1.0f;
116 else if (p.
l < black_threshold)
117 histogram[black_index] += 1.0f;
120 histogram[h_bin] += 1.0f;
129 std::vector<float>& histogram,
130 const int bin_size_per_channel,
131 const double white_threshold=0.1,
132 const double black_threshold=0.1)
134 histogram.resize(bin_size_per_channel * bin_size_per_channel + 2, 0.0
f);
135 int white_index = bin_size_per_channel * bin_size_per_channel;
136 int black_index = bin_size_per_channel * bin_size_per_channel + 1;
139 for (
size_t i = 0; i < cloud.points.size(); ++i) {
142 if (p.
l < white_threshold)
143 histogram[white_index] += 1.0f;
144 else if (p.
l > 1.0 - black_threshold)
145 histogram[black_index] += 1.0f;
149 histogram[s_bin * bin_size_per_channel + h_bin] += 1.0f;
158 std::vector<float>& output,
161 size_t bin_size = input.size() - 2;
162 int offset = std::floor(degree / 360.0 * bin_size);
163 output.resize(input.size());
164 for (
size_t h = 0;
h < bin_size; ++
h) {
165 int hout =
h + offset % bin_size;
166 output[hout] = input[
h];
170 int index = bin_size;
171 output[index] = input[index];
172 output[index+1] = input[index+1];
177 std::vector<float>& output,
180 size_t bin_size = (size_t)(std::sqrt(input.size() - 2));
181 int offset = std::floor(degree / 360.0 * bin_size);
182 output.resize(input.size());
183 for (
size_t s = 0;
s < bin_size; ++
s) {
184 for (
size_t h = 0;
h < bin_size; ++
h) {
185 int hout =
h + offset % bin_size;
186 output[
s * bin_size + hout] = input[
s * bin_size +
h];
191 int index = bin_size * bin_size;
192 output[index] = input[index];
193 output[index+1] = input[index+1];
198 const std::vector<float>& reference,
202 if (input.size() != reference.size()) {
203 ROS_ERROR(
"Mismatch histogram bin size");
208 size_t len = input.size();
210 for (
size_t i = 0; i < len; ++i) {
211 double a = input[i] - reference[i], b = input[i];
212 if (std::fabs(b) > DBL_EPSILON) distance += a * a / b;
215 double s1 = 0.0,
s2 = 0.0, s11 = 0.0, s12 = 0.0, s22 = 0.0;
216 for (
size_t i = 0; i < len; ++i) {
217 double a = input[i], b = reference[i];
218 s11 += a*a; s12 += a*b; s22 += b*b;
221 double num = s12 - s1*
s2/len;
222 double denom2 = (s11 - s1 * s1 / len) * (s22 -
s2 *
s2 / len);
223 distance = std::fabs(denom2) > DBL_EPSILON ? num / std::sqrt(denom2) : 1.0;
225 for (
size_t i = 0; i < len; ++i) {
226 distance += std::min(input[i], reference[i]);
229 double s1 = 0.0,
s2 = 0.0;
230 for (
size_t i = 0; i < len; ++i) {
231 distance += std::sqrt(input[i] * reference[i]);
232 s1 += input[i];
s2 += reference[i];
235 s1 = std::fabs(s1) > DBL_EPSILON ? 1.0 / std::sqrt(s1) : 1.0;
236 distance = std::sqrt(std::max(1.0 - distance * s1, 0.0));
238 for (
size_t i = 0; i < len; ++i) {
239 double p = input[i],
q = reference[i];
240 if (std::fabs(p) <= DBL_EPSILON)
continue;
241 if (std::fabs(q) <= DBL_EPSILON) q = 1e-10;
242 distance += p * std::log(p / q);
253 #endif // JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__ void HSV2HLS(const pcl::PointXYZHSV &hsv, PointXYZHLS &hls)
void rotateHistogram2d(const std::vector< float > &input, std::vector< float > &output, const double degree)
bool compareHistogram(const std::vector< float > &input, const std::vector< float > &reference, const ComparePolicy policy, double &distance)
void computeColorHistogram1d(const pcl::PointCloud< pcl::PointXYZHSV > &cloud, std::vector< float > &histogram, const int bin_size, const double white_threshold=0.1, const double black_threshold=0.1)
void computeColorHistogram2d(const pcl::PointCloud< pcl::PointXYZHSV > &cloud, std::vector< float > &histogram, const int bin_size_per_channel, const double white_threshold=0.1, const double black_threshold=0.1)
void normalizeHistogram(std::vector< float > &histogram)
int getHistogramBin(const double &val, const int &step, const double &min, const double &max)
void rotateHistogram1d(const std::vector< float > &input, std::vector< float > &output, const double degree)