PedestrianDetect: /home/laradmin/lar/perception/pedestrians/PedestrianDetect/src/pedfuncs.cpp Source File

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00027 #include "peddetect.h"
00028 
00029 
00030 Mat GradientMagnitude(Mat src)
00031 
00032 {
00033    Mat xsobel, ysobel, xsobel2, ysobel2, xysobel, GradMag, GrayImg;
00034    
00035    cvtColor(src,GrayImg, CV_RGB2GRAY,0);
00036   
00037    Sobel(GrayImg, xsobel, CV_32F, 1, 0, 3, 1, 0, BORDER_DEFAULT);
00038    Sobel(GrayImg, ysobel, CV_32F, 0, 1, 3, 1, 0, BORDER_DEFAULT);
00039    
00040    multiply(xsobel, xsobel, xsobel2, 1, -1 );
00041    multiply(ysobel, ysobel, ysobel2, 1, -1 );
00042    
00043    xysobel=xsobel2+ysobel2;
00044    sqrt(xysobel,GradMag);
00045    
00046    return GradMag;
00047   
00048 }
00049 
00050 Mat GradientMagnitude(Mat xsobel, Mat ysobel)
00051 
00052 {
00053    Mat xsobel2, ysobel2, xysobel, GradMag, GrayImg;
00054    
00055    multiply(xsobel, xsobel, xsobel2, 1, -1 );
00056    multiply(ysobel, ysobel, ysobel2, 1, -1 );
00057    
00058    xysobel=xsobel2+ysobel2;
00059    sqrt(xysobel,GradMag);
00060    
00061    return GradMag;
00062   
00063 }
00064 
00065 
00066 MatVector OrientedGradientsDiagram(Mat GradMag, Mat xsobel, Mat ysobel)
00067 {
00068   Mat xsobeldiv=xsobel + 0.00001;
00069   Mat div;
00070   float angl;
00071   
00072   MatVector BinVector;
00073    
00074   divide(ysobel,xsobeldiv,div);
00075 
00076    
00077    Mat Bins[6];
00078     
00079     for (int n=0; n<6; n++)
00080     {
00081       Bins[n].create(GradMag.rows,GradMag.cols,CV_32FC1);
00082       Bins[n] = Mat::zeros(GradMag.rows,GradMag.cols,CV_32FC1);
00083     }
00084  for( int r = 0; r < div.rows; r++ )
00085     {
00086       for( int c = 0; c < div.cols; c++ )
00087       {
00088         
00089         angl = ((atan (div.at<float>(r,c))) * (180 / PI)) + 90;
00090         if (angl<=30)
00091         {
00092           Bins[0].at<float>(r,c) = GradMag.at<float>(r,c);
00093           
00094         }
00095         
00096         else if (angl<=60)
00097         {
00098           Bins[1].at<float>(r,c) = GradMag.at<float>(r,c);
00099           
00100         }
00101         else if (angl<=90)
00102         {
00103           Bins[2].at<float>(r,c) = GradMag.at<float>(r,c);
00104           
00105         }
00106         else if (angl<=120)
00107         {
00108           Bins[3].at<float>(r,c) = GradMag.at<float>(r,c);
00109           
00110         }
00111         
00112         else if (angl<=150)
00113         {
00114           Bins[4].at<float>(r,c) = GradMag.at<float>(r,c);
00115           
00116         }
00117         else
00118         {
00119           Bins[5].at<float>(r,c) = GradMag.at<float>(r,c);
00120           
00121         }
00122       }
00123     
00124     }
00125   
00126   for(int i=0; i<6; i++)
00127   {
00128     BinVector.push_back(Bins[i]);
00129     
00130   }
00131   
00132   return BinVector;
00133   
00134 }
00135 
00136 MatVector LUVcolourchannels(Mat Img)
00137 {
00138   Mat LUV(Img.rows, Img.cols, CV_32FC3);
00139   MatVector LUVchannels; 
00140   
00141   cvtColor(Img,LUV, CV_RGB2Luv,CV_32F);
00142   
00143   LUV.assignTo(LUV, CV_32F );
00144   
00145   split(LUV,LUVchannels);
00146   
00147   
00148   return LUVchannels;
00149   
00150 }
00151 
00152 
00153  void GetFileList(string folder_path, PVector & vect)
00154 {
00155   
00156   path p = folder_path;
00157   
00158   try
00159   {
00160     if (exists(p))    // does p actually exist?
00161     {
00162 
00163       if (is_directory(p))      // is p a directory?
00164       {
00165         
00166         copy(directory_iterator(p), directory_iterator(), back_inserter(vect)); 
00167         
00168       }
00169       else
00170       cout << p << " is not a folder"<<endl;
00171 
00172     }
00173    
00174   }
00175   
00176 
00177   catch (const filesystem_error& ex)
00178   {
00179     cout << ex.what() << '\n';
00180   }
00181   
00182   
00183 }
00184 
00185 
00186 void ComputeChannels(Mat Image, Mat & MergedChannels)
00187 {
00188   
00189   Mat xsobel, ysobel, GradMag, GrayImg;
00190   MatVector BinVect, LUVchannels;
00191   
00192   cvtColor (Image, GrayImg, CV_RGB2GRAY, 0);
00193   
00194   Sobel (GrayImg, xsobel, CV_32FC1, 1, 0, 3, 1, 0, BORDER_DEFAULT);
00195   Sobel (GrayImg, ysobel, CV_32FC1, 0, 1, 3, 1, 0, BORDER_DEFAULT);
00196   
00197   GradMag = GradientMagnitude (xsobel, ysobel);
00198   
00199   BinVect = OrientedGradientsDiagram (GradMag, xsobel, ysobel);
00200   
00201   LUVchannels = LUVcolourchannels (Image);
00202   
00203   
00204   Mat ToBeMergedChannels[] =
00205       { GradMag, BinVect[0], BinVect[1], BinVect[2], BinVect[3], BinVect[4],
00206       BinVect[5], LUVchannels[0], LUVchannels[1], LUVchannels[2]
00207     };
00208     
00209     
00210 
00211     merge (ToBeMergedChannels, CHANNELNR, MergedChannels);
00212 
00213 }
00214 
00215 
00216 void GetRandParams(int seed, int NrFtrs, FtrVecParams2 & randparams, Rect region)
00217 {
00218   
00219   cv::RNG rng(seed);
00220   
00221 //   vector <int> record;
00222 //   ifstream infile("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/PredictorImportance.txt");
00223 // 
00224 //   while (infile)
00225 //   {
00226 //     string s;
00227 //     if (!getline( infile, s )) break;
00228 // 
00229 //     istringstream ss( s );
00230 // 
00231 //     while (ss)
00232 //     {
00233 //       string s;
00234 //       if (!getline( ss, s, ',' )) break;
00235 //       record.push_back( atoi(s.c_str() ));
00236 //     }
00237 //     
00238 //   }
00239 //   
00240 //   if (!infile.eof())
00241 //   {
00242 //     cerr << "Fooey!\n";
00243 //   }
00244 //   
00245 
00246   
00247   FtrParams2 Params;
00248   Params.nFtrs=NRFEATURE2;
00249   
00250   for (int n=0; n<NrFtrs; n++)
00251   {
00252 //     Params.channelidx=rng.uniform(0,CHANNELNR);
00253     
00254 //     if(std::find(record.begin(), record.end(), n) != record.end())
00255 //       
00256 //     {
00257 //     
00258     Params.width=rng.uniform(MINAREA,MAXAREAWIDTH+1); 
00259     Params.height=rng.uniform(MINAREA,MAXAREAHEIGHT+1);
00260     
00261     Params.x=rng.uniform(0,region.width-Params.width+1);
00262     Params.y=rng.uniform(0,region.height-Params.height+1);
00263     
00264     randparams.push_back(Params);
00265 //     }
00266     
00267   }
00268   
00269 
00270   
00271 }
00272 
00273 void GetChnFtrsOverImagePyramid(Mat Image , CvRect & region , vector<float> & features,int nOctUp, Size minSize, int nPerOct, FtrVecParams2 Params,PedRoiVec & PedRect, CvBoost & boost_classifier)
00274 {
00275   
00276   
00277   Size currSize=Image.size();
00278   float scaledown = pow(2,(-1.0/nPerOct)), scaleup = pow(2,1.0/nPerOct);
00279   
00280   Mat currScaledImg=Image , IntegralChns;
00281   
00282 //   cout<<"currsize w: "<<currSize.width<<" minSize w: "<<minSize.width<<" currsize height: "<<currSize.height<< " minsize height: "<<minSize.height<<endl;
00283   
00284   while (currSize.width>=minSize.width && currSize.height>=minSize.height)
00285   {
00286     
00287     Mat MergedChannels (currScaledImg.rows, currScaledImg.cols, CV_32FC (CHANNELNR));
00288     
00289     ComputeChannels(currScaledImg,MergedChannels);
00290    
00291     integral (MergedChannels, IntegralChns, CV_32FC (CHANNELNR));
00292     GetChnFtrsOverImage (IntegralChns, region, features, Params,PedRect,boost_classifier);
00293     
00294     currSize.width *=scaledown; currSize.height *=scaledown; 
00295     resize(Image, currScaledImg, currSize, 0, 0, INTER_LINEAR);
00296     
00297   
00298   } 
00299   
00300   if (nOctUp !=0)
00301   {
00302     
00303     currSize = Image.size();
00304     Size maxSize;
00305     
00306     maxSize.width = currSize.width+currSize.width*0.5*nOctUp;
00307     maxSize.height = currSize.height+currSize.height*0.5*nOctUp;
00308     currScaledImg=Image;
00309     
00310     while (currSize.width<maxSize.width && currSize.height<maxSize.height)
00311   {
00312     currSize.width *=scaleup; currSize.height *=scaleup; 
00313     resize(Image, currScaledImg, currSize, 0, 0, INTER_LINEAR); 
00314     
00315     Mat MergedChannels (currScaledImg.rows, currScaledImg.cols, CV_32FC (CHANNELNR));
00316     
00317     ComputeChannels(currScaledImg,MergedChannels);
00318    
00319     integral (MergedChannels, IntegralChns, CV_32FC (CHANNELNR));
00320    
00321     GetChnFtrsOverImage (IntegralChns, region, features, Params,PedRect,boost_classifier);
00322   
00323   }
00324     
00325   }
00326   
00327   
00328 }
00329 
00330 void GetChnFtrsOverImagePyramid(Mat Image , CvRect & region , vector<float> & features,vector<DVector> &  WindowFtrs,int nOctUp, Size minSize, int nPerOct, FtrVecParams2 Params)
00331 {
00332   
00333   
00334   Size currSize=Image.size();
00335   float scaledown = pow(2,(-1.0/nPerOct)), scaleup = pow(2,1.0/nPerOct);
00336   
00337   Mat currScaledImg=Image , IntegralChns;
00338   
00339 //   cout<<"currsize w: "<<currSize.width<<" minSize w: "<<minSize.width<<" currsize height: "<<currSize.height<< " minsize height: "<<minSize.height<<endl;
00340   
00341   while (currSize.width>=minSize.width && currSize.height>=minSize.height)
00342   {
00343     
00344     Mat MergedChannels (currScaledImg.rows, currScaledImg.cols, CV_32FC (CHANNELNR));
00345     
00346     ComputeChannels(currScaledImg,MergedChannels);
00347    
00348     integral (MergedChannels, IntegralChns, CV_32FC (CHANNELNR));
00349     GetChnFtrsOverImage (IntegralChns, region, features,WindowFtrs, Params);
00350     
00351     currSize.width *=scaledown; currSize.height *=scaledown; 
00352     resize(Image, currScaledImg, currSize, 0, 0, INTER_LINEAR);
00353     
00354   
00355   } 
00356   
00357   if (nOctUp !=0)
00358   {
00359     
00360     currSize = Image.size();
00361     Size maxSize;
00362     
00363     maxSize.width = currSize.width+currSize.width*0.5*nOctUp;
00364     maxSize.height = currSize.height+currSize.height*0.5*nOctUp;
00365     currScaledImg=Image;
00366     
00367     while (currSize.width<maxSize.width && currSize.height<maxSize.height)
00368   {
00369     currSize.width *=scaleup; currSize.height *=scaleup; 
00370     resize(Image, currScaledImg, currSize, 0, 0, INTER_LINEAR); 
00371     
00372     Mat MergedChannels (currScaledImg.rows, currScaledImg.cols, CV_32FC (CHANNELNR));
00373     
00374     ComputeChannels(currScaledImg,MergedChannels);
00375    
00376     integral (MergedChannels, IntegralChns, CV_32FC (CHANNELNR));
00377    
00378     GetChnFtrsOverImage (IntegralChns, region, features, WindowFtrs ,Params);
00379   
00380   }
00381     
00382   }
00383   
00384   
00385 }
00386 
00387 
00388 void GetChnFtrsOverImage(Mat IntegralChannels , CvRect & region ,vector<float> & features , FtrVecParams2 Params,PedRoiVec & PedRect,CvBoost & boost_classifier)
00389 {
00390 
00391   int Rows=0, Cols=0;
00392 
00393   for (Rows=0; Rows<IntegralChannels.rows-region.height-1; Rows+=STEP)
00394   {
00395     
00396     for (Cols=0; Cols<IntegralChannels.cols-region.width-1; Cols+=STEP)
00397     {
00398       
00399       
00400       region.x=Cols ; region.y=Rows;
00401       GetChnFtrsOverWindow(IntegralChannels ,features,Params,region,PedRect,boost_classifier);
00402       
00403       
00404     }
00405 
00406     
00407   }
00408 
00409   
00410     
00411     
00412     for (Cols=0 ; Cols<IntegralChannels.cols-region.width-1; Cols+=STEP)
00413     {
00414       
00415     region.x=Cols; region.y=IntegralChannels.rows-region.height-1;
00416     GetChnFtrsOverWindow(IntegralChannels ,features ,Params,region,PedRect,boost_classifier);
00417     
00418     }
00419 
00420   
00421   
00422 
00423     for (Rows=0 ; Rows<IntegralChannels.rows-region.height-1; Rows+=STEP)
00424     {
00425       
00426     region.y=Rows; region.x=IntegralChannels.cols-region.width-1;
00427     GetChnFtrsOverWindow(IntegralChannels ,features,Params,region,PedRect,boost_classifier);
00428     
00429     
00430     
00431     }
00432     
00433     region.y=IntegralChannels.rows-region.height-1; region.x=IntegralChannels.cols-region.width-1;
00434     
00435     GetChnFtrsOverWindow(IntegralChannels ,features,Params,region,PedRect,boost_classifier);
00436     
00437     
00438   
00439 }
00440 
00441 void GetChnFtrsOverImage(Mat IntegralChannels , CvRect & region ,vector<float> & features ,vector<DVector> & WindowFtrs , FtrVecParams2 Params)
00442 {
00443 
00444   int Rows=0, Cols=0;
00445 
00446   for (Rows=0; Rows<IntegralChannels.rows-region.height-1; Rows+=STEP)
00447   {
00448     
00449     for (Cols=0; Cols<IntegralChannels.cols-region.width-1; Cols+=STEP)
00450     {
00451       
00452       
00453       region.x=Cols ; region.y=Rows;
00454       GetChnFtrsOverWindow(IntegralChannels ,features,WindowFtrs,Params,region);
00455       
00456       
00457     }
00458 
00459     
00460   }
00461 
00462   
00463     
00464     
00465     for (Cols=0 ; Cols<IntegralChannels.cols-region.width-1; Cols+=STEP)
00466     {
00467       
00468     region.x=Cols; region.y=IntegralChannels.rows-region.height-1;
00469     GetChnFtrsOverWindow(IntegralChannels ,features,WindowFtrs ,Params,region);
00470     
00471     }
00472 
00473   
00474   
00475 
00476     for (Rows=0 ; Rows<IntegralChannels.rows-region.height-1; Rows+=STEP)
00477     {
00478       
00479     region.y=Rows; region.x=IntegralChannels.cols-region.width-1;
00480     GetChnFtrsOverWindow(IntegralChannels ,features,WindowFtrs,Params,region);
00481     
00482     
00483     }
00484     
00485     region.y=IntegralChannels.rows-region.height-1; region.x=IntegralChannels.cols-region.width-1;
00486 
00487     GetChnFtrsOverWindow(IntegralChannels ,features,WindowFtrs,Params,region);
00488 
00489     
00490   
00491 }
00492 
00493 void GetIntegralSum(Mat IntegralChannels, vector<float> & features,FtrParams2 Params, CvRect region)
00494 {
00495   
00496   vec10d tl, tr, bl, br , sum;
00497   int sR=Params.y+region.y, sC=Params.x+region.x, eR=sR+Params.height, eC=sC+Params.width;
00498   
00499   tl= IntegralChannels.at<vec10d>(sR,sC);
00500   tr= IntegralChannels.at<vec10d>(sR,eC);
00501   bl= IntegralChannels.at<vec10d>(eR,sC);
00502   br= IntegralChannels.at<vec10d>(eR,eC);
00503   
00504   
00505   sum=br-bl-tr+tl;
00506   
00507   for (int n=0;n<CHANNELNR;n++)
00508   features.push_back(sum[n]);  
00509   
00510 }
00511 
00512 
00513 
00514 void GetChnFtrsOverWindow(Mat IntegralChannels , vector<float> & features ,FtrVecParams2 Params, CvRect region , PedRoiVec & PedRect,CvBoost & boost_classifier)
00515 {
00516   
00517 
00518   Mat Test;
00519   Test=Mat::zeros(1,NRFEATURE,CV_32FC1);
00520   PedRoi ROI;
00521 //   vector<float> aux;
00522   
00523   
00524   for (int n=0; n< Params[0].nFtrs ; n++)
00525   {
00526    
00527   GetIntegralSum(IntegralChannels,features ,Params[n],region);
00528   
00529 
00530   }
00531   
00532     
00533     
00534   for(int n=0; n<NRFEATURE; n++)
00535   {
00536    Test.at<float>(0,n)=features[features.size()-NRFEATURE+n];
00537 //    aux.push_back(features[features.size()-NRFEATURE+n]);
00538    
00539   }
00540 //    CvMat TestMat = Test;
00541 //    CvMat * weak_responses = cvCreateMat(1,Test.cols,CV_32F);
00542 
00543   float x = boost_classifier.predict(Test,Mat(),Range::all(),false,true);
00544 //    float x = boost_classifier.predict(&TestMat,0,weak_responses,CV_WHOLE_SEQ,0,1);
00545    
00546 //    Mat WR(weak_responses,false);
00547    
00548 //    cout<<x<<endl;
00549    
00550   if (x<=THRESHOLD) 
00551   {
00552     ROI.x=region.x;
00553     ROI.y=region.y;
00554     ROI.Scale=IntegralChannels.size();
00555     PedRect.push_back(ROI);
00556  
00557     // UNCOMMENT BELLOW TO ENABLE BOOTSTRAPPING --> BE SURE TO CHANGE THE FILE WHERE YOU WANT DATA TO BE SAVED //
00558     
00559 //     fstream outfile;
00560 //     
00561 //     outfile.open ("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/train_15000_boot3.csv", fstream::in | fstream::out | fstream::app);
00562 //     
00563 //     outfile<<"2,";
00564 //     
00565 //     for (int i=0;i<NRFEATURE;i++)
00566 //     {
00567 //       if (i != NRFEATURE-1)
00568 //       {
00569 //       outfile<<Test.at<float>(0,i)<<",";
00570 //       }
00571 //       else
00572 //       {
00573 //      outfile<<Test.at<float>(0,i);
00574 //       }
00575 //     }
00576 //     
00577 //     outfile<<endl;
00578     }
00579     
00580   
00581 }
00582 
00583 void GetChnFtrsOverWindow(Mat IntegralChannels , vector<float> & features,vector<DVector> & WindowFtrs ,FtrVecParams2 Params, CvRect region)
00584 {
00585   vector<float> aux;
00586  
00587   for (int n=0; n< Params[0].nFtrs ; n++)
00588   {
00589    
00590   GetIntegralSum(IntegralChannels,features ,Params[n],region);
00591     
00592   }
00593   
00594   for(int n=0; n<NRFEATURE; n++)
00595   {
00596    aux.push_back(features[features.size()-NRFEATURE+n]);
00597   }
00598   
00599   WindowFtrs.push_back(aux);
00600   
00601 
00602   
00603   
00604 }
00605 
00606 void PostProcess(Mat Img, vector<Rect> PedRect, FtrVecParams2 randparams, CvBoost & boost_classifier, PedRoiVec & PedRect_Post)
00607 {
00608   // MAKE POST PROCESSING 
00609   
00610   
00611   
00612 }
00613