$search
00001 00002 00003 00004 00005 00006 00007 #ifdef __LINUX__ 00008 #include "cob_people_detection/PeopleDetector.h" 00009 #include "cob_people_detection/PeopleDetectorControlFlow.h" 00010 #else 00011 #include "cob_vision/cob_people_detection/common/include/cob_people_detection/PeopleDetector.h" 00012 #include "cob_vision/cob_people_detection/common/include/cob_people_detection/PeopleDetectorControlFlow.h" 00013 #endif 00014 00015 #include <iostream> 00016 #include <string> 00017 #include <fstream> 00018 #include <sstream> 00019 00020 00021 template<typename T> inline T abs_(T x) 00022 { 00023 return (x<0) ? -x : x; 00024 } 00025 00032 bool checkRectangleCorrespondence(cv::Rect& gt, cv::Rect& measurement, double areaRatioThreshold) 00033 { 00034 // overlapping area 00035 double dx = (min(gt.x+gt.width, measurement.x+measurement.width) - max(gt.x, measurement.x)); 00036 double dy = (min(gt.y+gt.height, measurement.y+measurement.height) - max(gt.y, measurement.y)); 00037 double overlapArea = dx * dy; 00038 00039 // check area ratios 00040 double gtArea = gt.width*gt.height; 00041 double measurementArea = measurement.width*measurement.height; 00042 //std::cout << overlapArea/gtArea << " " << overlapArea/measurementArea << "\n"; 00043 00044 if (dx<0 || dy<0 || (overlapArea/gtArea < areaRatioThreshold) || (overlapArea/measurementArea < areaRatioThreshold)) 00045 return false; 00046 00047 return true; 00048 } 00049 00050 00051 int main(int argc, char *argv[]) 00052 { 00053 if (argc != 4) 00054 { 00055 std::cout << "Usage of this tool:\nHaardetectorUnitTest <test_data_path> <detector_mode> <detector_path>\n"; 00056 std::cout << " test_data_path\t=\tPath to the 'testData.txt' file with the ground truth (each line contains the filename of the respective image followed by the number of contained faces followed by the coordinates of these faces in the style x y width height, use spaces as separator)\n"; 00057 std::cout << " detector_mode\t=\tswitches between range image detector (put a 0 as argument) and color image detector (put a 1 as argument)\n\n"; 00058 std::cout << " detector_path\t=\tthe path to the range and color image haarcascades (the cacscades should be placed in a folder haarcascades/ , provide the path to that folder)\n\n"; 00059 std::cout << "Example: HarrdetectorUnitTest ConfigurationFiles/TestData/ 0 ConfigurationFiles/\n\n"; 00060 return ipa_Utils::RET_OK; 00061 } 00062 00063 // detector mode (0=range, 1=color images) 00064 std::stringstream ss; 00065 ss << argv[2]; 00066 int detectorMode; 00067 ss >> detectorMode; 00068 00069 // initialize the people detector 00070 ipa_PeopleDetector::PeopleDetectorControlFlow peopleDetectorControlFlow; 00071 ipa_PeopleDetector::PeopleDetector peopleDetector; 00072 std::string directory = argv[3]; 00073 std::string iniFileNameAndPath = directory + "peopleDetectorIni.xml"; 00074 if (peopleDetector.Init(directory) & ipa_Utils::RET_FAILED) 00075 { 00076 std::cerr << "ERROR - HaardetectorUnitTest:" << std::endl; 00077 std::cerr << "\t ... Could not initialize people detector library.\n"; 00078 return ipa_Utils::RET_FAILED; 00079 } 00080 peopleDetectorControlFlow.m_PeopleDetector = &peopleDetector; 00081 if(peopleDetectorControlFlow.LoadParameters(iniFileNameAndPath.c_str()) & ipa_CameraSensors::RET_FAILED) 00082 { 00083 std::cerr << "ERROR - HaardetectorUnitTest:" << std::endl; 00084 std::cerr << "\t ... Error while loading configuration file '" << std::endl; 00085 std::cerr << "\t ... " << iniFileNameAndPath << "'.\n"; 00086 return ipa_Utils::RET_FAILED; 00087 } 00088 00089 // try different parameter settings 00090 peopleDetector.m_range_drop_groups; 00091 #ifndef __DEBUG__ 00092 int* parameter = 0; 00093 if (detectorMode == 0) 00094 parameter = &(peopleDetector.m_range_drop_groups); // range mode 00095 else 00096 parameter = &(peopleDetector.m_faces_drop_groups); // color mode 00097 for (*parameter = 20; *parameter < 80; *parameter += 2) 00098 { 00099 if (detectorMode == 0) 00100 std::cout << "\n\n\n---------------------------------------------------\nparam peopleDetector.m_range_drop_groups = " << peopleDetector.m_range_drop_groups << "\n"; 00101 else 00102 std::cout << "\n\n\n---------------------------------------------------\nparam peopleDetector.m_faces_drop_groups = " << peopleDetector.m_faces_drop_groups << "\n"; 00103 #endif 00104 00105 // file containing the ground truth 00106 std::string testDataDirectory = argv[1]; 00107 std::string filename = testDataDirectory + "testData.txt"; 00108 00109 // open the ground truth file 00110 std::fstream fin(filename.c_str(), std::fstream::in); 00111 if (!fin.is_open()) 00112 { 00113 std::cerr << "ERROR - HaardetectorUnitTest:\n"; 00114 std::cerr << "\t ... Could not open the ground truth file " << filename << ".\n"; 00115 return ipa_Utils::RET_FAILED; 00116 } 00117 00118 // statistics 00119 enum {TRUE_POS=0, FALSE_NEG, FALSE_POS, TRUE_NEG}; 00120 std::vector<int> stats(4, 0); 00121 int numberNegativeImages = 0; // number of negative images (without any face) in the test set 00122 int numberGroundtruthFaces = 0; // total number of faces in the whole test set 00123 00124 // open each image of the ground truth, run the detector and examine the detection results 00125 while (!fin.eof()) 00126 { 00127 // filename of the image, load image 00128 std::string imageFilename; 00129 fin >> imageFilename; 00130 if (imageFilename=="") break; 00131 imageFilename = testDataDirectory + imageFilename; 00132 cv::Mat img = cv::imread(imageFilename, -1); 00133 00134 //for (int v=0; v<img.rows; v++) 00135 //{ 00136 // for (int u=0; u<img.cols; u++) 00137 // { 00138 // if (img.at<uchar>(v,u) == 0) img.at<uchar>(v,u) = 255; 00139 // } 00140 //} 00141 00142 #ifdef __DEBUG__ 00143 cv::Mat dispImg; 00144 cv::cvtColor(img, dispImg, CV_GRAY2BGR, 3); 00145 #endif 00146 00147 // the ratio of overlapping area to ground truth area or to detected area must not fall below this threshold otherwise it is no detection 00148 const double maxFaceDistance = 0.6; 00149 00150 // number of contained faces 00151 int numFaces = 0; 00152 fin >> numFaces; 00153 numberGroundtruthFaces += numFaces; 00154 if (numFaces==0) numberNegativeImages++; 00155 00156 // detect faces 00157 std::vector<cv::Rect> faces; 00158 if (detectorMode==0) 00159 peopleDetector.DetectRangeFace(img, faces, false); // range images 00160 else 00161 peopleDetector.DetectColorFaces(img, faces); // color images 00162 00163 // check for each ground truth face whether it was found by the detector 00164 std::vector<bool> truePositiveDetection(faces.size(), false); // stores when a detected face turns out to be a true positive 00165 for (int gtfaceIndex=0; gtfaceIndex<numFaces; gtfaceIndex++) 00166 { 00167 // read the ground truth faces from file 00168 cv::Rect faceGt; 00169 fin >> faceGt.x >> faceGt.y >> faceGt.width >> faceGt.height; 00170 00171 #ifdef __DEBUG__ 00172 cv::rectangle(dispImg, faceGt, CV_RGB(0,255,0), 2); 00173 #endif 00174 00175 // compare with each detected face 00176 bool truePositiveFound = false; 00177 for (int faceIndex=0; faceIndex<(int)faces.size(); faceIndex++) 00178 { 00179 if (checkRectangleCorrespondence(faceGt, faces[faceIndex], maxFaceDistance) == true) 00180 { 00181 // both rectangles correspond 00182 if (truePositiveFound == false) stats[TRUE_POS]++; 00183 truePositiveDetection[faceIndex] = true; 00184 truePositiveFound = true; 00185 } 00186 } 00187 00188 // complete statistics - could the ground truth be detected in the image 00189 if (truePositiveFound == false) 00190 stats[FALSE_NEG]++; 00191 } 00192 00193 // complete statistics - check for false positives 00194 for (int faceIndex=0; faceIndex<(int)faces.size(); faceIndex++) 00195 if (truePositiveDetection[faceIndex]==false) 00196 stats[FALSE_POS]++; 00197 00198 // complete statistics - check for true negative (no face in ground truth and no detections) 00199 if (numFaces==0 && faces.size()==0) 00200 stats[TRUE_NEG]++; 00201 00202 #ifdef __DEBUG__ 00203 std::cout << stats[TRUE_POS] << "\t" << stats[FALSE_NEG] << "\t" << stats[FALSE_POS] << "\t" << stats[TRUE_NEG] << "\t\t" << numberGroundtruthFaces << "\t" << numberNegativeImages << "\n"; 00204 for (int faceIndex=0; faceIndex<(int)faces.size(); faceIndex++) 00205 cv::rectangle(dispImg, faces[faceIndex], CV_RGB(0,0,255), 2); 00206 cv::imshow("Groundtruth (green) vs detections (blue)", dispImg); 00207 cv::waitKey(); 00208 #endif 00209 } 00210 fin.close(); 00211 00212 // output results 00213 std::cout << "-----------\nStatistics:\n-----------\nTruePos\tFalseNeg\tFalsePos\tTrueNeg\t\tNumFaces\tNumNegatives\n"; 00214 std::cout << stats[TRUE_POS] << "\t" << stats[FALSE_NEG] << "\t\t" << stats[FALSE_POS] << "\t\t" << stats[TRUE_NEG] << "\t\t" << numberGroundtruthFaces << "\t\t" << numberNegativeImages << "\n"; 00215 std::cout << "\nPositive detection rate (#truePositives/#NumFaces): " << (double)stats[TRUE_POS]/(double)numberGroundtruthFaces << "\n"; 00216 std::cout << "\nFalse positive rate (#falsePositives/#NumFaces): " << (double)stats[FALSE_POS]/(double)numberGroundtruthFaces << "\n"; 00217 std::cout << "\nNegative detection rate (#trueNegatives/#NumNegatives): " << (double)stats[TRUE_NEG]/(double)numberNegativeImages << "\n"; 00218 #ifndef __DEBUG__ 00219 } 00220 #endif 00221 00222 getchar(); 00223 00224 return ipa_Utils::RET_OK; 00225 }