From 649778274d4fd5f86fb76fbfbdb1b6cea418eab9 Mon Sep 17 00:00:00 2001 From: Parag K Mital Date: Sat, 28 May 2011 22:55:09 +0100 Subject: [PATCH] adding least-squares calibration --- src/app.cpp | 123 +++++++++++++++++++++++++ src/app.h | 109 +++------------------- src/pkmFaceModeler.cpp | 126 +++++++++++++++++++++++++ src/pkmFaceModeler.h | 125 +++++-------------------- src/pkmFaceTracker.h | 18 ++-- src/pkmPoseCalibrator.cpp | 188 ++++++++++++++++++++++++++++++++++++++ src/pkmPoseCalibrator.h | 51 +++++++++++ 7 files changed, 533 insertions(+), 207 deletions(-) create mode 100644 src/pkmPoseCalibrator.cpp create mode 100644 src/pkmPoseCalibrator.h diff --git a/src/app.cpp b/src/app.cpp index 8d955e6..a30e070 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -8,3 +8,126 @@ #include "app.h" +app::app() +{ + +} +app::~app() +{ + delete faceTracker; + delete faceModeler; +} + +void app::setup(int w, int h, int n) +{ + width = w; + height = h; + + // initialize face tracker + numExamples = n; // number of images to train face model on + faceModeler = new pkmFaceModeler(numExamples); // appearance model + faceTracker = new pkmFaceTracker(); // shape model + poseCalibrator = new pkmPoseCalibrator(); // pose calibration + + // open a camera + camera = cvCreateCameraCapture(CV_CAP_ANY); + cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_WIDTH, (double)width); + cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_HEIGHT, (double)height); + + if(!camera) + { + printf("[ERROR]: Could not open any cameras!\n"); + std::exit(1); + } + + // initialize a window + window_faceTracker = "Face Tracking"; + cvNamedWindow(window_faceTracker.c_str(), 1); + + bTraining = false; + bAppearanceModelBuilt = false; + bPoseModelBuilt = false; +} + +void app::update() +{ + cameraImage = cvQueryFrame(camera); + if(!cameraImage) + return; + frame = cameraImage; + + if(bPoseModelBuilt) { + faceModeler->update(frame); + Mat av = faceModeler->getAppearanceVector(); + Mat pose_x, pose_y; + poseCalibrator->getPose(av, pose_x, pose_y); + } + else if(bAppearanceModelBuilt) { + faceModeler->update(frame); + } + else { + faceTracker->update(frame); + + if (bTraining) { + Mat shape = faceTracker->getShapeModel(); + if(faceModeler->addExample(frame, shape)) + bAppearanceModelBuilt = true; + } + } +} + +void app::draw() +{ + if (bPoseModelBuilt) { + faceModeler->draw(frame); + } + else if(bAppearanceModelBuilt) { + faceModeler->draw(frame); + } + else { + faceTracker->drawShapeModel(frame); + int count = faceModeler->getCurrentCount(); + char buf[256]; + sprintf(buf, "%03d / %03d", count, numExamples); + putText(frame, + buf, + Point(10,20), + CV_FONT_HERSHEY_SIMPLEX, + 0.5, + CV_RGB(255,255,255)); + + } + + imshow(window_faceTracker.c_str(), frame); +} + +void app::keyPressed(int c) +{ + // reset the shape model + if(c == 'd') + faceTracker->reset(); + // start training the AAM + else if(c == ' ') + { + bTraining = !bTraining; + } + // load a pre-existing AAM + else if(c == 'l') + { + bAppearanceModelBuilt = faceModeler->loadExistingModel(); + } + + // train for calibration + else if(c >= '1' && c <= '9' || c == '0' ) + { + char cc = char(c); + int point = atoi(&cc); + Mat av = faceModeler->getAppearanceVector(); + poseCalibrator->addExample(point, av); + } + + else if(c == 't') + { + bPoseModelBuilt = poseCalibrator->modelPose(); + } +} diff --git a/src/app.h b/src/app.h index 6b04419..e0192e8 100644 --- a/src/app.h +++ b/src/app.h @@ -11,118 +11,33 @@ #include "pkmFaceTracker.h" #include "pkmFaceModeler.h" +#include "pkmPoseCalibrator.h" + #include class app { public: - app() - { - - } - ~app() - { - delete faceTracker; - delete faceModeler; - } - - void setup(int w = 320, int h = 240) - { - width = w; - height = h; - - // initialize face tracker - faceModeler = new pkmFaceModeler(); // appearance model - faceTracker = new pkmFaceTracker(); // shape model - - // open a camera - camera = cvCreateCameraCapture(CV_CAP_ANY); - cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_WIDTH, (double)width); - cvSetCaptureProperty(camera, CV_CAP_PROP_FRAME_HEIGHT, (double)height); - - if(!camera) - { - printf("[ERROR]: Could not open any cameras!\n"); - std::exit(1); - } - - // initialize a window - window_faceTracker = "Face Tracking"; - cvNamedWindow(window_faceTracker.c_str(), 1); - - bTraining = false; - bModelBuilt = false; - } - - void update() - { - cameraImage = cvQueryFrame(camera); - if(!cameraImage) - return; - frame = cameraImage; - - if (bModelBuilt) { - faceModeler->update(frame); - } - else { - faceTracker->update(frame); - - if (bTraining) { - Mat shape = faceTracker->getShapeModel(); - if(faceModeler->addExample(frame, shape)) - bModelBuilt = true; - } - } - } - - void draw() - { - if (bModelBuilt) { - faceModeler->draw(frame); - } - else { - faceTracker->drawShapeModel(frame); - int count = faceModeler->getCurrentCount(); - char buf[256]; - sprintf(buf, "%03d / 100", count); - putText(frame, - buf, - Point(10,20), - CV_FONT_HERSHEY_SIMPLEX, - 0.5, - CV_RGB(255,255,255)); - - } - - imshow(window_faceTracker.c_str(), frame); - } - - void keyPressed(int c) - { - if(char(c) == 'd') - faceTracker->reset(); - else if(char(c) == ' ') - { - bTraining = !bTraining; - } - else if(char(c) == 'l') - { - bModelBuilt = faceModeler->loadExistingModel(); - } - } - + app(); + ~app(); + void setup(int w = 640, int h = 480, int n = 200); + void update(); + void draw(); + void keyPressed(int c); private: pkmFaceTracker *faceTracker; - pkmFaceModeler *faceModeler; + pkmPoseCalibrator *poseCalibrator; CvCapture *camera; IplImage *cameraImage; Mat frame; string window_faceTracker; + int numExamples; + int width, height; - bool bTraining, bModelBuilt; + bool bTraining, bAppearanceModelBuilt, bPoseModelBuilt; }; \ No newline at end of file diff --git a/src/pkmFaceModeler.cpp b/src/pkmFaceModeler.cpp index d0ebf30..fc7b501 100644 --- a/src/pkmFaceModeler.cpp +++ b/src/pkmFaceModeler.cpp @@ -8,3 +8,129 @@ #include "pkmFaceModeler.h" +pkmFaceModeler::pkmFaceModeler(int n) +{ + char cascadeFileName[] = "../model/haarcascade_frontalface_alt2.xml"; + faceDet.LoadCascade(cascadeFileName); + + currentExample = 0; // current example to train + numExamples = n; // number of examples before model is built + + bModelLoaded = false; + bDraw = false; +} + +pkmFaceModeler::~pkmFaceModeler() +{ + +} + +bool pkmFaceModeler::addExample(Mat &img, Mat &shape) +{ + if (bModelLoaded) { + return true; + } + + currentExample++; + + int numPoints = shape.rows / 2; + + // save the image + char filename[256]; + sprintf(filename, "img_%03d.jpg", currentExample); + imwrite(filename, img); + imageFiles.push_back(filename); + + // save the points + FILE *fp; + sprintf(filename, "img_%03d.pts", currentExample); + fp = fopen(filename, "w"); + fprintf(fp, "points %d\n", numPoints); + for (int i = 0; i < numPoints; i++) { + fprintf(fp, "%f %f\n", shape.at(i,0),shape.at(i+numPoints,0)); + } + fclose(fp); + pointFiles.push_back(filename); + + // check if we can build a model yet + if (currentExample >= numExamples) { + return buildModel(); + } + else { + return false; + } +} + +// we've learned enough examples, now train an AAM, storing to "model.aam" +bool pkmFaceModeler::buildModel() +{ + int type = TYPE_AAM_IC; + int level = 1; + int color = 3; + aamModel.Build(pointFiles, imageFiles, type, level, color); + aamModel.BuildDetectMapping(pointFiles, imageFiles, faceDet, 100); + aamModel.WriteModel("model.aam"); + bModelLoaded = true; + return true; +} + +// load a model we've saved before +bool pkmFaceModeler::loadExistingModel() +{ + aamModel.ReadModel("model.aam"); + bModelLoaded = true; + return bModelLoaded; +} + +// how many examples have we stored so far? +int pkmFaceModeler::getCurrentCount() +{ + return currentExample; +} + +// update with new frame +void pkmFaceModeler::update(Mat &frame) +{ + // if we have loaded a model + if (bModelLoaded) { + // track the face + IplImage image = frame; + if (aamModel.InitShapeFromDetBox(aamShape, faceDet, &image) == false) { + // no face found + bDraw = false; + return; + } + // found a face, now fit an AAM + else { + bDraw = true; + aamModel.Fit(&image, aamShape, 1, false); + + // output eigen-values to console + //aamModel.PrintAppearanceVector(); + + // store appearance vector + CvMat *appearanceVector = aamModel.GetAppearanceVector(); + currentAppearanceVector = Mat(appearanceVector, true); + } + } +} + +void pkmFaceModeler::draw(Mat &frame) +{ + if (bDraw) + { + // draw the appearance model in the image + IplImage image = frame; + aamModel.Draw(&image, aamShape, 2); + } +} + +// return the matrix of appearance values +Mat pkmFaceModeler::getAppearanceVector() +{ + if (bModelLoaded) + return currentAppearanceVector; + else { + return Mat(); + } +} \ No newline at end of file diff --git a/src/pkmFaceModeler.h b/src/pkmFaceModeler.h index 347cc17..2cc3109 100644 --- a/src/pkmFaceModeler.h +++ b/src/pkmFaceModeler.h @@ -24,120 +24,43 @@ using namespace cv; class pkmFaceModeler { public: - pkmFaceModeler(int n = 100) - { - char cascadeFileName[] = "../model/haarcascade_frontalface_alt2.xml"; - faceDet.LoadCascade(cascadeFileName); - - currentExample = 0; // current example to train - numExamples = n; // number of examples before model is built - - bModelLoaded = false; - bDraw = false; - } + pkmFaceModeler(int n = 100); - ~pkmFaceModeler() - { - - } + ~pkmFaceModeler(); - bool addExample(Mat &img, Mat &shape) - { - if (bModelLoaded) { - return true; - } - - currentExample++; - - int numPoints = shape.rows / 2; - - // save the image - char filename[256]; - sprintf(filename, "img_%03d.jpg", currentExample); - imwrite(filename, img); - imageFiles.push_back(filename); - - // save the points - FILE *fp; - sprintf(filename, "img_%03d.pts", currentExample); - fp = fopen(filename, "w"); - fprintf(fp, "points %d\n", numPoints); - for (int i = 0; i < numPoints; i++) { - fprintf(fp, "%f %f\n", shape.at(i,0),shape.at(i+numPoints,0)); - } - fclose(fp); - pointFiles.push_back(filename); - - // check if we can build a model yet - if (currentExample >= numExamples) { - return buildModel(); - } - else { - return false; - } - } + bool addExample(Mat &img, Mat &shape); - bool buildModel() - { - int type = TYPE_AAM_IC; - int level = 1; - int color = 3; - aamModel.Build(pointFiles, imageFiles, type, level, color); - aamModel.BuildDetectMapping(pointFiles, imageFiles, faceDet, 100); - aamModel.WriteModel("model.aam"); - bModelLoaded = true; - return true; - } + // we've learned enough examples, now train an AAM, storing to "model.aam" + bool buildModel(); - bool loadExistingModel() - { - aamModel.ReadModel("model.aam"); - bModelLoaded = true; - return bModelLoaded; - } + // load a model we've saved before + bool loadExistingModel(); - int getCurrentCount() - { - return currentExample; - } + // how many examples have we stored so far? + int getCurrentCount(); - void update(Mat &frame) - { - if (bModelLoaded) { - IplImage image = frame; - if (aamModel.InitShapeFromDetBox(aamShape, faceDet, &image) == false) { - bDraw = false; - return; - } - else { - bDraw = true; - aamModel.Fit(&image, aamShape, 1, false); - } - } - } + // update with new frame + void update(Mat &frame); - void draw(Mat &frame) - { - if (bDraw) - { - IplImage image = frame; - aamModel.Draw(&image, aamShape, 2); - } - } + void draw(Mat &frame); + + // return the matrix of appearance values + Mat getAppearanceVector(); private: + Mat currentAppearanceVector; - vector imageFiles, - pointFiles; + vector imageFiles, + pointFiles; - int numExamples, - currentExample; + int numExamples, + currentExample; - AAM_Pyramid aamModel; - AAM_Shape aamShape; - VJfacedetect faceDet; + AAM_Pyramid aamModel; + AAM_Shape aamShape; + VJfacedetect faceDet; - bool bModelLoaded, bDraw; + bool bModelLoaded, bDraw; }; \ No newline at end of file diff --git a/src/pkmFaceTracker.h b/src/pkmFaceTracker.h index 9de7819..86cb6d9 100644 --- a/src/pkmFaceTracker.h +++ b/src/pkmFaceTracker.h @@ -36,17 +36,17 @@ class pkmFaceTracker // tracking parameters wSize1.resize(1); - wSize1[0] = 7; wSize2.resize(3); - wSize2[0] = 11; - wSize2[1] = 9; - wSize2[2] = 7; + wSize1[0] = 5; // 7 + wSize2[0] = 7; // 11 + wSize2[1] = 5; // 9 + wSize2[2] = 3; // 7 - nIter = 5; - clamp = 3; - fTol = 0.01; + nIter = 50; // 5 + clamp = 3; // 3 + fTol = 0.99; // 0.01 - fpd = -1; + fpd = -1; // -1 bFCheck = false; bFailed = true; @@ -69,7 +69,7 @@ class pkmFaceTracker scale * frame.rows)); // flip horizontal - cv::flip(im, im, 1); + // cv::flip(im, im, 1); // go to grayscale cv::cvtColor(im, gray, CV_BGR2GRAY); diff --git a/src/pkmPoseCalibrator.cpp b/src/pkmPoseCalibrator.cpp new file mode 100644 index 0000000..cdf61e4 --- /dev/null +++ b/src/pkmPoseCalibrator.cpp @@ -0,0 +1,188 @@ +/* + * pkmCalibrator.cpp + * xcode + * + * Created by Mr. Magoo on 5/28/11. + * Copyright 2011 __MyCompanyName__. All rights reserved. + * + */ + +#include "pkmPoseCalibrator.h" + +pkmPoseCalibrator::pkmPoseCalibrator() +{ + numCalibrationPoints = 10; // number of calibration points + numViews = 2; // observations for each calibration point + numPoints = 4; // number of eigen values to train on + + eigenValues = Mat(numViews * numCalibrationPoints, numPoints, CV_64FC1); + + // keep an index of the current view for each calibration point + for (int i = 0; i < numCalibrationPoints; i++) { + currentView.push_back(0); + bCurrentViewLoaded.push_back(false); + } + + // store the calibration points (screen coordinates to map to) + vector p_x, p_y; + // + // 1 2 3 + // 10 + // 4 5 6 + // + // 7 8 9 + // + + for(int i = 0; i < numViews; i++) + { + p_x.push_back(0.5); + p_y.push_back(0.5); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(-1.0); + p_y.push_back(1.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(0.0); + p_y.push_back(1.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(1.0); + p_y.push_back(1.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(-1.0); + p_y.push_back(0.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(0.0); + p_y.push_back(0.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(1.0); + p_y.push_back(0.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(-1.0); + p_y.push_back(-1.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(0.0); + p_y.push_back(-1.0); + } + for(int i = 0; i < numViews; i++) + { + p_x.push_back(1.0); + p_y.push_back(-1.0); + } + + poseX = cv::Mat(p_x).clone(); + poseY = cv::Mat(p_y).clone(); + //printMatrix(poseX, "poseX"); + //printMatrix(poseY, "poseY"); + + bBuiltModel = false; +} +pkmPoseCalibrator::~pkmPoseCalibrator() +{ + +} + +void pkmPoseCalibrator::addExample(int calibrationPoint, Mat &values) +{ + printf("Adding example for point %d.\n", calibrationPoint); + + if (values.rows != 1 || values.cols < numPoints) { + printf("[ERROR]: expected a 1x%d matrix. Received a %dx%d matrix.\n", + numPoints, + values.rows, + values.cols); + return; + } + + // get the index in the huge matrix of observations + int indx = calibrationPoint*numViews + currentView[calibrationPoint]; + + // store these values + Mat r = eigenValues.row(indx); + values.colRange(0, numPoints).copyTo(r); + printMatrix(r, "new row"); + + // increment the number of views loaded + currentView[calibrationPoint] = (currentView[calibrationPoint] + 1) % numViews; + + // check if we have cycled around the number of views needed to training this point + bCurrentViewLoaded[calibrationPoint] = (currentView[calibrationPoint]==0) | + bCurrentViewLoaded[calibrationPoint]; +} + +void pkmPoseCalibrator::getPose(Mat &values, Mat &pose_x, Mat &pose_y) +{ + if (bBuiltModel) { + Mat px = values.colRange(0, 4) * poseXInv; + px.copyTo(pose_x); + Mat py = values.colRange(0, 4) * poseYInv; + py.copyTo(pose_y); + printMatrix(px, "pose_x"); + printMatrix(py, "pose_y"); + } +} + +bool pkmPoseCalibrator::isReadyForTraining() +{ + bool ready = true; + for (int i = 0; i < numCalibrationPoints; i++) { + ready &= bCurrentViewLoaded[i]; + } + return ready; +} + +bool pkmPoseCalibrator::modelPose() +{ + if (isReadyForTraining()) { + printf("Modeling pose...\n"); + + printMatrix(eigenValues, "eigenValues"); + Mat ete = eigenValues.t() * eigenValues; + printMatrix(ete, "ete"); + Mat etei = ete.inv(); + printMatrix(etei, "etei"); + Mat eteit = etei * eigenValues.t(); + printMatrix(eteit, "eteit"); + + Mat epx = eteit * poseX; + epx.copyTo(poseXInv); + Mat epy = eteit * poseY; + epy.copyTo(poseYInv); + + printMatrix(epx, "poseXInv"); + printMatrix(epy, "poseYInv"); + bBuiltModel = true; + return true; + } + else { + printf("Not all calibration points have been trained!\n"); + return false; + } + +} + +void pkmPoseCalibrator::printMatrix(Mat X, string name) +{ + printf("%s: %d, %d\n", name.c_str(), X.rows, X.cols); + for (int i = 0; i < X.rows; i++) { + for (int j = 0; j < X.cols; j++) { + printf("%3.4lf ", X.at(i,j)); + } + printf("\n"); + } + printf("\n"); +} \ No newline at end of file diff --git a/src/pkmPoseCalibrator.h b/src/pkmPoseCalibrator.h new file mode 100644 index 0000000..e8c4610 --- /dev/null +++ b/src/pkmPoseCalibrator.h @@ -0,0 +1,51 @@ +/* + * pkmCalibrator.h + + [ R11 R12 R13 ] [ x ] [ dx ] [ X ] + [ R21 R22 R23 ] * [ y ] + [ dy ] = [ Y ] + [ R31 R32 R33 ] [ z ] [ dz ] [ Z ] + + So there is some mapping of the first 4 eigenvalues of the AAM and the 3 translation + values of the central bounding box to these 3 rotation + 3 translation values... + + */ + +#pragma once + +#include +#include + +using namespace std; +using namespace cv; + +class pkmPoseCalibrator +{ +public: + pkmPoseCalibrator(); + ~pkmPoseCalibrator(); + + void addExample(int calibrationPoint, Mat &values); + + void getPose(Mat &values, Mat &pose_x, Mat &pose_y); + + bool isReadyForTraining(); + + bool modelPose(); + + void printMatrix(Mat X, string name = ""); + +private: + vector currentView; + vector bCurrentViewLoaded; + Mat poseX, + poseY; + Mat eigenValues; + Mat poseXInv, + poseYInv; + + int numCalibrationPoints, + numViews, + numPoints; + + bool bBuiltModel; +}; \ No newline at end of file