Point Cloud Library (PCL)  1.7.1
person_classifier.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2013-, Open Perception, 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 the copyright holder(s) 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  * person_classifier.hpp
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #include <pcl/people/person_classifier.h>
42 
43 #ifndef PCL_PEOPLE_PERSON_CLASSIFIER_HPP_
44 #define PCL_PEOPLE_PERSON_CLASSIFIER_HPP_
45 
46 template <typename PointT>
48 
49 template <typename PointT>
51 
52 template <typename PointT> bool
54 {
55  std::string line;
56  std::ifstream SVM_file;
57  SVM_file.open(svm_filename.c_str());
58 
59  getline (SVM_file,line); // read window_height line
60  size_t tok_pos = line.find_first_of(":", 0); // search for token ":"
61  window_height_ = std::atoi(line.substr(tok_pos+1, line.npos - tok_pos-1).c_str());
62 
63  getline (SVM_file,line); // read window_width line
64  tok_pos = line.find_first_of(":", 0); // search for token ":"
65  window_width_ = std::atoi(line.substr(tok_pos+1, line.npos - tok_pos-1).c_str());
66 
67  getline (SVM_file,line); // read SVM_offset line
68  tok_pos = line.find_first_of(":", 0); // search for token ":"
69  SVM_offset_ = std::atof(line.substr(tok_pos+1, line.npos - tok_pos-1).c_str());
70 
71  getline (SVM_file,line); // read SVM_weights line
72  tok_pos = line.find_first_of("[", 0); // search for token "["
73  size_t tok_end_pos = line.find_first_of("]", 0); // search for token "]" , end of SVM weights
74  size_t prev_tok_pos;
75  while (tok_pos < tok_end_pos) // while end of SVM_weights is not reached
76  {
77  prev_tok_pos = tok_pos;
78  tok_pos = line.find_first_of(",", prev_tok_pos+1); // search for token ","
79  SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str()));
80  }
81  SVM_file.close();
82 
83  if (SVM_weights_.size() == 0)
84  {
85  PCL_ERROR ("[pcl::people::PersonClassifier::loadSVMFromFile] Invalid SVM file!\n");
86  return (false);
87  }
88  else
89  {
90  return (true);
91  }
92 }
93 
94 template <typename PointT> void
95 pcl::people::PersonClassifier<PointT>::setSVM (int window_height, int window_width, std::vector<float> SVM_weights, float SVM_offset)
96 {
97  window_height_ = window_height;
98  window_width_ = window_width;
99  SVM_weights_ = SVM_weights;
100  SVM_offset_ = SVM_offset;
101 }
102 
103 template <typename PointT> void
104 pcl::people::PersonClassifier<PointT>::getSVM (int& window_height, int& window_width, std::vector<float>& SVM_weights, float& SVM_offset)
105 {
106  window_height = window_height_;
107  window_width = window_width_;
108  SVM_weights = SVM_weights_;
109  SVM_offset = SVM_offset_;
110 }
111 
112 template <typename PointT> void
114  PointCloudPtr& output_image,
115  int width,
116  int height)
117 {
118  PointT new_point;
119  new_point.r = 0;
120  new_point.g = 0;
121  new_point.b = 0;
122 
123  // Allocate the vector of points:
124  output_image->points.resize(width*height, new_point);
125  output_image->height = height;
126  output_image->width = width;
127 
128  // Compute scale factor:
129  float scale1 = float(height) / float(input_image->height);
130  float scale2 = float(width) / float(input_image->width);
131 
132  Eigen::Matrix3f T_inv;
133  T_inv << 1/scale1, 0, 0,
134  0, 1/scale2, 0,
135  0, 0, 1;
136 
137  Eigen::Vector3f A;
138  int c1, c2, f1, f2;
139  PointT g1, g2, g3, g4;
140  float w1, w2;
141  for (unsigned int i = 0; i < height; i++) // for every row
142  {
143  for (unsigned int j = 0; j < width; j++) // for every column
144  {
145  A = T_inv * Eigen::Vector3f(i, j, 1);
146  c1 = ceil(A(0));
147  f1 = floor(A(0));
148  c2 = ceil(A(1));
149  f2 = floor(A(1));
150 
151  if ( (f1 < 0) ||
152  (c1 < 0) ||
153  (f1 >= input_image->height) ||
154  (c1 >= input_image->height) ||
155  (f2 < 0) ||
156  (c2 < 0) ||
157  (f2 >= input_image->width) ||
158  (c2 >= input_image->width))
159  { // if out of range, continue
160  continue;
161  }
162 
163  g1 = (*input_image)(f2, c1);
164  g3 = (*input_image)(f2, f1);
165  g4 = (*input_image)(c2, f1);
166  g2 = (*input_image)(c2, c1);
167 
168  w1 = (A(0) - f1);
169  w2 = (A(1) - f2);
170  new_point.r = int((1 - w1) * ((1 - w2) * g1.r + w2 * g4.r) + w1 * ((1 - w2) * g3.r + w2 * g4.r));
171  new_point.g = int((1 - w1) * ((1 - w2) * g1.g + w2 * g4.g) + w1 * ((1 - w2) * g3.g + w2 * g4.g));
172  new_point.b = int((1 - w1) * ((1 - w2) * g1.b + w2 * g4.b) + w1 * ((1 - w2) * g3.b + w2 * g4.b));
173 
174  // Insert the point in the output image:
175  (*output_image)(j,i) = new_point;
176  }
177  }
178 }
179 
180 template <typename PointT> void
182  PointCloudPtr& output_image,
183  int xmin,
184  int ymin,
185  int width,
186  int height)
187 {
188  PointT black_point;
189  black_point.r = 0;
190  black_point.g = 0;
191  black_point.b = 0;
192  output_image->points.resize(height*width, black_point);
193  output_image->width = width;
194  output_image->height = height;
195 
196  int x_start_in = std::max(0, xmin);
197  int x_end_in = std::min(int(input_image->width-1), xmin+width-1);
198  int y_start_in = std::max(0, ymin);
199  int y_end_in = std::min(int(input_image->height-1), ymin+height-1);
200 
201  int x_start_out = std::max(0, -xmin);
202  //int x_end_out = x_start_out + (x_end_in - x_start_in);
203  int y_start_out = std::max(0, -ymin);
204  //int y_end_out = y_start_out + (y_end_in - y_start_in);
205 
206  for (unsigned int i = 0; i < (y_end_in - y_start_in + 1); i++)
207  {
208  for (unsigned int j = 0; j < (x_end_in - x_start_in + 1); j++)
209  {
210  (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i);
211  }
212  }
213 }
214 
215 template <typename PointT> double
217  float xc,
218  float yc,
219  PointCloudPtr& image)
220 {
221  if (SVM_weights_.size() == 0)
222  {
223  PCL_ERROR ("[pcl::people::PersonClassifier::evaluate] SVM has not been set!\n");
224  return (-1000);
225  }
226 
227  int height = floor((height_person * window_height_) / (0.75 * window_height_) + 0.5); // floor(i+0.5) = round(i)
228  int width = floor((height_person * window_width_) / (0.75 * window_height_) + 0.5);
229  int xmin = floor(xc - width / 2 + 0.5);
230  int ymin = floor(yc - height / 2 + 0.5);
231  double confidence;
232 
233  if (height > 0)
234  {
235  // If near the border, fill with black:
236  PointCloudPtr box(new PointCloud);
237  copyMakeBorder(image, box, xmin, ymin, width, height);
238 
239  // Make the image match the correct size (used in the training stage):
240  PointCloudPtr sample(new PointCloud);
241  resize(box, sample, window_width_, window_height_);
242 
243  // Convert the image to array of float:
244  float* sample_float = new float[sample->width * sample->height * 3];
245  int delta = sample->height * sample->width;
246  for(int row = 0; row < sample->height; row++)
247  {
248  for(int col = 0; col < sample->width; col++)
249  {
250  sample_float[row + sample->height * col] = ((float) ((*sample)(col, row).r))/255; //ptr[col * 3 + 2];
251  sample_float[row + sample->height * col + delta] = ((float) ((*sample)(col, row).g))/255; //ptr[col * 3 + 1];
252  sample_float[row + sample->height * col + delta * 2] = (float) (((*sample)(col, row).b))/255; //ptr[col * 3];
253  }
254  }
255 
256  // Calculate HOG descriptor:
257  pcl::people::HOG hog;
258  float *descriptor = (float*) calloc(SVM_weights_.size(), sizeof(float));
259  hog.compute(sample_float, descriptor);
260 
261  // Calculate confidence value by dot product:
262  confidence = 0.0;
263  for(unsigned int i = 0; i < SVM_weights_.size(); i++)
264  {
265  confidence += SVM_weights_[i] * descriptor[i];
266  }
267  // Confidence correction:
268  confidence -= SVM_offset_;
269 
270  delete[] descriptor;
271  delete[] sample_float;
272  }
273  else
274  {
275  confidence = std::numeric_limits<double>::quiet_NaN();
276  }
277 
278  return confidence;
279 }
280 
281 template <typename PointT> double
283  Eigen::Vector3f& bottom,
284  Eigen::Vector3f& top,
285  Eigen::Vector3f& centroid,
286  bool vertical)
287 {
288  float pixel_height;
289  float pixel_width;
290 
291  if (!vertical)
292  {
293  pixel_height = bottom(1) - top(1);
294  pixel_width = pixel_height / 2.0f;
295  }
296  else
297  {
298  pixel_width = top(0) - bottom(0);
299  pixel_height = pixel_width / 2.0f;
300  }
301  float pixel_xc = centroid(0);
302  float pixel_yc = centroid(1);
303 
304  if (!vertical)
305  {
306  return (evaluate(pixel_height, pixel_xc, pixel_yc, image));
307  }
308  else
309  {
310  return (evaluate(pixel_width, pixel_yc, image->height-pixel_xc+1, image));
311  }
312 }
313 #endif /* PCL_PEOPLE_PERSON_CLASSIFIER_HPP_ */
bool loadSVMFromFile(std::string svm_filename)
Load SVM parameters from a text file.
void copyMakeBorder(PointCloudPtr &input_image, PointCloudPtr &output_image, int xmin, int ymin, int width, int height)
Copies an image and makes a black border around it, where the source image is not present...
virtual ~PersonClassifier()
Destructor.
void resize(PointCloudPtr &input_image, PointCloudPtr &output_image, int width, int height)
Resize an image represented by a pointcloud containing RGB information.
void getSVM(int &window_height, int &window_width, std::vector< float > &SVM_weights, float &SVM_offset)
Get trained SVM for person confidence estimation.
boost::shared_ptr< PointCloud > PointCloudPtr
HOG represents a class for computing the HOG descriptor described in Dalal, N.
Definition: hog.h:55
void compute(float *I, int h, int w, int n_channels, int bin_size, int n_orients, bool soft_bin, float *descriptor)
Compute HOG descriptor.
void setSVM(int window_height, int window_width, std::vector< float > SVM_weights, float SVM_offset)
Set trained SVM for person confidence estimation.
A point structure representing Euclidean xyz coordinates, and the RGB color.
double evaluate(float height, float xc, float yc, PointCloudPtr &image)
Classify the given portion of image.