41 #include <pcl/people/person_classifier.h>
43 #ifndef PCL_PEOPLE_PERSON_CLASSIFIER_HPP_
44 #define PCL_PEOPLE_PERSON_CLASSIFIER_HPP_
46 template <
typename Po
intT>
49 template <
typename Po
intT>
52 template <
typename Po
intT>
bool
56 std::ifstream SVM_file;
57 SVM_file.open(svm_filename.c_str());
59 getline (SVM_file,line);
60 std::size_t tok_pos = line.find_first_of(
':', 0);
61 window_height_ = std::atoi(line.substr(tok_pos+1, std::string::npos - tok_pos-1).c_str());
63 getline (SVM_file,line);
64 tok_pos = line.find_first_of(
':', 0);
65 window_width_ = std::atoi(line.substr(tok_pos+1, std::string::npos - tok_pos-1).c_str());
67 getline (SVM_file,line);
68 tok_pos = line.find_first_of(
':', 0);
69 SVM_offset_ = std::atof(line.substr(tok_pos+1, std::string::npos - tok_pos-1).c_str());
71 getline (SVM_file,line);
72 tok_pos = line.find_first_of(
'[', 0);
73 std::size_t tok_end_pos = line.find_first_of(
']', 0);
74 std::size_t prev_tok_pos;
75 while (tok_pos < tok_end_pos)
77 prev_tok_pos = tok_pos;
78 tok_pos = line.find_first_of(
',', prev_tok_pos+1);
79 SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str()));
83 if (SVM_weights_.empty ())
85 PCL_ERROR (
"[pcl::people::PersonClassifier::loadSVMFromFile] Invalid SVM file!\n");
91 template <
typename Po
intT>
void
94 window_height_ = window_height;
95 window_width_ = window_width;
96 SVM_weights_ = SVM_weights;
97 SVM_offset_ = SVM_offset;
100 template <
typename Po
intT>
void
103 window_height = window_height_;
104 window_width = window_width_;
105 SVM_weights = SVM_weights_;
106 SVM_offset = SVM_offset_;
109 template <
typename Po
intT>
void
121 output_image->points.resize(width*height, new_point);
122 output_image->height = height;
123 output_image->width = width;
126 float scale1 = float(height) / float(input_image->height);
127 float scale2 = float(width) / float(input_image->width);
129 Eigen::Matrix3f T_inv;
130 T_inv << 1/scale1, 0, 0,
138 for (
int i = 0; i < height; i++)
140 for (
int j = 0; j < width; j++)
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));
150 (f1 >=
static_cast<int> (input_image->height)) ||
151 (c1 >=
static_cast<int> (input_image->height)) ||
154 (f2 >=
static_cast<int> (input_image->width)) ||
155 (c2 >=
static_cast<int> (input_image->width)))
160 g1 = (*input_image)(f2, c1);
161 g3 = (*input_image)(f2, f1);
162 g4 = (*input_image)(c2, f1);
163 g2 = (*input_image)(c2, c1);
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));
172 (*output_image)(j,i) = new_point;
177 template <
typename Po
intT>
void
189 output_image->points.resize(height*width, black_point);
190 output_image->width = width;
191 output_image->height = height;
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);
198 int x_start_out = std::max(0, -xmin);
200 int y_start_out = std::max(0, -ymin);
203 for (
int i = 0; i < (y_end_in - y_start_in + 1); i++)
205 for (
int j = 0; j < (x_end_in - x_start_in + 1); j++)
207 (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i);
212 template <
typename Po
intT>
double
218 if (SVM_weights_.empty ())
220 PCL_ERROR (
"[pcl::people::PersonClassifier::evaluate] SVM has not been set!\n");
224 int height = std::floor((height_person * window_height_) / (0.75 * window_height_) + 0.5);
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);
234 copyMakeBorder(image, box, xmin, ymin, width, height);
238 resize(box, sample, window_width_, window_height_);
241 float* sample_float =
new float[sample->width * sample->height * 3];
242 int delta = sample->height * sample->width;
247 sample_float[row + sample->height * col] = ((float) ((*sample)(col, row).r))/255;
248 sample_float[row + sample->height * col + delta] = ((float) ((*sample)(col, row).g))/255;
249 sample_float[row + sample->height * col + delta * 2] = (float) (((*sample)(col, row).b))/255;
255 float *descriptor = (
float*) calloc(SVM_weights_.size(),
sizeof(float));
256 hog.
compute(sample_float, descriptor);
260 for(std::size_t i = 0; i < SVM_weights_.size(); i++)
262 confidence += SVM_weights_[i] * descriptor[i];
265 confidence -= SVM_offset_;
268 delete[] sample_float;
272 confidence = std::numeric_limits<double>::quiet_NaN();
278 template <
typename Po
intT>
double
280 Eigen::Vector3f& bottom,
281 Eigen::Vector3f& top,
282 Eigen::Vector3f& centroid,
290 pixel_height = bottom(1) - top(1);
291 pixel_width = pixel_height / 2.0f;
295 pixel_width = top(0) - bottom(0);
296 pixel_height = pixel_width / 2.0f;
298 float pixel_xc = centroid(0);
299 float pixel_yc = centroid(1);
303 return (evaluate(pixel_height, pixel_xc, pixel_yc, image));
305 return (evaluate(pixel_width, pixel_yc, image->height-pixel_xc+1, image));