person_classifier.hpp
Go to the documentation of this file.
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_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:28:14