Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2013-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * person_classifier.hpp 00037 * Created on: Nov 30, 2012 00038 * Author: Matteo Munaro 00039 */ 00040 00041 #include <pcl/people/person_classifier.h> 00042 00043 #ifndef PCL_PEOPLE_PERSON_CLASSIFIER_HPP_ 00044 #define PCL_PEOPLE_PERSON_CLASSIFIER_HPP_ 00045 00046 template <typename PointT> 00047 pcl::people::PersonClassifier<PointT>::PersonClassifier () {} 00048 00049 template <typename PointT> 00050 pcl::people::PersonClassifier<PointT>::~PersonClassifier () {} 00051 00052 template <typename PointT> bool 00053 pcl::people::PersonClassifier<PointT>::loadSVMFromFile (std::string svm_filename) 00054 { 00055 std::string line; 00056 std::ifstream SVM_file; 00057 SVM_file.open(svm_filename.c_str()); 00058 00059 getline (SVM_file,line); // read window_height line 00060 size_t tok_pos = line.find_first_of(":", 0); // search for token ":" 00061 window_height_ = std::atoi(line.substr(tok_pos+1, line.npos - tok_pos-1).c_str()); 00062 00063 getline (SVM_file,line); // read window_width line 00064 tok_pos = line.find_first_of(":", 0); // search for token ":" 00065 window_width_ = std::atoi(line.substr(tok_pos+1, line.npos - tok_pos-1).c_str()); 00066 00067 getline (SVM_file,line); // read SVM_offset line 00068 tok_pos = line.find_first_of(":", 0); // search for token ":" 00069 SVM_offset_ = std::atof(line.substr(tok_pos+1, line.npos - tok_pos-1).c_str()); 00070 00071 getline (SVM_file,line); // read SVM_weights line 00072 tok_pos = line.find_first_of("[", 0); // search for token "[" 00073 size_t tok_end_pos = line.find_first_of("]", 0); // search for token "]" , end of SVM weights 00074 size_t prev_tok_pos; 00075 while (tok_pos < tok_end_pos) // while end of SVM_weights is not reached 00076 { 00077 prev_tok_pos = tok_pos; 00078 tok_pos = line.find_first_of(",", prev_tok_pos+1); // search for token "," 00079 SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str())); 00080 } 00081 SVM_file.close(); 00082 00083 if (SVM_weights_.size() == 0) 00084 { 00085 PCL_ERROR ("[pcl::people::PersonClassifier::loadSVMFromFile] Invalid SVM file!\n"); 00086 return (false); 00087 } 00088 else 00089 { 00090 return (true); 00091 } 00092 } 00093 00094 template <typename PointT> void 00095 pcl::people::PersonClassifier<PointT>::setSVM (int window_height, int window_width, std::vector<float> SVM_weights, float SVM_offset) 00096 { 00097 window_height_ = window_height; 00098 window_width_ = window_width; 00099 SVM_weights_ = SVM_weights; 00100 SVM_offset_ = SVM_offset; 00101 } 00102 00103 template <typename PointT> void 00104 pcl::people::PersonClassifier<PointT>::getSVM (int& window_height, int& window_width, std::vector<float>& SVM_weights, float& SVM_offset) 00105 { 00106 window_height = window_height_; 00107 window_width = window_width_; 00108 SVM_weights = SVM_weights_; 00109 SVM_offset = SVM_offset_; 00110 } 00111 00112 template <typename PointT> void 00113 pcl::people::PersonClassifier<PointT>::resize (PointCloudPtr& input_image, 00114 PointCloudPtr& output_image, 00115 int width, 00116 int height) 00117 { 00118 PointT new_point; 00119 new_point.r = 0; 00120 new_point.g = 0; 00121 new_point.b = 0; 00122 00123 // Allocate the vector of points: 00124 output_image->points.resize(width*height, new_point); 00125 output_image->height = height; 00126 output_image->width = width; 00127 00128 // Compute scale factor: 00129 float scale1 = float(height) / float(input_image->height); 00130 float scale2 = float(width) / float(input_image->width); 00131 00132 Eigen::Matrix3f T_inv; 00133 T_inv << 1/scale1, 0, 0, 00134 0, 1/scale2, 0, 00135 0, 0, 1; 00136 00137 Eigen::Vector3f A; 00138 int c1, c2, f1, f2; 00139 PointT g1, g2, g3, g4; 00140 float w1, w2; 00141 for (unsigned int i = 0; i < height; i++) // for every row 00142 { 00143 for (unsigned int j = 0; j < width; j++) // for every column 00144 { 00145 A = T_inv * Eigen::Vector3f(i, j, 1); 00146 c1 = ceil(A(0)); 00147 f1 = floor(A(0)); 00148 c2 = ceil(A(1)); 00149 f2 = floor(A(1)); 00150 00151 if ( (f1 < 0) || 00152 (c1 < 0) || 00153 (f1 >= input_image->height) || 00154 (c1 >= input_image->height) || 00155 (f2 < 0) || 00156 (c2 < 0) || 00157 (f2 >= input_image->width) || 00158 (c2 >= input_image->width)) 00159 { // if out of range, continue 00160 continue; 00161 } 00162 00163 g1 = (*input_image)(f2, c1); 00164 g3 = (*input_image)(f2, f1); 00165 g4 = (*input_image)(c2, f1); 00166 g2 = (*input_image)(c2, c1); 00167 00168 w1 = (A(0) - f1); 00169 w2 = (A(1) - f2); 00170 new_point.r = int((1 - w1) * ((1 - w2) * g1.r + w2 * g4.r) + w1 * ((1 - w2) * g3.r + w2 * g4.r)); 00171 new_point.g = int((1 - w1) * ((1 - w2) * g1.g + w2 * g4.g) + w1 * ((1 - w2) * g3.g + w2 * g4.g)); 00172 new_point.b = int((1 - w1) * ((1 - w2) * g1.b + w2 * g4.b) + w1 * ((1 - w2) * g3.b + w2 * g4.b)); 00173 00174 // Insert the point in the output image: 00175 (*output_image)(j,i) = new_point; 00176 } 00177 } 00178 } 00179 00180 template <typename PointT> void 00181 pcl::people::PersonClassifier<PointT>::copyMakeBorder (PointCloudPtr& input_image, 00182 PointCloudPtr& output_image, 00183 int xmin, 00184 int ymin, 00185 int width, 00186 int height) 00187 { 00188 PointT black_point; 00189 black_point.r = 0; 00190 black_point.g = 0; 00191 black_point.b = 0; 00192 output_image->points.resize(height*width, black_point); 00193 output_image->width = width; 00194 output_image->height = height; 00195 00196 int x_start_in = std::max(0, xmin); 00197 int x_end_in = std::min(int(input_image->width-1), xmin+width-1); 00198 int y_start_in = std::max(0, ymin); 00199 int y_end_in = std::min(int(input_image->height-1), ymin+height-1); 00200 00201 int x_start_out = std::max(0, -xmin); 00202 //int x_end_out = x_start_out + (x_end_in - x_start_in); 00203 int y_start_out = std::max(0, -ymin); 00204 //int y_end_out = y_start_out + (y_end_in - y_start_in); 00205 00206 for (unsigned int i = 0; i < (y_end_in - y_start_in + 1); i++) 00207 { 00208 for (unsigned int j = 0; j < (x_end_in - x_start_in + 1); j++) 00209 { 00210 (*output_image)(x_start_out + j, y_start_out + i) = (*input_image)(x_start_in + j, y_start_in + i); 00211 } 00212 } 00213 } 00214 00215 template <typename PointT> double 00216 pcl::people::PersonClassifier<PointT>::evaluate (float height_person, 00217 float xc, 00218 float yc, 00219 PointCloudPtr& image) 00220 { 00221 if (SVM_weights_.size() == 0) 00222 { 00223 PCL_ERROR ("[pcl::people::PersonClassifier::evaluate] SVM has not been set!\n"); 00224 return (-1000); 00225 } 00226 00227 int height = floor((height_person * window_height_) / (0.75 * window_height_) + 0.5); // floor(i+0.5) = round(i) 00228 int width = floor((height_person * window_width_) / (0.75 * window_height_) + 0.5); 00229 int xmin = floor(xc - width / 2 + 0.5); 00230 int ymin = floor(yc - height / 2 + 0.5); 00231 double confidence; 00232 00233 if (height > 0) 00234 { 00235 // If near the border, fill with black: 00236 PointCloudPtr box(new PointCloud); 00237 copyMakeBorder(image, box, xmin, ymin, width, height); 00238 00239 // Make the image match the correct size (used in the training stage): 00240 PointCloudPtr sample(new PointCloud); 00241 resize(box, sample, window_width_, window_height_); 00242 00243 // Convert the image to array of float: 00244 float* sample_float = new float[sample->width * sample->height * 3]; 00245 int delta = sample->height * sample->width; 00246 for(int row = 0; row < sample->height; row++) 00247 { 00248 for(int col = 0; col < sample->width; col++) 00249 { 00250 sample_float[row + sample->height * col] = ((float) ((*sample)(col, row).r))/255; //ptr[col * 3 + 2]; 00251 sample_float[row + sample->height * col + delta] = ((float) ((*sample)(col, row).g))/255; //ptr[col * 3 + 1]; 00252 sample_float[row + sample->height * col + delta * 2] = (float) (((*sample)(col, row).b))/255; //ptr[col * 3]; 00253 } 00254 } 00255 00256 // Calculate HOG descriptor: 00257 pcl::people::HOG hog; 00258 float *descriptor = (float*) calloc(SVM_weights_.size(), sizeof(float)); 00259 hog.compute(sample_float, descriptor); 00260 00261 // Calculate confidence value by dot product: 00262 confidence = 0.0; 00263 for(unsigned int i = 0; i < SVM_weights_.size(); i++) 00264 { 00265 confidence += SVM_weights_[i] * descriptor[i]; 00266 } 00267 // Confidence correction: 00268 confidence -= SVM_offset_; 00269 00270 delete[] descriptor; 00271 delete[] sample_float; 00272 } 00273 else 00274 { 00275 confidence = std::numeric_limits<double>::quiet_NaN(); 00276 } 00277 00278 return confidence; 00279 } 00280 00281 template <typename PointT> double 00282 pcl::people::PersonClassifier<PointT>::evaluate (PointCloudPtr& image, 00283 Eigen::Vector3f& bottom, 00284 Eigen::Vector3f& top, 00285 Eigen::Vector3f& centroid, 00286 bool vertical) 00287 { 00288 float pixel_height; 00289 float pixel_width; 00290 00291 if (!vertical) 00292 { 00293 pixel_height = bottom(1) - top(1); 00294 pixel_width = pixel_height / 2.0f; 00295 } 00296 else 00297 { 00298 pixel_width = top(0) - bottom(0); 00299 pixel_height = pixel_width / 2.0f; 00300 } 00301 float pixel_xc = centroid(0); 00302 float pixel_yc = centroid(1); 00303 00304 if (!vertical) 00305 { 00306 return (evaluate(pixel_height, pixel_xc, pixel_yc, image)); 00307 } 00308 else 00309 { 00310 return (evaluate(pixel_width, pixel_yc, image->height-pixel_xc+1, image)); 00311 } 00312 } 00313 #endif /* PCL_PEOPLE_PERSON_CLASSIFIER_HPP_ */