Point Cloud Library (PCL)  1.14.1-dev
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  std::size_t tok_pos = line.find_first_of(':', 0); // search for token ":"
61  window_height_ = std::atoi(line.substr(tok_pos+1, std::string::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, std::string::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, std::string::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  std::size_t tok_end_pos = line.find_first_of(']', 0); // search for token "]" , end of SVM weights
74  while (tok_pos < tok_end_pos) // while end of SVM_weights is not reached
75  {
76  std::size_t prev_tok_pos = tok_pos;
77  tok_pos = line.find_first_of(',', prev_tok_pos+1); // search for token ","
78  SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str()));
79  }
80  SVM_file.close();
81 
82  if (SVM_weights_.empty ())
83  {
84  PCL_ERROR ("[pcl::people::PersonClassifier::loadSVMFromFile] Invalid SVM file!\n");
85  return (false);
86  }
87  return (true);
88 }
89 
90 template <typename PointT> void
91 pcl::people::PersonClassifier<PointT>::setSVM (int window_height, int window_width, std::vector<float> SVM_weights, float SVM_offset)
92 {
93  window_height_ = window_height;
94  window_width_ = window_width;
95  SVM_weights_ = SVM_weights;
96  SVM_offset_ = SVM_offset;
97 }
98 
99 template <typename PointT> void
100 pcl::people::PersonClassifier<PointT>::getSVM (int& window_height, int& window_width, std::vector<float>& SVM_weights, float& SVM_offset)
101 {
102  window_height = window_height_;
103  window_width = window_width_;
104  SVM_weights = SVM_weights_;
105  SVM_offset = SVM_offset_;
106 }
107 
108 template <typename PointT> void
110  PointCloudPtr& output_image,
111  int width,
112  int height)
113 {
114  PointT new_point;
115  new_point.r = 0;
116  new_point.g = 0;
117  new_point.b = 0;
118 
119  // Allocate the vector of points:
120  output_image->points.resize(width*height, new_point);
121  output_image->height = height;
122  output_image->width = width;
123 
124  // Compute scale factor:
125  float scale1 = static_cast<float>(height) / static_cast<float>(input_image->height);
126  float scale2 = static_cast<float>(width) / static_cast<float>(input_image->width);
127 
128  Eigen::Matrix3f T_inv;
129  T_inv << 1/scale1, 0, 0,
130  0, 1/scale2, 0,
131  0, 0, 1;
132 
133  Eigen::Vector3f A;
134  int c1, c2, f1, f2;
135  PointT g1, g2, g3, g4;
136  float w1, w2;
137  for (int i = 0; i < height; i++) // for every row
138  {
139  for (int j = 0; j < width; j++) // for every column
140  {
141  A = T_inv * Eigen::Vector3f(i, j, 1);
142  c1 = std::ceil(A(0));
143  f1 = std::floor(A(0));
144  c2 = std::ceil(A(1));
145  f2 = std::floor(A(1));
146 
147  if ( (f1 < 0) ||
148  (c1 < 0) ||
149  (f1 >= static_cast<int> (input_image->height)) ||
150  (c1 >= static_cast<int> (input_image->height)) ||
151  (f2 < 0) ||
152  (c2 < 0) ||
153  (f2 >= static_cast<int> (input_image->width)) ||
154  (c2 >= static_cast<int> (input_image->width)))
155  { // if out of range, continue
156  continue;
157  }
158 
159  g1 = (*input_image)(f2, c1);
160  g3 = (*input_image)(f2, f1);
161  g4 = (*input_image)(c2, f1);
162  g2 = (*input_image)(c2, c1);
163 
164  w1 = (A(0) - f1);
165  w2 = (A(1) - f2);
166  new_point.r = static_cast<int>((1 - w1) * ((1 - w2) * g1.r + w2 * g4.r) + w1 * ((1 - w2) * g3.r + w2 * g4.r));
167  new_point.g = static_cast<int>((1 - w1) * ((1 - w2) * g1.g + w2 * g4.g) + w1 * ((1 - w2) * g3.g + w2 * g4.g));
168  new_point.b = static_cast<int>((1 - w1) * ((1 - w2) * g1.b + w2 * g4.b) + w1 * ((1 - w2) * g3.b + w2 * g4.b));
169 
170  // Insert the point in the output image:
171  (*output_image)(j,i) = new_point;
172  }
173  }
174 }
175 
176 template <typename PointT> void
178  PointCloudPtr& output_image,
179  int xmin,
180  int ymin,
181  int width,
182  int height)
183 {
184  PointT black_point;
185  black_point.r = 0;
186  black_point.g = 0;
187  black_point.b = 0;
188  output_image->points.resize(height*width, black_point);
189  output_image->width = width;
190  output_image->height = height;
191 
192  int x_start_in = std::max(0, xmin);
193  int x_end_in = std::min(static_cast<int>(input_image->width-1), xmin+width-1);
194  int y_start_in = std::max(0, ymin);
195  int y_end_in = std::min(static_cast<int>(input_image->height-1), ymin+height-1);
196 
197  int x_start_out = std::max(0, -xmin);
198  //int x_end_out = x_start_out + (x_end_in - x_start_in);
199  int y_start_out = std::max(0, -ymin);
200  //int y_end_out = y_start_out + (y_end_in - y_start_in);
201 
202  for (int i = 0; i < (y_end_in - y_start_in + 1); i++)
203  {
204  for (int j = 0; j < (x_end_in - x_start_in + 1); j++)
205  {
206  (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i);
207  }
208  }
209 }
210 
211 template <typename PointT> double
213  float xc,
214  float yc,
215  PointCloudPtr& image)
216 {
217  if (SVM_weights_.empty ())
218  {
219  PCL_ERROR ("[pcl::people::PersonClassifier::evaluate] SVM has not been set!\n");
220  return (-1000);
221  }
222 
223  int height = std::floor((height_person * window_height_) / (0.75 * window_height_) + 0.5); // std::floor(i+0.5) = round(i)
224  int width = std::floor((height_person * window_width_) / (0.75 * window_height_) + 0.5);
225  int xmin = std::floor(xc - width / 2 + 0.5);
226  int ymin = std::floor(yc - height / 2 + 0.5);
227  double confidence;
228 
229  if (height > 0)
230  {
231  // If near the border, fill with black:
232  PointCloudPtr box(new PointCloud);
233  copyMakeBorder(image, box, xmin, ymin, width, height);
234 
235  // Make the image match the correct size (used in the training stage):
236  PointCloudPtr sample(new PointCloud);
237  resize(box, sample, window_width_, window_height_);
238 
239  // Convert the image to array of float:
240  float* sample_float = new float[sample->width * sample->height * 3];
241  int delta = sample->height * sample->width;
242  for (std::uint32_t row = 0; row < sample->height; row++)
243  {
244  for (std::uint32_t col = 0; col < sample->width; col++)
245  {
246  sample_float[row + sample->height * col] = (static_cast<float> ((*sample)(col, row).r))/255; //ptr[col * 3 + 2];
247  sample_float[row + sample->height * col + delta] = (static_cast<float> ((*sample)(col, row).g))/255; //ptr[col * 3 + 1];
248  sample_float[row + sample->height * col + delta * 2] = static_cast<float> (((*sample)(col, row).b))/255; //ptr[col * 3];
249  }
250  }
251 
252  // Calculate HOG descriptor:
253  pcl::people::HOG hog;
254  float *descriptor = new float[SVM_weights_.size()];
255  std::fill_n(descriptor, SVM_weights_.size(), 0.0f);
256  hog.compute(sample_float, descriptor);
257 
258  // Calculate confidence value by dot product:
259  confidence = 0.0;
260  for(std::size_t i = 0; i < SVM_weights_.size(); i++)
261  {
262  confidence += SVM_weights_[i] * descriptor[i];
263  }
264  // Confidence correction:
265  confidence -= SVM_offset_;
266 
267  delete[] descriptor;
268  delete[] sample_float;
269  }
270  else
271  {
272  confidence = std::numeric_limits<double>::quiet_NaN();
273  }
274 
275  return confidence;
276 }
277 
278 template <typename PointT> double
280  Eigen::Vector3f& bottom,
281  Eigen::Vector3f& top,
282  Eigen::Vector3f& centroid,
283  bool vertical)
284 {
285  float pixel_height;
286  float pixel_width;
287 
288  if (!vertical)
289  {
290  pixel_height = bottom(1) - top(1);
291  pixel_width = pixel_height / 2.0f;
292  }
293  else
294  {
295  pixel_width = top(0) - bottom(0);
296  pixel_height = pixel_width / 2.0f;
297  }
298  float pixel_xc = centroid(0);
299  float pixel_yc = centroid(1);
300 
301  if (!vertical)
302  {
303  return (evaluate(pixel_height, pixel_xc, pixel_yc, image));
304  }
305  return (evaluate(pixel_width, pixel_yc, image->height-pixel_xc+1, image));
306 }
307 #endif /* PCL_PEOPLE_PERSON_CLASSIFIER_HPP_ */
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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 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.
double evaluate(float height, float xc, float yc, PointCloudPtr &image)
Classify the given portion of image.
void setSVM(int window_height, int window_width, std::vector< float > SVM_weights, float SVM_offset)
Set trained SVM for person confidence estimation.
bool loadSVMFromFile(const std::string &svm_filename)
Load SVM parameters from a text file.
void getSVM(int &window_height, int &window_width, std::vector< float > &SVM_weights, float &SVM_offset)
Get trained SVM for person confidence estimation.
void resize(PointCloudPtr &input_image, PointCloudPtr &output_image, int width, int height)
Resize an image represented by a pointcloud containing RGB information.
A point structure representing Euclidean xyz coordinates, and the RGB color.