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) & 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
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
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
00107 m_eigenVectors.clear();
00108
00109
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
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
00152
00153
00154
00155
00156
00157
00158
00159
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
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
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
00189 cv::imwrite(img.str().c_str(), m_faceImages[i]);
00190 }
00191
00192
00193 fileStorage << "eigens_num" << m_nEigens;
00194
00195
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
00204 fileStorage << "eigen_val_mat" << m_eigenValMat;
00205
00206
00207 fileStorage << "avg_image" << m_avgImage;
00208
00209
00210 fileStorage << "projected_train_face_mat" << m_projectedTrainFaceMat;
00211
00212
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
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
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
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
00268 cv::Mat temp = cv::imread(path.c_str(),0);
00269 m_faceImages.push_back(temp);
00270 }
00271
00272
00273 m_nEigens = (int)fileStorage["eigens_num"];
00274
00275
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
00286 m_eigenValMat = cv::Mat();
00287 fileStorage["eigen_val_mat"] >> m_eigenValMat;
00288
00289
00290 m_avgImage = cv::Mat();
00291 fileStorage["avg_image"] >> m_avgImage;
00292
00293
00294 m_projectedTrainFaceMat = cv::Mat();
00295 fileStorage["projected_train_face_mat"] >> m_projectedTrainFaceMat;
00296
00297
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
00308 m_faceClassAvgProjections = cv::Mat();
00309 fileStorage["face_class_avg_projections"] >> m_faceClassAvgProjections;
00310
00311 fileStorage.release();
00312
00313
00314
00315
00316
00317 std::cout << "INFO - PeopleDetectorControlFlow::LoadTrainingData:" << std::endl;
00318 std::cout << "\t ... " << faces_num << " images loaded.\n";
00319
00320
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
00336 cv::normalize(m_eigenVectors[index], eigenface, 0, 255, cv::NORM_MINMAX, CV_8UC1);
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
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
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
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);
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);
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
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);
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
00465
00466
00467 TiXmlElement *p_xmlElement_Root = NULL;
00468 p_xmlElement_Root = p_configXmlDocument->FirstChildElement( "PeopleDetector" );
00469
00470 if ( p_xmlElement_Root )
00471 {
00472
00473
00474
00475
00476
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
00485
00486
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
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
00509
00510
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
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
00533
00534
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
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
00557
00558
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
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
00581
00582
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
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
00605
00606
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
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
00629
00630
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
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
00653
00654
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
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
00677
00678
00679
00680
00681
00682
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
00691
00692
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
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
00714
00715
00716 p_xmlElement_Child = NULL;
00717 p_xmlElement_Child = p_xmlElement_Root_OD->FirstChildElement( "Threshold_Facespace" );
00718
00719 if ( p_xmlElement_Child )
00720 {
00721
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
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
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 }