00001
00002
00003 #ifdef __LINUX__
00004 #include "cob_people_detection/people_detector.h"
00005 #include "cob_vision_utils/GlobalDefines.h"
00006 #else
00007 #include "cob_vision/cob_people_detection/common/include/cob_people_detection/PeopleDetector.h"
00008 #include "cob_common/cob_vision_utils/common/include/cob_vision_utils/GlobalDefines.h"
00009 #endif
00010
00011 #include <opencv2/opencv.hpp>
00012
00013 using namespace ipa_PeopleDetector;
00014
00015 PeopleDetector::PeopleDetector(void)
00016 {
00017 m_face_cascade = 0;
00018 m_range_cascade = 0;
00019 }
00020
00021 unsigned long PeopleDetector::Init(std::string directory)
00022 {
00023
00024 std::string faceCascadePath = directory + "haarcascades/haarcascade_frontalface_alt2.xml";
00025
00026 std::string rangeCascadePath = directory + "haarcascades/haarcascade_range_multiview_5p_bg.xml";
00027
00028 m_face_cascade = (CvHaarClassifierCascade*)cvLoad(faceCascadePath.c_str(), 0, 0, 0);
00029 m_range_cascade = (CvHaarClassifierCascade*)cvLoad(rangeCascadePath.c_str(), 0, 0, 0);
00030
00031
00032 m_storage = cvCreateMemStorage(0);
00033
00034 return ipa_Utils::RET_OK;
00035 }
00036
00037 PeopleDetector::~PeopleDetector(void)
00038 {
00039
00040 cvReleaseHaarClassifierCascade(&m_face_cascade);
00041 cvReleaseHaarClassifierCascade(&m_range_cascade);
00042 cvReleaseMemStorage(&m_storage);
00043 }
00044
00045 unsigned long PeopleDetector::DetectColorFaces(cv::Mat& img, std::vector<cv::Rect>& faceCoordinates)
00046 {
00047 IplImage imgPtr = (IplImage)img;
00048 CvSeq* faces = cvHaarDetectObjects(&imgPtr, m_face_cascade, m_storage, m_faces_increase_search_scale, m_faces_drop_groups, CV_HAAR_DO_CANNY_PRUNING,
00049 cvSize(m_faces_min_search_scale_x, m_faces_min_search_scale_y));
00050
00051 cv::Size parentSize;
00052 cv::Point roiOffset;
00053 for (int i = 0; i < faces->total; i++)
00054 {
00055 cv::Rect* face = (cv::Rect*)cvGetSeqElem(faces, i);
00056 img.locateROI(parentSize, roiOffset);
00057 face->x += roiOffset.x;
00058 face->y += roiOffset.y;
00059 faceCoordinates.push_back(*face);
00060 }
00061
00062 return ipa_Utils::RET_OK;
00063 }
00064
00065 unsigned long PeopleDetector::InterpolateUnassignedPixels(cv::Mat& img)
00066 {
00067 CV_Assert( img.type() == CV_8UC3 )
00068 ;
00069
00070 cv::Mat temp = img.clone();
00071
00072 uchar* data = img.data;
00073 uchar* data2 = temp.data;
00074 int stride = img.step;
00075 for (int repetitions = 0; repetitions < 10; repetitions++)
00076 {
00077
00078 for (int v = 1; v < img.rows - 1; v++)
00079 {
00080 for (int u = 1; u < img.cols - 1; u++)
00081 {
00082
00083 int index = v * stride + 3 * u;
00084 if (data[index] != 0)
00085 {
00086 uchar val = data[index];
00087 if (data2[index - 3] == 0)
00088 for (int i = -3; i < 0; i++)
00089 data2[index + i] = val;
00090 if (data2[index + 3] == 0)
00091 for (int i = 3; i < 6; i++)
00092 data2[index + i] = val;
00093 if (data2[index - stride] == 0)
00094 for (int i = -stride; i < -stride + 3; i++)
00095 data2[index + i] = val;
00096 if (data2[index + stride] == 0)
00097 for (int i = stride; i < stride + 3; i++)
00098 data2[index + i] = val;
00099 }
00100 }
00101 }
00102
00103 for (int i = 0; i < img.rows * stride; i++)
00104 data[i] = data2[i];
00105 }
00106 return ipa_Utils::RET_OK;
00107 }
00108
00109 unsigned long PeopleDetector::DetectRangeFace(cv::Mat& img, std::vector<cv::Rect>& rangeFaceCoordinates, bool fillUnassignedDepthValues)
00110 {
00111 rangeFaceCoordinates.clear();
00112
00113 if (fillUnassignedDepthValues)
00114 InterpolateUnassignedPixels(img);
00115
00116
00117
00118 IplImage imgPtr = (IplImage)img;
00119 CvSeq* rangeFaces = cvHaarDetectObjects(&imgPtr, m_range_cascade, m_storage, m_range_increase_search_scale, m_range_drop_groups, CV_HAAR_DO_CANNY_PRUNING,
00120 cvSize(m_range_min_search_scale_x, m_range_min_search_scale_y));
00121
00122 for (int i = 0; i < rangeFaces->total; i++)
00123 {
00124 cv::Rect *rangeFace = (cv::Rect*)cvGetSeqElem(rangeFaces, i);
00125 rangeFaceCoordinates.push_back(*rangeFace);
00126 }
00127
00128 return ipa_Utils::RET_OK;
00129 }
00130
00131 unsigned long PeopleDetector::DetectFaces(cv::Mat& img, cv::Mat& rangeImg, std::vector<cv::Rect>& colorFaceCoordinates, std::vector<cv::Rect>& rangeFaceCoordinates,
00132 std::set<size_t>& colorToRangeFaceDependency, bool fillUnassignedDepthValues)
00133 {
00134 colorFaceCoordinates.clear();
00135 colorToRangeFaceDependency.clear();
00136
00137
00138 DetectRangeFace(rangeImg, rangeFaceCoordinates, fillUnassignedDepthValues);
00139 for (int i = 0; i < (int)rangeFaceCoordinates.size(); i++)
00140 {
00141 cv::Rect rangeFace = rangeFaceCoordinates[i];
00142
00143 rangeFace.y += (int)(rangeFace.height * 0.1);
00144
00145 cv::Mat areaImg = img(rangeFace);
00146
00147
00148 size_t numberColorFacesBefore = colorFaceCoordinates.size();
00149 DetectColorFaces(areaImg, colorFaceCoordinates);
00150 if ((colorFaceCoordinates.size() - numberColorFacesBefore) != 0)
00151 colorToRangeFaceDependency.insert(i);
00152 }
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195 return ipa_Utils::RET_OK;
00196 }
00197
00198 unsigned long PeopleDetector::AddFace(cv::Mat& img, cv::Rect& face, std::string id, std::vector<cv::Mat>& images, std::vector<std::string>& ids)
00199 {
00200
00201 cv::Mat resized_8U1(100, 100, CV_8UC1);
00202 ConvertAndResize(img, resized_8U1, face);
00203
00204
00205 images.push_back(resized_8U1);
00206 ids.push_back(id);
00207
00208 return ipa_Utils::RET_OK;
00209 }
00210
00211 unsigned long PeopleDetector::ConvertAndResize(cv::Mat& img, cv::Mat& resized, cv::Rect& face)
00212 {
00213 cv::Mat temp;
00214 cv::cvtColor(img, temp, CV_BGR2GRAY);
00215 cv::Mat roi = temp(face);
00216 cv::resize(roi, resized, resized.size());
00217
00218 return ipa_Utils::RET_OK;
00219 }
00220
00221 cv::Mat PeopleDetector::preprocessImage(cv::Mat& input_image)
00222 {
00223
00224 return input_image;
00225
00226
00227 cv::Mat output(input_image.cols, input_image.rows, input_image.type());
00228
00229
00230
00231 for (int v = 0; v < input_image.rows; v++)
00232 {
00233 uchar* srcPtr = input_image.ptr(v);
00234
00235 uchar* outPtr = output.ptr(v);
00236 for (int u = 0; u < input_image.cols; u++)
00237 {
00238 int ctOutcome = 0;
00239 int offset = -1;
00240 for (int dv = -1; dv <= 1; dv++)
00241 {
00242 for (int du = -1; du <= 1; du++)
00243 {
00244 if (dv == 0 && du == 0)
00245 continue;
00246 offset++;
00247 if (v + dv < 0 || v + dv >= input_image.rows || u + du < 0 || u + du >= input_image.cols)
00248 continue;
00249
00250 if (*srcPtr < *(srcPtr + dv * input_image.step + du))
00251 ctOutcome += 1 << offset;
00252 }
00253 }
00254 *outPtr = ctOutcome;
00255
00256 srcPtr++;
00257 outPtr++;
00258 }
00259 }
00260
00261
00262
00263
00264 return output;
00265 }
00266
00267 unsigned long PeopleDetector::PCA(int* nEigens, std::vector<cv::Mat>& eigenVectors, cv::Mat& eigenValMat, cv::Mat& avgImage, std::vector<cv::Mat>& faceImages,
00268 cv::Mat& projectedTrainFaceMat)
00269 {
00270 CvTermCriteria calcLimit;
00271
00272
00273 (*nEigens) = faceImages.size() - 1;
00274
00275
00276 cv::Size faceImgSize(faceImages[0].cols, faceImages[0].rows);
00277 eigenVectors.resize(*nEigens, cv::Mat(faceImgSize, CV_32FC1));
00278 eigenValMat.create(1, *nEigens, CV_32FC1);
00279 avgImage.create(faceImgSize, CV_32FC1);
00280
00281
00282 calcLimit = cvTermCriteria(CV_TERMCRIT_ITER, (*nEigens), 1);
00283
00284
00285 IplImage** faceImgArr = (IplImage**)cvAlloc((int)faceImages.size() * sizeof(IplImage*));
00286 for (int j = 0; j < (int)faceImages.size(); j++)
00287 {
00288
00289 cv::Mat preprocessedImage = preprocessImage(faceImages[j]);
00290 IplImage temp = (IplImage)preprocessedImage;
00291 faceImgArr[j] = cvCloneImage(&temp);
00292 }
00293
00294
00295 IplImage** eigenVectArr = (IplImage**)cvAlloc((int)eigenVectors.size() * sizeof(IplImage*));
00296 for (int j = 0; j < (int)eigenVectors.size(); j++)
00297 {
00298 IplImage temp = (IplImage)eigenVectors[j];
00299 eigenVectArr[j] = cvCloneImage(&temp);
00300 }
00301
00302
00303 IplImage avgImageIpl = (IplImage)avgImage;
00304 cvCalcEigenObjects((int)faceImages.size(), (void*)faceImgArr, (void*)eigenVectArr, CV_EIGOBJ_NO_CALLBACK, 0, 0, &calcLimit, &avgImageIpl, (float*)(eigenValMat.data));
00305
00306
00307 cv::normalize(eigenValMat, eigenValMat, 1, 0, CV_L2);
00308
00309
00310 projectedTrainFaceMat.create(faceImages.size(), *nEigens, CV_32FC1);
00311 for (int i = 0; i < (int)faceImages.size(); i++)
00312 {
00313 IplImage temp = (IplImage)faceImages[i];
00314 cvEigenDecomposite(&temp, *nEigens, eigenVectArr, 0, 0, &avgImageIpl, (float*)projectedTrainFaceMat.data + i * *nEigens);
00315 };
00316
00317
00318 int eigenVectorsCount = (int)eigenVectors.size();
00319 eigenVectors.clear();
00320 for (int i = 0; i < (int)eigenVectorsCount; i++)
00321 eigenVectors.push_back(cv::Mat(eigenVectArr[i], true));
00322
00323
00324 for (int i = 0; i < (int)faceImages.size(); i++)
00325 cvReleaseImage(&(faceImgArr[i]));
00326 for (int i = 0; i < (int)eigenVectors.size(); i++)
00327 cvReleaseImage(&(eigenVectArr[i]));
00328 cvFree(&faceImgArr);
00329 cvFree(&eigenVectArr);
00330
00331 return ipa_Utils::RET_OK;
00332 }
00333
00334 #if CV_MAJOR_VERSION == 2
00335 unsigned long PeopleDetector::RecognizeFace(cv::Mat& colorImage, std::vector<cv::Rect>& colorFaceCoordinates, int* nEigens, std::vector<cv::Mat>& eigenVectors, cv::Mat& avgImage,
00336 cv::Mat& faceClassAvgProjections, std::vector<int>& index, int *threshold, int *threshold_FS, cv::Mat& eigenValMat, cv::SVM* personClassifier)
00337 #else
00338
00339 unsigned long PeopleDetector::RecognizeFace(cv::Mat& colorImage, std::vector<cv::Rect>& colorFaceCoordinates, int* nEigens, std::vector<cv::Mat>& eigenVectors, cv::Mat& avgImage,
00340 cv::Mat& faceClassAvgProjections, std::vector<int>& index, int *threshold, int *threshold_FS, cv::Mat& eigenValMat, cv::ml::SVM* personClassifier)
00341 #endif
00342 {
00343 float* eigenVectorWeights = 0;
00344
00345 cv::Mat resized_8U1(100, 100, CV_8UC1);
00346
00347 eigenVectorWeights = (float *)cvAlloc(*nEigens * sizeof(float));
00348
00349
00350 IplImage** eigenVectArr = (IplImage**)cvAlloc((int)eigenVectors.size() * sizeof(IplImage*));
00351 for (int j = 0; j < (int)eigenVectors.size(); j++)
00352 {
00353 IplImage temp = (IplImage)eigenVectors[j];
00354 eigenVectArr[j] = cvCloneImage(&temp);
00355 }
00356
00357 for (int i = 0; i < (int)colorFaceCoordinates.size(); i++)
00358 {
00359 cv::Rect face = colorFaceCoordinates[i];
00360 ConvertAndResize(colorImage, resized_8U1, face);
00361
00362 cv::Mat preprocessedImage = preprocessImage(resized_8U1);
00363
00364 IplImage avgImageIpl = (IplImage)avgImage;
00365
00366
00367 IplImage resized_8U1Ipl = (IplImage)resized_8U1;
00368 cvEigenDecomposite(&resized_8U1Ipl, *nEigens, eigenVectArr, 0, 0, &avgImageIpl, eigenVectorWeights);
00369
00370
00371 cv::Mat srcReconstruction = cv::Mat::zeros(eigenVectors[0].size(), eigenVectors[0].type());
00372 for (int i = 0; i < (int)eigenVectors.size(); i++)
00373 srcReconstruction += eigenVectorWeights[i] * eigenVectors[i];
00374 cv::Mat temp;
00375
00376
00377
00378
00379
00380
00381
00382
00383 resized_8U1.convertTo(temp, CV_32FC1, 1.0 / 255.0);
00384 double distance = cv::norm((temp - avgImage), srcReconstruction, cv::NORM_L2);
00385
00386
00387
00388 std::cout << "FS_Distance: " << distance << std::endl;
00389
00390
00391
00392
00393 if (distance > *threshold_FS)
00394 {
00395
00396 index.push_back(-2);
00397
00398 }
00399 else
00400 {
00401 int nearest;
00402 ClassifyFace(eigenVectorWeights, &nearest, nEigens, faceClassAvgProjections, threshold, eigenValMat, personClassifier);
00403 if (nearest < 0)
00404 index.push_back(-1);
00405 else
00406 index.push_back(nearest);
00407 }
00408 }
00409
00410
00411 for (int i = 0; i < (int)eigenVectors.size(); i++)
00412 cvReleaseImage(&(eigenVectArr[i]));
00413 cvFree(&eigenVectorWeights);
00414 cvFree(&eigenVectArr);
00415 return ipa_Utils::RET_OK;
00416 }
00417
00418 #if CV_MAJOR_VERSION == 2
00419 unsigned long PeopleDetector::ClassifyFace(float *eigenVectorWeights, int *nearest, int *nEigens, cv::Mat& faceClassAvgProjections, int *threshold, cv::Mat& eigenValMat,
00420 cv::SVM* personClassifier)
00421 #else
00422
00423 unsigned long PeopleDetector::ClassifyFace(float *eigenVectorWeights, int *nearest, int *nEigens, cv::Mat& faceClassAvgProjections, int *threshold, cv::Mat& eigenValMat,
00424 cv::ml::SVM* personClassifier)
00425 #endif
00426 {
00427 double leastDistSq = DBL_MAX;
00428
00429 int metric = 2;
00430
00431
00432 for (int i = 0; i < faceClassAvgProjections.rows; i++)
00433 {
00434 double distance = 0;
00435 double cos = 0;
00436 double length_sample = 0;
00437 double length_projection = 0;
00438 for (int e = 0; e < *nEigens; e++)
00439 {
00440 if (metric < 2)
00441 {
00442 float d = eigenVectorWeights[e] - ((float*)(faceClassAvgProjections.data))[i * *nEigens + e];
00443 if (metric == 0)
00444 distance += d * d;
00445 else
00446 distance += d * d / ((float*)(eigenValMat.data))[e];
00447 }
00448 else
00449 {
00450 cos += eigenVectorWeights[e] * ((float*)(faceClassAvgProjections.data))[i * *nEigens + e] / ((float*)(eigenValMat.data))[e];
00451 length_projection += ((float*)(faceClassAvgProjections.data))[i * *nEigens + e] * ((float*)(faceClassAvgProjections.data))[i * *nEigens + e]
00452 / ((float*)(eigenValMat.data))[e];
00453 length_sample += eigenVectorWeights[e] * eigenVectorWeights[e] / ((float*)(eigenValMat.data))[e];
00454 }
00455 }
00456 if (metric < 2)
00457 distance = sqrt(distance);
00458 else
00459 {
00460 length_sample = sqrt(length_sample);
00461 length_projection = sqrt(length_projection);
00462 cos /= (length_projection * length_sample);
00463 distance = -cos;
00464 }
00465
00466
00467
00468 std::cout << "Distance_FC: " << distance << std::endl;
00469
00470
00471 if (distance < leastDistSq)
00472 {
00473 leastDistSq = distance;
00474 if (leastDistSq > *threshold)
00475 *nearest = -1;
00476 else
00477 *nearest = i;
00478 }
00479 }
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489 return ipa_Utils::RET_OK;
00490 }
00491
00492 #if CV_MAJOR_VERSION == 2
00493 unsigned long PeopleDetector::CalculateFaceClasses(cv::Mat& projectedTrainFaceMat, std::vector<std::string>& id, int *nEigens, cv::Mat& faceClassAvgProjections,
00494 std::vector<std::string>& idUnique, cv::SVM* personClassifier)
00495 #else
00496
00497 unsigned long PeopleDetector::CalculateFaceClasses(cv::Mat& projectedTrainFaceMat, std::vector<std::string>& id, int *nEigens, cv::Mat& faceClassAvgProjections,
00498 std::vector<std::string>& idUnique, cv::ml::SVM* personClassifier)
00499 #endif
00500 {
00501 std::cout << "PeopleDetector::CalculateFaceClasses ... ";
00502
00503
00504 idUnique.clear();
00505 for (int i = 0; i < (int)id.size(); i++)
00506 {
00507 std::string face_class = id[i];
00508 bool class_exists = false;
00509
00510 for (int j = 0; j < (int)idUnique.size(); j++)
00511 {
00512 if (!idUnique[j].compare(face_class))
00513 {
00514 class_exists = true;
00515 }
00516 }
00517
00518 if (!class_exists)
00519 {
00520 idUnique.push_back(face_class);
00521 }
00522 }
00523
00524
00525
00526 cv::Mat temp = cv::Mat::zeros((int)idUnique.size(), *nEigens, projectedTrainFaceMat.type());
00527 temp.convertTo(faceClassAvgProjections, projectedTrainFaceMat.type());
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558 for (int i = 0; i < (int)idUnique.size(); i++)
00559 {
00560 std::string face_class = idUnique[i];
00561
00562 for (int e = 0; e < *nEigens; e++)
00563 {
00564 int count = 0;
00565 for (int j = 0; j < (int)id.size(); j++)
00566 {
00567 if (!(id[j].compare(face_class)))
00568 {
00569 ((float*)(faceClassAvgProjections.data))[i * *nEigens + e] += ((float*)(projectedTrainFaceMat.data))[j * *nEigens + e];
00570 count++;
00571 }
00572 }
00573 ((float*)(faceClassAvgProjections.data))[i * *nEigens + e] /= (float)count;
00574 }
00575 }
00576
00577
00578 if (personClassifier != 0)
00579 {
00580
00581
00582 cv::Mat data(id.size(), *nEigens, CV_32FC1);
00583 cv::Mat labels(id.size(), 1, CV_32SC1);
00584 std::ofstream fout("svm.dat", std::ios::out);
00585 for (int sample = 0; sample < (int)id.size(); sample++)
00586 {
00587
00588 for (int e = 0; e < *nEigens; e++)
00589 {
00590 data.at<float>(sample, e) = ((float*)projectedTrainFaceMat.data)[sample * *nEigens + e];
00591 fout << data.at<float>(sample, e) << "\t";
00592 }
00593
00594 for (int i = 0; i < (int)idUnique.size(); i++)
00595 if (!(id[sample].compare(idUnique[i])))
00596 labels.at<int>(sample) = i;
00597 fout << labels.at<int>(sample) << "\n";
00598 }
00599 fout.close();
00600
00601
00602 #if CV_MAJOR_VERSION == 2
00603 cv::SVMParams svmParams(CvSVM::NU_SVC, CvSVM::RBF, 0.0, 0.001953125, 0.0, 0.0, 0.8, 0.0, 0, cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, FLT_EPSILON));
00604
00605 #else
00606
00607 cv::ml::SVMParams svmParams(CvSVM::NU_SVC, CvSVM::RBF, 0.0, 0.001953125, 0.0, 0.0, 0.8, 0.0, 0, cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, FLT_EPSILON));
00608
00609 #endif
00610 personClassifier->train(data, labels, cv::Mat(), cv::Mat(), svmParams);
00611 #if CV_MAJOR_VERSION == 2
00612 cv::SVMParams svmParamsOptimal = personClassifier->get_params();
00613 #else
00614
00615 cv::ml::SVMParams svmParamsOptimal = personClassifier->get_params();
00616 #endif
00617 std::cout << "Optimal SVM params: gamma=" << svmParamsOptimal.gamma << " nu=" << svmParamsOptimal.nu << "\n";
00618 }
00619
00620 std::cout << "done\n";
00621
00622 return ipa_Utils::RET_OK;
00623 }
00624