$search
00001 00002 00003 00004 #ifdef __LINUX__ 00005 #include "cob_people_detection/PeopleDetectorControlFlow.h" 00006 #else 00007 #include "cob_vision/cob_people_detection/common/include/cob_people_detection/PeopleDetectorControlFlow.h" 00008 #endif 00009 00010 00011 using namespace ipa_PeopleDetector; 00012 00013 PeopleDetectorControlFlow::PeopleDetectorControlFlow(void) 00014 { 00015 m_PeopleDetector = 0; 00016 m_filname = 0; 00017 } 00018 00019 PeopleDetectorControlFlow::~PeopleDetectorControlFlow(void) 00020 { 00021 } 00022 00023 unsigned long PeopleDetectorControlFlow::Init(std::string directory, 00024 ipa_CameraSensors::AbstractColorCameraPtr* colorCamera0, 00025 ipa_CameraSensors::AbstractColorCameraPtr* colorCamera1, 00026 ipa_CameraSensors::AbstractRangeImagingSensorPtr* rangeImagingSensor) 00027 { 00028 std::string iniFileNameAndPath = directory + "peopleDetectorIni.xml"; 00029 00030 if (CameraSensorsControlFlow::Init(directory, "peopleDetectorIni.xml", colorCamera0, colorCamera1, rangeImagingSensor) & ipa_Utils::RET_FAILED) 00031 { 00032 std::cerr << "ERROR - CameraDataViewerControlFlow::Init:" << std::endl; 00033 std::cerr << "\t ... Could not initialize 'CameraSensorsControlFlow'" << std::endl; 00034 return ipa_Utils::RET_FAILED; 00035 } 00036 00037 m_PeopleDetector = new ipa_PeopleDetector::PeopleDetector(); 00038 00039 if (m_PeopleDetector->Init(directory) & ipa_Utils::RET_FAILED) 00040 { 00041 std::cerr << "ERROR - PeopleDetectorControlFlow::Init:" << std::endl; 00042 std::cerr << "\t ... Could not initialize people detector library.\n"; 00043 return ipa_Utils::RET_FAILED; 00044 } 00045 00046 if(LoadParameters(iniFileNameAndPath.c_str()) & ipa_CameraSensors::RET_FAILED) 00047 { 00048 std::cerr << "ERROR - PeopleDetector::Init:" << std::endl; 00049 std::cerr << "\t ... Error while loading configuration file '" << std::endl; 00050 std::cerr << "\t ... " << iniFileNameAndPath << "'.\n"; 00051 return ipa_Utils::RET_FAILED; 00052 } 00053 00054 m_runPCA = false; 00055 00056 return ipa_Utils::RET_OK; 00057 } 00058 00059 unsigned long PeopleDetectorControlFlow::DetectFaces(ipa_SensorFusion::ColoredPointCloudPtr pc) 00060 { 00061 cv::Mat xyzImage_8U3; 00062 ipa_Utils::ConvertToShowImage(pc->GetXYZImage(), xyzImage_8U3, 3); 00063 00064 std::set<size_t> colorToRangeDependency; 00065 if (m_PeopleDetector->DetectFaces(pc->GetColorImage(), xyzImage_8U3, m_colorFaces, m_rangeFaces, colorToRangeDependency, false/*(m_RangeImagingCameraType==ipa_CameraSensors::CAM_KINECT)*/) & ipa_Utils::RET_FAILED) 00066 { 00067 std::cerr << "ERROR - PeopleDetectorControlFlow::DetectFaces" << std::endl; 00068 std::cerr << "\t ... Could not detect faces.\n"; 00069 return ipa_Utils::RET_FAILED; 00070 } 00071 00072 return ipa_Utils::RET_OK; 00073 } 00074 00075 unsigned long PeopleDetectorControlFlow::AddFace(cv::Mat& image, std::string id) 00076 { 00077 // addFace should only be called if there is exactly one face found --> so we access it with m_colorFaces[0] 00078 if (m_PeopleDetector->AddFace(image, m_colorFaces[0], id, m_faceImages, m_id) & ipa_Utils::RET_FAILED) 00079 { 00080 std::cerr << "ERROR - PeopleDetectorControlFlow::AddFace:" << std::endl; 00081 std::cerr << "\t ... Could not save face.\n"; 00082 return ipa_Utils::RET_FAILED; 00083 } 00084 m_runPCA = true; 00085 00086 return ipa_Utils::RET_OK; 00087 } 00088 00089 unsigned long PeopleDetectorControlFlow::PCA() 00090 { 00091 // only run PCA if new data has arrived after last computation 00092 if(!m_runPCA) 00093 { 00094 std::cout << "INFO - PeopleDetectorControlFlow::PCA:" << std::endl; 00095 std::cout << "\t ... PCA algorithm skipped.\n"; 00096 return ipa_Utils::RET_OK; 00097 } 00098 00099 if(m_faceImages.size() < 2) 00100 { 00101 std::cout << "WARNING - PeopleDetector::ConsoleGUI:" << std::endl; 00102 std::cout << "\t ... Less than two images are trained.\n"; 00103 return ipa_Utils::RET_OK; 00104 } 00105 00106 // Delete memory 00107 m_eigenVectors.clear(); 00108 00109 // Do PCA 00110 if (m_PeopleDetector->PCA(&m_nEigens, m_eigenVectors, m_eigenValMat, m_avgImage, m_faceImages, m_projectedTrainFaceMat) & ipa_Utils::RET_FAILED) 00111 { 00112 std::cerr << "ERROR - PeopleDetectorControlFlow::PCA:" << std::endl; 00113 std::cerr << "\t ... Error while PCA.\n"; 00114 return ipa_Utils::RET_FAILED; 00115 } 00116 00117 // Calculate FaceClasses 00118 if (m_PeopleDetector->CalculateFaceClasses(m_projectedTrainFaceMat, m_id, &m_nEigens, m_faceClassAvgProjections, m_idUnique) & ipa_Utils::RET_FAILED) 00119 { 00120 std::cerr << "ERROR - PeopleDetectorControlFlow::PCA:" << std::endl; 00121 std::cerr << "\t ... Error while calculating FaceClasses.\n"; 00122 return ipa_Utils::RET_FAILED; 00123 } 00124 00125 m_runPCA = false; 00126 00127 return ipa_Utils::RET_OK; 00128 } 00129 00130 unsigned long PeopleDetectorControlFlow::RecognizeFace(ipa_SensorFusion::ColoredPointCloudPtr pc, std::vector<int>& index) 00131 { 00132 if (m_PeopleDetector->RecognizeFace(pc->GetColorImage(), m_colorFaces, &m_nEigens, m_eigenVectors, m_avgImage, m_faceClassAvgProjections, index, &m_threshold, &m_threshold_FS, m_eigenValMat) & ipa_Utils::RET_FAILED) 00133 { 00134 std::cerr << "ERROR - PeopleDetectorControlFlow::RecognizeFace:" << std::endl; 00135 std::cerr << "\t ... Error while recognizing faces.\n"; 00136 return ipa_Utils::RET_FAILED; 00137 } 00138 00139 return ipa_Utils::RET_OK; 00140 } 00141 00142 unsigned long PeopleDetectorControlFlow::SaveTrainingData() 00143 { 00144 std::string path = "ConfigurationFiles/TrainingData/"; 00145 std::string filename = "data.xml"; 00146 std::string img_ext = ".bmp"; 00147 00148 std::ostringstream complete; 00149 complete << path << filename; 00150 00151 // try 00152 // { 00153 // fs::remove_all(path.c_str()); 00154 // fs::create_directory(path.c_str()); 00155 // } 00156 // catch (const std::exception &ex) 00157 // { 00158 // std::cerr << "ERROR - PeopleDetectorControlFlow::SaveTrainingData():" << std::endl; 00159 // std::cerr << "\t ... Exception catch of '" << ex.what() << "'" << std::endl; 00160 // } 00161 00162 cv::FileStorage fileStorage(complete.str().c_str(), cv::FileStorage::WRITE); 00163 if(!fileStorage.isOpened()) 00164 { 00165 std::cout << "WARNING - PeopleDetectorControlFlow::SaveTrainingData:" << std::endl; 00166 std::cout << "\t ... Can't save training data.\n"; 00167 return ipa_Utils::RET_OK; 00168 } 00169 00170 // Ids 00171 fileStorage << "id_num" << (int)m_id.size(); 00172 for(int i=0; i<(int)m_id.size(); i++) 00173 { 00174 std::ostringstream tag; 00175 tag << "id_" << i; 00176 fileStorage << tag.str().c_str() << m_id[i].c_str(); 00177 } 00178 00179 // Face images 00180 fileStorage << "faces_num" << (int)m_faceImages.size(); 00181 for(int i=0; i<(int)m_faceImages.size(); i++) 00182 { 00183 std::ostringstream img; 00184 img << path << i << img_ext; 00185 std::ostringstream tag; 00186 tag << "img_" << i; 00187 fileStorage << tag.str().c_str() << img.str().c_str(); 00188 //cvSaveImage(img.str().c_str(), &m_faceImages[i]); 00189 cv::imwrite(img.str().c_str(), m_faceImages[i]); 00190 } 00191 00192 // Number eigenvalues/eigenvectors 00193 fileStorage << "eigens_num" << m_nEigens; 00194 00195 // Eigenvectors 00196 for (int i=0; i<m_nEigens; i++) 00197 { 00198 std::ostringstream tag; 00199 tag << "ev_" << i; 00200 fileStorage << tag.str().c_str() << m_eigenVectors[i]; 00201 } 00202 00203 // Eigenvalue matrix 00204 fileStorage << "eigen_val_mat" << m_eigenValMat; 00205 00206 // Average image 00207 fileStorage << "avg_image" << m_avgImage; 00208 00209 // Projections of the training faces 00210 fileStorage << "projected_train_face_mat" << m_projectedTrainFaceMat; 00211 00212 // Unique Ids (m_idUnique[i] stores the corresponding id to the average face coordinates in the face subspace in m_faceClassAvgProjections.row(i)) 00213 fileStorage << "id_unique_num" << (int)m_idUnique.size(); 00214 for(int i=0; i<(int)m_idUnique.size(); i++) 00215 { 00216 std::ostringstream tag; 00217 tag << "id_unique_" << i; 00218 fileStorage << tag.str().c_str() << m_idUnique[i].c_str(); 00219 } 00220 00221 // The average factors of the eigenvector decomposition from each face class 00222 fileStorage << "face_class_avg_projections" << m_faceClassAvgProjections; 00223 00224 fileStorage.release(); 00225 00226 std::cout << "INFO - PeopleDetectorControlFlow::SaveTrainingData:" << std::endl; 00227 std::cout << "\t ... " << m_faceImages.size() << " images saved.\n"; 00228 return ipa_Utils::RET_OK; 00229 } 00230 00231 unsigned long PeopleDetectorControlFlow::LoadTrainingData() 00232 { 00233 std::string path = "ConfigurationFiles/TrainingData/"; 00234 std::string filename = "data.xml"; 00235 00236 std::ostringstream complete; 00237 complete << path << filename; 00238 00239 if(fs::is_directory(path.c_str())) 00240 { 00241 cv::FileStorage fileStorage(complete.str().c_str(), cv::FileStorage::READ); 00242 if(!fileStorage.isOpened()) 00243 { 00244 std::cout << "WARNING - PeopleDetectorControlFlow::LoadTrainingData:" << std::endl; 00245 std::cout << "\t ... Cant open " << complete.str() << ".\n"; 00246 return ipa_Utils::RET_OK; 00247 } 00248 00249 // Ids 00250 m_id.clear(); 00251 int id_num = (int)fileStorage["id_num"]; 00252 for(int i=0; i<id_num; i++) 00253 { 00254 std::ostringstream tag; 00255 tag << "id_" << i; 00256 m_id.push_back((std::string)fileStorage[tag.str().c_str()]); 00257 } 00258 00259 // Images 00260 m_faceImages.clear(); 00261 int faces_num = (int)fileStorage["faces_num"]; 00262 for(int i=0; i<faces_num; i++) 00263 { 00264 std::ostringstream tag; 00265 tag << "img_" << i; 00266 std::string path = (std::string)fileStorage[tag.str().c_str()]; 00267 //IplImage *tmp = cvLoadImage(path.c_str(),0); 00268 cv::Mat temp = cv::imread(path.c_str(),0); 00269 m_faceImages.push_back(temp); 00270 } 00271 00272 // Number eigenvalues/eigenvectors 00273 m_nEigens = (int)fileStorage["eigens_num"]; 00274 00275 // Eigenvectors 00276 m_eigenVectors.clear(); 00277 m_eigenVectors.resize(m_nEigens, cv::Mat()); 00278 for (int i=0; i<m_nEigens; i++) 00279 { 00280 std::ostringstream tag; 00281 tag << "ev_" << i; 00282 fileStorage[tag.str().c_str()] >> m_eigenVectors[i]; 00283 } 00284 00285 // Eigenvalue matrix 00286 m_eigenValMat = cv::Mat(); 00287 fileStorage["eigen_val_mat"] >> m_eigenValMat; 00288 00289 // Average image 00290 m_avgImage = cv::Mat(); 00291 fileStorage["avg_image"] >> m_avgImage; 00292 00293 // Projections of the training faces 00294 m_projectedTrainFaceMat = cv::Mat(); 00295 fileStorage["projected_train_face_mat"] >> m_projectedTrainFaceMat; 00296 00297 // Unique Ids (m_idUnique[i] stores the corresponding id to the average face coordinates in the face subspace in m_faceClassAvgProjections.row(i)) 00298 m_idUnique.clear(); 00299 int idUniqueNum = (int)fileStorage["id_unique_num"]; 00300 for(int i=0; i<idUniqueNum; i++) 00301 { 00302 std::ostringstream tag; 00303 tag << "id_unique_" << i; 00304 m_idUnique.push_back((std::string)fileStorage[tag.str().c_str()]); 00305 } 00306 00307 // The average factors of the eigenvector decomposition from each face class 00308 m_faceClassAvgProjections = cv::Mat(); 00309 fileStorage["face_class_avg_projections"] >> m_faceClassAvgProjections; 00310 00311 fileStorage.release(); 00312 00313 // Run PCA 00314 // m_runPCA = true; 00315 // PCA(); 00316 00317 std::cout << "INFO - PeopleDetectorControlFlow::LoadTrainingData:" << std::endl; 00318 std::cout << "\t ... " << faces_num << " images loaded.\n"; 00319 00320 //cvReleaseFileStorage(&fileStorage); 00321 } 00322 else 00323 { 00324 std::cerr << "ERROR - PeopleDetectorControlFlow::LoadTrainingData():" << std::endl; 00325 std::cerr << "\t .... Path '" << path << "' is not a directory." << std::endl; 00326 return ipa_CameraSensors::RET_FAILED; 00327 } 00328 00329 return ipa_Utils::RET_OK; 00330 } 00331 00332 00333 unsigned long PeopleDetectorControlFlow::GetEigenface(cv::Mat& eigenface, int index) 00334 { 00335 //eigenface.create(100, 100, CV_8UC1); 00336 cv::normalize(m_eigenVectors[index], eigenface, 0, 255, cv::NORM_MINMAX, CV_8UC1); 00337 00338 00339 // Get the Eigenface Raw data 00340 // float *Image; 00341 // cvGetImageRawData(m_eigenVectArr[index], (uchar**)&Image); 00342 // 00343 // // Create a new 8 bit single channel image and get the raw data 00344 // *eigenface = cvCreateImage( cvSize( 100, 100), IPL_DEPTH_8U, 1); 00345 // uchar *fImg; 00346 // cvGetImageRawData( *eigenface, (uchar**)&fImg); 00347 // 00348 // // Find the Maximum and Minimum of the pixel values of the eigenfaces 00349 // float max, min; 00350 // max = min = 0.0; 00351 // for(int i=0; i< (*eigenface)->width * (*eigenface)->height; i++) 00352 // { 00353 // if(max < Image[i]) 00354 // max = Image[i]; 00355 // if(min > Image[i]) 00356 // min = Image[i]; 00357 // } 00358 // 00359 // /// Normalize the eigenface values between 0 and 255 00360 // for(int i = 0; i< (*eigenface)->width * (*eigenface)->height; i++) 00361 // { 00362 // fImg[i] = (uchar)(( 255 * (( Image[i] - min)/ (max- min)) )); 00363 // } 00364 00365 return ipa_Utils::RET_OK; 00366 } 00367 00368 unsigned long PeopleDetectorControlFlow::ShowAVGImage(cv::Mat& avgImage) 00369 { 00370 if(!m_runPCA && m_faceImages.size()<2) 00371 { 00372 std::cerr << "PeopleDetector::showAvgImage()" << std::endl; 00373 std::cerr << "No AVG image calculated" << std::endl; 00374 return 0; 00375 } 00376 00377 cv::normalize(m_avgImage, avgImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); 00378 00379 00380 // Get the AVG Raw data 00381 // float *Image; 00382 // cvGetImageRawData(m_avgImage, (uchar**)&Image); 00383 // 00384 // // Get the raw data 00385 // uchar *fImg; 00386 // cvGetImageRawData( avgImage, (uchar**)&fImg); 00387 // 00388 // // Find the Maximum and Minimum of the pixel values of the avg image 00389 // float max, min; 00390 // max = min = 0.0; 00391 // for(int i=0; i< avgImage->width * avgImage->height; i++) 00392 // { 00393 // if(max < Image[i]) 00394 // { 00395 // max = Image[i]; 00396 // } 00397 // if(min > Image[i]) 00398 // { 00399 // min = Image[i]; 00400 // } 00401 // } 00402 // 00403 // // Normalize the avg values between 0 and 255 00404 // for(int i = 0; i< avgImage->width * avgImage->height; i++) 00405 // { 00406 // fImg[i] = (uchar)(( 255 * (( Image[i] - min)/ (max- min)) )); 00407 // } 00408 00409 return ipa_Utils::RET_OK; 00410 } 00411 00412 unsigned long PeopleDetectorControlFlow::SaveRangeTrainImages(ipa_SensorFusion::ColoredPointCloudPtr pc) 00413 { 00414 std::string path = "ConfigurationFiles/haartraining/"; 00415 std::string img_ext = ".jpg"; 00416 cv::Mat xyzImage_8U3(pc->GetXYZImage().size(), CV_8UC3); //IplImage* xyzImage_8U3 = cvCreateImage(cvGetSize(pc->GetXYZImage()), 8, 3); 00417 ipa_Utils::ConvertToShowImage(pc->GetXYZImage(), xyzImage_8U3, 3); 00418 00419 for(int i=0; i<(int)m_colorFaces.size(); i++) 00420 { 00421 cv::Mat xyzImage_8U3_resized(100, 100, CV_8UC3); //cvCreateImage(cvSize(100,100), 8, 3); 8=IPL_DEPTH_8U 00422 00423 double scale = 1.6; 00424 cv::Rect rangeFace; 00425 rangeFace.height = (int)(m_colorFaces[i].height*scale); 00426 rangeFace.width = (int)(m_colorFaces[i].width*scale); 00427 rangeFace.x = m_colorFaces[i].x - ((rangeFace.width - m_colorFaces[i].width)/2); 00428 rangeFace.y = m_colorFaces[i].y - ((rangeFace.height - m_colorFaces[i].height)/2)-10; 00429 00430 cv::Mat xyzImage_8U3_roi = xyzImage_8U3(rangeFace); 00431 //cvSetImageROI(xyzImage_8U3, rangeFace); 00432 cv::resize(xyzImage_8U3_roi, xyzImage_8U3_resized, xyzImage_8U3_resized.size()); 00433 00434 std::ostringstream file; 00435 file << path << m_filname << img_ext; 00436 00437 cv::imwrite(file.str().c_str(), xyzImage_8U3_resized);//cvSaveImage(file.str().c_str(), xyzImage_8U3_resized); 00438 m_filname++; 00439 } 00440 00441 return ipa_Utils::RET_OK; 00442 } 00443 00444 unsigned long PeopleDetectorControlFlow::LoadParameters(const char* iniFileName) 00445 { 00447 TiXmlDocument* p_configXmlDocument = new TiXmlDocument( iniFileName ); 00448 if (!p_configXmlDocument->LoadFile()) 00449 { 00450 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00451 std::cerr << "\t ... Error while loading xml configuration file" << std::endl; 00452 std::cerr << "\t ... (Check filename and syntax of the file):" << std::endl; 00453 std::cerr << "\t ... '" << iniFileName << "'" << std::endl; 00454 return ipa_Utils::RET_FAILED; 00455 } 00456 std::cout << "INFO - PeopleDetector::LoadParameters:" << std::endl; 00457 std::cout << "\t ... Parsing xml configuration file:" << std::endl; 00458 std::cout << "\t ... '" << iniFileName << "'" << std::endl; 00459 00460 if ( p_configXmlDocument ) 00461 { 00462 00463 //************************************************************************************ 00464 // BEGIN PeopleDetector 00465 //************************************************************************************ 00466 // Tag element "PeopleDetector" of Xml Inifile 00467 TiXmlElement *p_xmlElement_Root = NULL; 00468 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "PeopleDetector" ); 00469 00470 if ( p_xmlElement_Root ) 00471 { 00472 00473 //************************************************************************************ 00474 // BEGIN PeopleDetector->adaBoost 00475 //************************************************************************************ 00476 // Tag element "adaBoost" of Xml Inifile 00477 TiXmlElement *p_xmlElement_Root_OD = NULL; 00478 p_xmlElement_Root_OD = p_xmlElement_Root->FirstChildElement( "adaBoost" ); 00479 00480 if ( p_xmlElement_Root_OD ) 00481 { 00482 00483 //************************************************************************************ 00484 // BEGIN PeopleDetector->adaBoost->Faces_increase_search_scale 00485 //************************************************************************************ 00486 // Subtag element "adaBoost" of Xml Inifile 00487 TiXmlElement *p_xmlElement_Child = NULL; 00488 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Faces_increase_search_scale" ); 00489 00490 if ( p_xmlElement_Child ) 00491 { 00492 // read and save value of attribute 00493 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_faces_increase_search_scale) != TIXML_SUCCESS) 00494 { 00495 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00496 std::cerr << "\t ... Can't find attribute 'value' of tag 'Faces_increase_search_scale'" << std::endl; 00497 return ipa_Utils::RET_FAILED; 00498 } 00499 } 00500 else 00501 { 00502 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00503 std::cerr << "\t ... Can't find tag 'Faces_increase_search_scale'" << std::endl; 00504 return ipa_Utils::RET_FAILED; 00505 } 00506 00507 //************************************************************************************ 00508 // BEGIN PeopleDetector->adaBoost->Faces_drop_groups 00509 //************************************************************************************ 00510 // Subtag element "adaBoost" of Xml Inifile 00511 p_xmlElement_Child = NULL; 00512 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Faces_drop_groups" ); 00513 00514 if ( p_xmlElement_Child ) 00515 { 00516 // read and save value of attribute 00517 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_faces_drop_groups) != TIXML_SUCCESS) 00518 { 00519 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00520 std::cerr << "\t ... Can't find attribute 'value' of tag 'Faces_drop_groups'" << std::endl; 00521 return ipa_Utils::RET_FAILED; 00522 } 00523 } 00524 else 00525 { 00526 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00527 std::cerr << "\t ... Can't find tag 'Faces_drop_groups'" << std::endl; 00528 return ipa_Utils::RET_FAILED; 00529 } 00530 00531 //************************************************************************************ 00532 // BEGIN PeopleDetector->adaBoost->Faces_min_search_scale_x 00533 //************************************************************************************ 00534 // Subtag element "adaBoost" of Xml Inifile 00535 p_xmlElement_Child = NULL; 00536 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Faces_min_search_scale_x" ); 00537 00538 if ( p_xmlElement_Child ) 00539 { 00540 // read and save value of attribute 00541 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_faces_min_search_scale_x) != TIXML_SUCCESS) 00542 { 00543 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00544 std::cerr << "\t ... Can't find attribute 'value' of tag 'Faces_min_search_scale_x'" << std::endl; 00545 return ipa_Utils::RET_FAILED; 00546 } 00547 } 00548 else 00549 { 00550 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00551 std::cerr << "\t ... Can't find tag 'Faces_min_search_scale_x'" << std::endl; 00552 return ipa_Utils::RET_FAILED; 00553 } 00554 00555 //************************************************************************************ 00556 // BEGIN PeopleDetector->adaBoost->Faces_min_search_scale_y 00557 //************************************************************************************ 00558 // Subtag element "adaBoost" of Xml Inifile 00559 p_xmlElement_Child = NULL; 00560 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Faces_min_search_scale_y" ); 00561 00562 if ( p_xmlElement_Child ) 00563 { 00564 // read and save value of attribute 00565 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_faces_min_search_scale_y) != TIXML_SUCCESS) 00566 { 00567 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00568 std::cerr << "\t ... Can't find attribute 'value' of tag 'Faces_min_search_scale_y'" << std::endl; 00569 return ipa_Utils::RET_FAILED; 00570 } 00571 } 00572 else 00573 { 00574 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00575 std::cerr << "\t ... Can't find tag 'Faces_min_search_scale_y'" << std::endl; 00576 return ipa_Utils::RET_FAILED; 00577 } 00578 00579 //************************************************************************************ 00580 // BEGIN PeopleDetector->adaBoost->Range_increase_search_scale 00581 //************************************************************************************ 00582 // Subtag element "adaBoost" of Xml Inifile 00583 p_xmlElement_Child = NULL; 00584 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Range_increase_search_scale" ); 00585 00586 if ( p_xmlElement_Child ) 00587 { 00588 // read and save value of attribute 00589 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_range_increase_search_scale) != TIXML_SUCCESS) 00590 { 00591 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00592 std::cerr << "\t ... Can't find attribute 'value' of tag 'Range_increase_search_scale'" << std::endl; 00593 return ipa_Utils::RET_FAILED; 00594 } 00595 } 00596 else 00597 { 00598 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00599 std::cerr << "\t ... Can't find tag 'Range_increase_search_scale'" << std::endl; 00600 return ipa_Utils::RET_FAILED; 00601 } 00602 00603 //************************************************************************************ 00604 // BEGIN PeopleDetector->adaBoost->Range_drop_groups 00605 //************************************************************************************ 00606 // Subtag element "adaBoost" of Xml Inifile 00607 p_xmlElement_Child = NULL; 00608 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Range_drop_groups" ); 00609 00610 if ( p_xmlElement_Child ) 00611 { 00612 // read and save value of attribute 00613 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_range_drop_groups) != TIXML_SUCCESS) 00614 { 00615 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00616 std::cerr << "\t ... Can't find attribute 'value' of tag 'Range_drop_groups'" << std::endl; 00617 return ipa_Utils::RET_FAILED; 00618 } 00619 } 00620 else 00621 { 00622 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00623 std::cerr << "\t ... Can't find tag 'Range_drop_groups'" << std::endl; 00624 return ipa_Utils::RET_FAILED; 00625 } 00626 00627 //************************************************************************************ 00628 // BEGIN PeopleDetector->adaBoost->Range_min_search_scale_x 00629 //************************************************************************************ 00630 // Subtag element "adaBoost" of Xml Inifile 00631 p_xmlElement_Child = NULL; 00632 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Range_min_search_scale_x" ); 00633 00634 if ( p_xmlElement_Child ) 00635 { 00636 // read and save value of attribute 00637 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_range_min_search_scale_x) != TIXML_SUCCESS) 00638 { 00639 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00640 std::cerr << "\t ... Can't find attribute 'value' of tag 'Range_min_search_scale_x'" << std::endl; 00641 return ipa_Utils::RET_FAILED; 00642 } 00643 } 00644 else 00645 { 00646 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00647 std::cerr << "\t ... Can't find tag 'Range_min_search_scale_x'" << std::endl; 00648 return ipa_Utils::RET_FAILED; 00649 } 00650 00651 //************************************************************************************ 00652 // BEGIN PeopleDetector->adaBoost->Range_min_search_scale_y 00653 //************************************************************************************ 00654 // Subtag element "adaBoost" of Xml Inifile 00655 p_xmlElement_Child = NULL; 00656 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Range_min_search_scale_y" ); 00657 00658 if ( p_xmlElement_Child ) 00659 { 00660 // read and save value of attribute 00661 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_PeopleDetector->m_range_min_search_scale_y) != TIXML_SUCCESS) 00662 { 00663 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00664 std::cerr << "\t ... Can't find attribute 'value' of tag 'Range_min_search_scale_y'" << std::endl; 00665 return ipa_Utils::RET_FAILED; 00666 } 00667 } 00668 else 00669 { 00670 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00671 std::cerr << "\t ... Can't find tag 'Range_min_search_scale_y'" << std::endl; 00672 return ipa_Utils::RET_FAILED; 00673 } 00674 } 00675 //************************************************************************************ 00676 // END CameraDataViewerControlFlow->adaBoost 00677 //************************************************************************************ 00678 00679 //************************************************************************************ 00680 // BEGIN PeopleDetector->eigenfaces 00681 //************************************************************************************ 00682 // Tag element "adaBoost" of Xml Inifile 00683 p_xmlElement_Root_OD = NULL; 00684 p_xmlElement_Root_OD = p_xmlElement_Root->FirstChildElement( "eigenfaces" ); 00685 00686 if ( p_xmlElement_Root_OD ) 00687 { 00688 00689 //************************************************************************************ 00690 // BEGIN PeopleDetector->eigenfaces->Threshold_Face_Class 00691 //************************************************************************************ 00692 // Subtag element "adaBoost" of Xml Inifile 00693 TiXmlElement *p_xmlElement_Child = NULL; 00694 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Threshold_Face_Class" ); 00695 00696 if ( p_xmlElement_Child ) 00697 { 00698 // read and save value of attribute 00699 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_threshold) != TIXML_SUCCESS) 00700 { 00701 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00702 std::cerr << "\t ... Can't find attribute 'value' of tag 'Threshold_Face_Class'" << std::endl; 00703 return ipa_Utils::RET_FAILED; 00704 } 00705 } 00706 else 00707 { 00708 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00709 std::cerr << "\t ... Can't find tag 'Threshold_Face_Class'" << std::endl; 00710 return ipa_Utils::RET_FAILED; 00711 } 00712 //************************************************************************************ 00713 // BEGIN PeopleDetector->eigenfaces->Threshold_Facespace 00714 //************************************************************************************ 00715 // Subtag element "adaBoost" of Xml Inifile 00716 p_xmlElement_Child = NULL; 00717 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Threshold_Facespace" ); 00718 00719 if ( p_xmlElement_Child ) 00720 { 00721 // read and save value of attribute 00722 if ( p_xmlElement_Child->QueryValueAttribute( "value", &m_threshold_FS) != TIXML_SUCCESS) 00723 { 00724 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00725 std::cerr << "\t ... Can't find attribute 'value' of tag 'Threshold_Facespace'" << std::endl; 00726 return ipa_Utils::RET_FAILED; 00727 } 00728 } 00729 else 00730 { 00731 std::cerr << "ERROR - PeopleDetector::LoadParameters:" << std::endl; 00732 std::cerr << "\t ... Can't find tag 'Threshold_Facespace'" << std::endl; 00733 return ipa_Utils::RET_FAILED; 00734 } 00735 //************************************************************************************ 00736 // END CameraDataViewerControlFlow->eigenfaces 00737 //************************************************************************************ 00738 00739 } 00740 else 00741 { 00742 std::cerr << "ERROR - CameraDataViewerControlFlow::LoadParameters:" << std::endl; 00743 std::cerr << "\t ... Can't find tag 'ObjectDetectorParameters'" << std::endl; 00744 return ipa_Utils::RET_FAILED; 00745 } 00746 00747 } 00748 00749 00750 //************************************************************************************ 00751 // END ObjectDetector 00752 //************************************************************************************ 00753 else 00754 { 00755 std::cerr << "ERROR - CameraDataViewerControlFlow::LoadParameters:" << std::endl; 00756 std::cerr << "\t ... Can't find tag 'ObjectDetector'" << std::endl; 00757 return ipa_Utils::RET_FAILED; 00758 } 00759 } 00760 return ipa_Utils::RET_OK; 00761 }