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
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
00040 double gtArea = gt.width*gt.height;
00041 double measurementArea = measurement.width*measurement.height;
00042
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
00064 std::stringstream ss;
00065 ss << argv[2];
00066 int detectorMode;
00067 ss >> detectorMode;
00068
00069
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
00090 peopleDetector.m_range_drop_groups;
00091 #ifndef __DEBUG__
00092 int* parameter = 0;
00093 if (detectorMode == 0)
00094 parameter = &(peopleDetector.m_range_drop_groups);
00095 else
00096 parameter = &(peopleDetector.m_faces_drop_groups);
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
00106 std::string testDataDirectory = argv[1];
00107 std::string filename = testDataDirectory + "testData.txt";
00108
00109
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
00119 enum {TRUE_POS=0, FALSE_NEG, FALSE_POS, TRUE_NEG};
00120 std::vector<int> stats(4, 0);
00121 int numberNegativeImages = 0;
00122 int numberGroundtruthFaces = 0;
00123
00124
00125 while (!fin.eof())
00126 {
00127
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
00135
00136
00137
00138
00139
00140
00141
00142 #ifdef __DEBUG__
00143 cv::Mat dispImg;
00144 cv::cvtColor(img, dispImg, CV_GRAY2BGR, 3);
00145 #endif
00146
00147
00148 const double maxFaceDistance = 0.6;
00149
00150
00151 int numFaces = 0;
00152 fin >> numFaces;
00153 numberGroundtruthFaces += numFaces;
00154 if (numFaces==0) numberNegativeImages++;
00155
00156
00157 std::vector<cv::Rect> faces;
00158 if (detectorMode==0)
00159 peopleDetector.DetectRangeFace(img, faces, false);
00160 else
00161 peopleDetector.DetectColorFaces(img, faces);
00162
00163
00164 std::vector<bool> truePositiveDetection(faces.size(), false);
00165 for (int gtfaceIndex=0; gtfaceIndex<numFaces; gtfaceIndex++)
00166 {
00167
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
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
00182 if (truePositiveFound == false) stats[TRUE_POS]++;
00183 truePositiveDetection[faceIndex] = true;
00184 truePositiveFound = true;
00185 }
00186 }
00187
00188
00189 if (truePositiveFound == false)
00190 stats[FALSE_NEG]++;
00191 }
00192
00193
00194 for (int faceIndex=0; faceIndex<(int)faces.size(); faceIndex++)
00195 if (truePositiveDetection[faceIndex]==false)
00196 stats[FALSE_POS]++;
00197
00198
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
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 }