Point Cloud Library (PCL) 1.12.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
46template <typename PointT>
48
49template <typename PointT>
51
52template <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
90template <typename PointT> void
91pcl::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}
99template <typename PointT> void
100pcl::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
108template <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 = float(height) / float(input_image->height);
126 float scale2 = float(width) / 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 = int((1 - w1) * ((1 - w2) * g1.r + w2 * g4.r) + w1 * ((1 - w2) * g3.r + w2 * g4.r));
167 new_point.g = int((1 - w1) * ((1 - w2) * g1.g + w2 * g4.g) + w1 * ((1 - w2) * g3.g + w2 * g4.g));
168 new_point.b = 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
176template <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(int(input_image->width-1), xmin+width-1);
194 int y_start_in = std::max(0, ymin);
195 int y_end_in = std::min(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
211template <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] = ((float) ((*sample)(col, row).r))/255; //ptr[col * 3 + 2];
247 sample_float[row + sample->height * col + delta] = ((float) ((*sample)(col, row).g))/255; //ptr[col * 3 + 1];
248 sample_float[row + sample->height * col + delta * 2] = (float) (((*sample)(col, row).b))/255; //ptr[col * 3];
249 }
250 }
251
252 // Calculate HOG descriptor:
254 float *descriptor = (float*) calloc(SVM_weights_.size(), sizeof(float));
255 hog.compute(sample_float, descriptor);
256
257 // Calculate confidence value by dot product:
258 confidence = 0.0;
259 for(std::size_t i = 0; i < SVM_weights_.size(); i++)
260 {
261 confidence += SVM_weights_[i] * descriptor[i];
262 }
263 // Confidence correction:
264 confidence -= SVM_offset_;
265
266 delete[] descriptor;
267 delete[] sample_float;
268 }
269 else
270 {
271 confidence = std::numeric_limits<double>::quiet_NaN();
272 }
273
274 return confidence;
275}
276
277template <typename PointT> double
279 Eigen::Vector3f& bottom,
280 Eigen::Vector3f& top,
281 Eigen::Vector3f& centroid,
282 bool vertical)
283{
284 float pixel_height;
285 float pixel_width;
286
287 if (!vertical)
288 {
289 pixel_height = bottom(1) - top(1);
290 pixel_width = pixel_height / 2.0f;
291 }
292 else
293 {
294 pixel_width = top(0) - bottom(0);
295 pixel_height = pixel_width / 2.0f;
296 }
297 float pixel_xc = centroid(0);
298 float pixel_yc = centroid(1);
299
300 if (!vertical)
301 {
302 return (evaluate(pixel_height, pixel_xc, pixel_yc, image));
303 }
304 return (evaluate(pixel_width, pixel_yc, image->height-pixel_xc+1, image));
305}
306#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.
bool loadSVMFromFile(std::string svm_filename)
Load SVM parameters from a text file.
void setSVM(int window_height, int window_width, std::vector< float > SVM_weights, float SVM_offset)
Set trained SVM for person confidence estimation.
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.