adding least-squares calibration

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();
+	}
+}

src/app.h

@@ -11,118 +11,33 @@
 
 #include "pkmFaceTracker.h"
 #include "pkmFaceModeler.h"
+#include "pkmPoseCalibrator.h"
+
 #include <opencv2/opencv.hpp>
 
 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;
 };

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<double>(i,0),shape.at<double>(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();
+	}
+}