Point Cloud Library (PCL)  1.10.0
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  std::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_.empty ())
84  {
85  PCL_ERROR ("[pcl::people::PersonClassifier::loadSVMFromFile] Invalid SVM file!\n");
86  return (false);
87  }
88  return (true);
89 }
90 
91 template <typename PointT> void
92 pcl::people::PersonClassifier<PointT>::setSVM (int window_height, int window_width, std::vector<float> SVM_weights, float SVM_offset)
93 {
94  window_height_ = window_height;
95  window_width_ = window_width;
96  SVM_weights_ = SVM_weights;
97  SVM_offset_ = SVM_offset;
98 }
99 
100 template <typename PointT> void
101 pcl::people::PersonClassifier<PointT>::getSVM (int& window_height, int& window_width, std::vector<float>& SVM_weights, float& SVM_offset)
102 {
103  window_height = window_height_;
104  window_width = window_width_;
105  SVM_weights = SVM_weights_;
106  SVM_offset = SVM_offset_;
107 }
108 
109 template <typename PointT> void
111  PointCloudPtr& output_image,
112  int width,
113  int height)
114 {
115  PointT new_point;
116  new_point.r = 0;
117  new_point.g = 0;
118  new_point.b = 0;
119 
120  // Allocate the vector of points:
121  output_image->points.resize(width*height, new_point);
122  output_image->height = height;
123  output_image->width = width;
124 
125  // Compute scale factor:
126  float scale1 = float(height) / float(input_image->height);
127  float scale2 = float(width) / float(input_image->width);
128 
129  Eigen::Matrix3f T_inv;
130  T_inv << 1/scale1, 0, 0,
131  0, 1/scale2, 0,
132  0, 0, 1;
133 
134  Eigen::Vector3f A;
135  int c1, c2, f1, f2;
136  PointT g1, g2, g3, g4;
137  float w1, w2;
138  for (int i = 0; i < height; i++) // for every row
139  {
140  for (int j = 0; j < width; j++) // for every column
141  {
142  A = T_inv * Eigen::Vector3f(i, j, 1);
143  c1 = std::ceil(A(0));
144  f1 = std::floor(A(0));
145  c2 = std::ceil(A(1));
146  f2 = std::floor(A(1));
147 
148  if ( (f1 < 0) ||
149  (c1 < 0) ||
150  (f1 >= static_cast<int> (input_image->height)) ||
151  (c1 >= static_cast<int> (input_image->height)) ||
152  (f2 < 0) ||
153  (c2 < 0) ||
154  (f2 >= static_cast<int> (input_image->width)) ||
155  (c2 >= static_cast<int> (input_image->width)))
156  { // if out of range, continue
157  continue;
158  }
159 
160  g1 = (*input_image)(f2, c1);
161  g3 = (*input_image)(f2, f1);
162  g4 = (*input_image)(c2, f1);
163  g2 = (*input_image)(c2, c1);
164 
165  w1 = (A(0) - f1);
166  w2 = (A(1) - f2);
167  new_point.r = int((1 - w1) * ((1 - w2) * g1.r + w2 * g4.r) + w1 * ((1 - w2) * g3.r + w2 * g4.r));
168  new_point.g = int((1 - w1) * ((1 - w2) * g1.g + w2 * g4.g) + w1 * ((1 - w2) * g3.g + w2 * g4.g));
169  new_point.b = int((1 - w1) * ((1 - w2) * g1.b + w2 * g4.b) + w1 * ((1 - w2) * g3.b + w2 * g4.b));
170 
171  // Insert the point in the output image:
172  (*output_image)(j,i) = new_point;
173  }
174  }
175 }
176 
177 template <typename PointT> void
179  PointCloudPtr& output_image,
180  int xmin,
181  int ymin,
182  int width,
183  int height)
184 {
185  PointT black_point;
186  black_point.r = 0;
187  black_point.g = 0;
188  black_point.b = 0;
189  output_image->points.resize(height*width, black_point);
190  output_image->width = width;
191  output_image->height = height;
192 
193  int x_start_in = std::max(0, xmin);
194  int x_end_in = std::min(int(input_image->width-1), xmin+width-1);
195  int y_start_in = std::max(0, ymin);
196  int y_end_in = std::min(int(input_image->height-1), ymin+height-1);
197 
198  int x_start_out = std::max(0, -xmin);
199  //int x_end_out = x_start_out + (x_end_in - x_start_in);
200  int y_start_out = std::max(0, -ymin);
201  //int y_end_out = y_start_out + (y_end_in - y_start_in);
202 
203  for (int i = 0; i < (y_end_in - y_start_in + 1); i++)
204  {
205  for (int j = 0; j < (x_end_in - x_start_in + 1); j++)
206  {
207  (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i);
208  }
209  }
210 }
211 
212 template <typename PointT> double
214  float xc,
215  float yc,
216  PointCloudPtr& image)
217 {
218  if (SVM_weights_.empty ())
219  {
220  PCL_ERROR ("[pcl::people::PersonClassifier::evaluate] SVM has not been set!\n");
221  return (-1000);
222  }
223 
224  int height = std::floor((height_person * window_height_) / (0.75 * window_height_) + 0.5); // std::floor(i+0.5) = round(i)
225  int width = std::floor((height_person * window_width_) / (0.75 * window_height_) + 0.5);
226  int xmin = std::floor(xc - width / 2 + 0.5);
227  int ymin = std::floor(yc - height / 2 + 0.5);
228  double confidence;
229 
230  if (height > 0)
231  {
232  // If near the border, fill with black:
233  PointCloudPtr box(new PointCloud);
234  copyMakeBorder(image, box, xmin, ymin, width, height);
235 
236  // Make the image match the correct size (used in the training stage):
237  PointCloudPtr sample(new PointCloud);
238  resize(box, sample, window_width_, window_height_);
239 
240  // Convert the image to array of float:
241  float* sample_float = new float[sample->width * sample->height * 3];
242  int delta = sample->height * sample->width;
243  for (std::uint32_t row = 0; row < sample->height; row++)
244  {
245  for (std::uint32_t col = 0; col < sample->width; col++)
246  {
247  sample_float[row + sample->height * col] = ((float) ((*sample)(col, row).r))/255; //ptr[col * 3 + 2];
248  sample_float[row + sample->height * col + delta] = ((float) ((*sample)(col, row).g))/255; //ptr[col * 3 + 1];
249  sample_float[row + sample->height * col + delta * 2] = (float) (((*sample)(col, row).b))/255; //ptr[col * 3];
250  }
251  }
252 
253  // Calculate HOG descriptor:
254  pcl::people::HOG hog;
255  float *descriptor = (float*) calloc(SVM_weights_.size(), sizeof(float));
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_ */
pcl::uint32_t
std::uint32_t uint32_t
Definition: pcl_macros.h:96
pcl::PointCloud< pcl::RGB >
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::people::PersonClassifier::resize
void resize(PointCloudPtr &input_image, PointCloudPtr &output_image, int width, int height)
Resize an image represented by a pointcloud containing RGB information.
Definition: person_classifier.hpp:110
pcl::people::HOG
HOG represents a class for computing the HOG descriptor described in Dalal, N.
Definition: hog.h:54
pcl::people::PersonClassifier< pcl::RGB >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: person_classifier.h:72
pcl::people::HOG::compute
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.
pcl::people::PersonClassifier::evaluate
double evaluate(float height, float xc, float yc, PointCloudPtr &image)
Classify the given portion of image.
Definition: person_classifier.hpp:213
pcl::people::PersonClassifier::setSVM
void setSVM(int window_height, int window_width, std::vector< float > SVM_weights, float SVM_offset)
Set trained SVM for person confidence estimation.
Definition: person_classifier.hpp:92
pcl::people::PersonClassifier::copyMakeBorder
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.
Definition: person_classifier.hpp:178
pcl::people::PersonClassifier::getSVM
void getSVM(int &window_height, int &window_width, std::vector< float > &SVM_weights, float &SVM_offset)
Get trained SVM for person confidence estimation.
Definition: person_classifier.hpp:101
pcl::people::PersonClassifier::PersonClassifier
PersonClassifier()
Constructor.
Definition: person_classifier.hpp:47
pcl::people::PersonClassifier::~PersonClassifier
virtual ~PersonClassifier()
Destructor.
Definition: person_classifier.hpp:50
pcl::people::PersonClassifier::loadSVMFromFile
bool loadSVMFromFile(std::string svm_filename)
Load SVM parameters from a text file.
Definition: person_classifier.hpp:53