color_histogram.h
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 /*
36  * color_histogram.h
37  * Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
38  */
39 
40 #ifndef JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__
41 #define JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__
42 
43 #include <algorithm>
44 #include <iterator>
45 #include <vector>
46 #include <pcl/point_cloud.h>
47 #include <pcl/point_types.h>
48 
49 namespace jsk_recognition_utils {
51  HUE = 0,
53  };
54 
61  };
62 
63  struct PointXYZHLS {
64  double x, y, z;
65  double h, l, s;
66  };
67 
68  inline void
69  HSV2HLS(const pcl::PointXYZHSV& hsv, PointXYZHLS& hls) {
70  hls.x = hsv.x; hls.y = hsv.y; hls.z = hsv.z;
71  hls.h = hsv.h;
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;
75  hls.l /= 2.0;
76  }
77 
78  inline int
79  getHistogramBin(const double& val, const int& step,
80  const double& min, const double& max)
81  {
82  int idx = std::floor((val - min) / (max - min) * step);
83  if (idx >= step) return step - 1;
84  else if (idx <= 0) return 0;
85  else return idx;
86  }
87 
88  inline void
89  normalizeHistogram(std::vector<float>& histogram)
90  {
91  float sum = 0.0f;
92  for (size_t i = 0; i < histogram.size(); ++i)
93  sum += histogram[i];
94  if (sum != 0.0) {
95  for (size_t i = 0; i < histogram.size(); ++i)
96  histogram[i] /= sum;
97  }
98  }
99 
100  inline void
101  computeColorHistogram1d(const pcl::PointCloud<pcl::PointXYZHSV>& cloud,
102  std::vector<float>& histogram,
103  const int bin_size,
104  const double white_threshold=0.1,
105  const double black_threshold=0.1)
106  {
107  histogram.resize(bin_size + 2, 0.0f); // h + w + b
108  int white_index = bin_size;
109  int black_index = bin_size + 1;
110 
111  for (size_t i = 0; i < cloud.points.size(); ++i) {
112  PointXYZHLS p;
113  HSV2HLS(cloud.points[i], p);
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;
118  else {
119  int h_bin = getHistogramBin(p.h, bin_size, 0.0, 360.0);
120  histogram[h_bin] += 1.0f;
121  }
122  }
123 
124  normalizeHistogram(histogram);
125  }
126 
127  inline void
128  computeColorHistogram2d(const pcl::PointCloud<pcl::PointXYZHSV>& cloud,
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)
133  {
134  histogram.resize(bin_size_per_channel * bin_size_per_channel + 2, 0.0f); // h * s + w + b
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;
137 
138  // vote
139  for (size_t i = 0; i < cloud.points.size(); ++i) {
140  PointXYZHLS p;
141  HSV2HLS(cloud.points[i], p);
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;
146  else {
147  int h_bin = getHistogramBin(p.h, bin_size_per_channel, 0.0, 360.0);
148  int s_bin = getHistogramBin(p.s, bin_size_per_channel, 0.0, 1.0);
149  histogram[s_bin * bin_size_per_channel + h_bin] += 1.0f;
150  }
151  }
152 
153  normalizeHistogram(histogram);
154  }
155 
156  inline void
157  rotateHistogram1d(const std::vector<float>& input,
158  std::vector<float>& output,
159  const double degree)
160  {
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];
167  }
168 
169  // copy white + black
170  int index = bin_size;
171  output[index] = input[index];
172  output[index+1] = input[index+1];
173  }
174 
175  inline void
176  rotateHistogram2d(const std::vector<float>& input,
177  std::vector<float>& output,
178  const double degree)
179  {
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];
187  }
188  }
189 
190  // copy white + black
191  int index = bin_size * bin_size;
192  output[index] = input[index];
193  output[index+1] = input[index+1];
194  }
195 
196  inline bool
197  compareHistogram(const std::vector<float>& input,
198  const std::vector<float>& reference,
199  const ComparePolicy policy,
200  double& distance)
201  {
202  if (input.size() != reference.size()) {
203  ROS_ERROR("Mismatch histogram bin size");
204  return false;
205  }
206 
207  distance = 0.0;
208  size_t len = input.size();
209  if (policy == CHISQUARE) {
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;
213  }
214  } else if (policy == CORRELATION) {
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;
219  s1 += a; s2 += b;
220  }
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;
224  } else if (policy == INTERSECTION) {
225  for (size_t i = 0; i < len; ++i) {
226  distance += std::min(input[i], reference[i]);
227  }
228  } else if (policy == BHATTACHARYYA) {
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];
233  }
234  s1 *= s2;
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));
237  } else if (policy == KL_DIVERGENCE) {
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);
243  }
244  } else {
245  ROS_ERROR("Invalid compare policy");
246  return false;
247  }
248 
249  return true;
250  }
251 } // namespace
252 
253 #endif // JSK_RECOGNITION_UTILS_COLOR_HISTOGRAM_H__
void HSV2HLS(const pcl::PointXYZHSV &hsv, PointXYZHLS &hls)
num
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)
f
s1
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)
s2
q
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)
p
#define ROS_ERROR(...)


jsk_recognition_utils
Author(s):
autogenerated on Mon May 3 2021 03:03:03