facesearch.cpp

机器人程序

// FaceSearch.cpp - by Robin Hewitt, 2005
// http://www.robinhewitt.com/mavis
// This is free software. See license at the bottom
// of this file for details.
//

//////////////////////////////////////////////////////////////
// FaceSearch class implementation.
//

#include "MVLib.h"
#include "FaceSearch.h"
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <memory.h>
#include <string.h>
#include <list>

using namespace std;
using namespace dipole;

typedef MVObjSearch::Result_t  Result_t;
typedef MVObjSearch::Hit_t     Hit_t;

////////////////////////////
// Constructor
//
FaceSearch::FaceSearch(FaceSearchParams & params) throw(MVErr) :
scaleArr(0), cascadeArr(0), pIntegralImg(0), pGradHistIntegral(0)
{
	///// temp code
	//cout << "in FaceSearch constructor\n";
	//cout << "dataDir = " << params.dataDir << endl;
	//cout << "modelNum = " << params.modelNum << "\n";
	///// end of temp code

	// generate model-file name
	ostringstream stream;
	stream << "models/objModel" << params.modelNum << ".dat";
	MVUtils::makeFullPath(params.dataDir, stream.str(), params.modelFile);

	///// temp code
	//cout << "modelFile = " << params.modelFile << "\n";
	///// end of temp code

	// read in the model-file data
	ifstream fin(params.modelFile.c_str());
	if( !fin.is_open() )
		throw MVErr("Can\'t open the model file: " + params.modelFile);

	// read in names of cascade and feature files
	const int MAXBUF = 500;
	char      buf[MAXBUF];

	fin.getline(buf, MAXBUF);
	params.cascadeFile = buf;
	params.cascadeFile = "models/" + params.cascadeFile;
	MVUtils::makeFullPath(params.dataDir, params.cascadeFile, params.cascadeFile);
	//cout << "cascadeFile = " << params.cascadeFile << "\n";

	fin.getline(buf, MAXBUF);
	params.featFile = buf;
	params.featFile = "models/" + params.featFile;
	MVUtils::makeFullPath(params.dataDir, params.featFile, params.featFile);
	//cout << "featFile = " << params.featFile << "\n";

	fin >> thresh;
	//cout << "threshold = " << thresh << endl;

	fin.close();

	// load the cascade
	loadCascade(params.cascadeFile);

	// load the feature vector
	loadFeatures(params.featFile);
}


////////////////////////////
// Destructor
//
FaceSearch::~FaceSearch()
{
	if(pIntegralImg) delete pIntegralImg;
	if(scaleArr) delete[] scaleArr;
	if(cascadeArr) delete[] cascadeArr;
	MVUtils::deleteElements(&matchVect);
	if(pGradHistIntegral) delete pGradHistIntegral;
}


////////////////////////////
// search()
//
Result_t * FaceSearch::search(VideoFrame & frame, int imgNum)
{
	Result_t * pResult = new Result_t();

	//// temp code
	//pResult->pHit = 0;
	//return pResult;
	//// end of temp code

	const bool VIS = false;

	imgSz.w = frame.getWidth();
	imgSz.h = frame.getHeight();

	MVImg<int> * pImg = 0;
	MVImgUtils::frame2ColorChannel(frame, MVImgUtils::RED, &pImg);
	getMatches(pImg);

	if(VIS)
		MVImgUtils::Vis::grayscale(frame); // visualization

	// build a sparse gradient-direction-histogram integral image
	if(matchVect.size())
	{
		MVImg<int> * pFeatMask = new MVImg<int>(pImg->getSize(), 0);
		int ** maskData = pFeatMask->getYXData();
		ImgROI_t featBounds;
		featBounds.xlo = pImg->getWidth()-2;
		featBounds.xhi = 1;
		featBounds.ylo = pImg->getHeight()-2;
		featBounds.yhi = 1;
		for(int iRoi=0; iRoi<matchVect.size(); iRoi++)
		{
			ImgROI_t roi = (matchVect[iRoi])->roi;
			roi.xlo -= 10; roi.ylo -= 10;
			roi.xhi += 10; roi.yhi += 10;
			if(roi.xlo < 1) roi.xlo = 1;
			if(roi.ylo < 1) roi.ylo = 1;
			if(roi.xhi > imgSz.w-2) roi.xhi = imgSz.w-2;
			if(roi.yhi > imgSz.h-2) roi.yhi = imgSz.h-2;
			if(roi.xlo < featBounds.xlo)
				featBounds.xlo = roi.xlo;
			if(roi.ylo < featBounds.ylo)
				featBounds.ylo = roi.ylo;
			if(roi.xhi > featBounds.xhi)
				featBounds.xhi = roi.xhi;
			if(roi.yhi > featBounds.yhi)
				featBounds.yhi = roi.yhi;

			for(int y=roi.ylo; y<=roi.yhi; y++)
				for(int x=roi.xlo; x<=roi.xhi; x++)
					maskData[y][x] = 1;
		}
		pGradHistIntegral = new SparseGradIntegral(pImg, featBounds, pFeatMask);

		int ibestMatch = -1;
		double distMin = 1e6;
		for(int i=0; i<matchVect.size(); i++)
		{
			setDistance(matchVect[i]);
			if(matchVect[i]->dist < distMin)
			{
				ibestMatch = i;
				distMin = matchVect[i]->dist;
			}
			if(VIS)
				MVImgUtils::Vis::outlineROI(frame, (matchVect[i])->roi, 0xffff00);
		}

		if(VIS && -1 != ibestMatch)
			MVImgUtils::Vis::outlineROI(frame, (matchVect[ibestMatch])->roi, 0xff0000);

		if(-1 != ibestMatch && distMin < thresh)
		{
			//cout << "ibestMatch = " << ibestMatch << ", distMin = " << distMin << endl;
			pResult->pHit = new Hit_t;
			pResult->pHit->roi = (matchVect[ibestMatch])->roi;
			pResult->pHit->strength = pResult->pHit->data1 = distMin;
		}


		// clean up
		delete pGradHistIntegral; pGradHistIntegral = 0;
		delete pFeatMask;
	}

	// visualization
	if(VIS)
	{
		char filename[1000];
		sprintf(filename, "out%d.bmp", imgNum);
		ofstream fout(filename, ios::binary );
		MVImgUtils::frameToFile(fout, frame);
		fout.close();
	}

	// cleanup
	delete pImg;
	MVUtils::deleteElements(&matchVect);
	matchVect.clear();

	return pResult;
}

void FaceSearch::getMatches(MVImg<int> * pImg)
{
	if(pIntegralImg) delete pIntegralImg;
	pIntegralImg = new IntegralImg(pImg);

	for(int i=0; i<nScales; i++)
	{
		ImgROI_t mask;
		mask.xlo = 1; mask.ylo = 1;
		mask.xhi = pIntegralImg->getWidth()  - (2+cascadeArr[i].sz.w);
		mask.yhi = pIntegralImg->getHeight() - (2+cascadeArr[i].sz.h);

		vector<Pixel_t *> * pPxVect = cascadeArr[i].runCascade(pIntegralImg, &mask);
		if(pPxVect->size())
		{
			MVImg<int> * pFlagImg = new MVImg<int>(pImg->getSize(), 0);
			int ** pFlagData = pFlagImg->getYXData();
			int xlo = mask.xhi;
			int xhi = mask.xlo;
			int ylo = mask.yhi;
			int yhi = mask.ylo;
			for(int iPx=0; iPx<pPxVect->size(); iPx++)
			{
				int x = (*pPxVect)[iPx]->x;
				int y = (*pPxVect)[iPx]->y;
				pFlagData[y][x] = 1;

				if(x < xlo) xlo = x;
				if(x > xhi) xhi = x;
				if(y < ylo) ylo = y;
				if(y > yhi) yhi = y;
			}

			vector<Region *> regVector;
			RegSearch  regSearch;
			regSearch.setMinW(1);
			regSearch.setMaxW(cascadeArr[i].maxRegWidth);
			regSearch.setMinH(1);
			regSearch.setMaxH(cascadeArr[i].maxRegHeight);
			regSearch.setRegionValue(1);
			regSearch.setYStart(ylo);
			regSearch.setYEnd(yhi);
			regSearch.setXStart(xlo);
			regSearch.setXEnd(xhi);
			regSearch.search(pFlagImg, &regVector);

			for(int iR=0; iR<regVector.size(); iR++)
			{
				// find the ROI
				int regXCtr = (regVector[iR]->getMinX() + regVector[iR]->getMaxX())/2;
				int regYCtr = (regVector[iR]->getMinY() + regVector[iR]->getMaxY())/2;
				ImgROI_t hypoROI;
				hypoROI.xlo = regXCtr; hypoROI.xhi = regXCtr + cascadeArr[i].sz.w;
				hypoROI.ylo = regYCtr; hypoROI.yhi = regYCtr + cascadeArr[i].sz.h;

				refineROI(&hypoROI, cascadeArr[i]);
			}
			MVUtils::deleteElements(&regVector);
			delete pFlagImg;
		}
		MVUtils::deleteAll(&pPxVect);
	}

	if(pIntegralImg) delete pIntegralImg;
	pIntegralImg = 0;
}

double FaceSearch::refineROI(ImgROI_t * pRoi, Cascade_t & currCascade)
{
	double maxDiff = 0;
	double ctrX = (double)(pRoi->xlo + pRoi->xhi) / 2.0;
	double ctrY = (double)(pRoi->ylo + pRoi->yhi) / 2.0;

	ImgROI_t extents;
	extents.xlo = 1; extents.ylo = 1;

	for(int ii=0; ii<currCascade.nFineScales; ii++)
	{
		ImgROI_t scaledROI;
		double halfW = (double)(currCascade.fineScaleArr[ii].sz.w - 1) / 2.0;
		double halfH = (double)(currCascade.fineScaleArr[ii].sz.h - 1) / 2.0;

		extents.xhi = imgSz.w - 1 - currCascade.fineScaleArr[ii].sz.w;
		extents.yhi = imgSz.h - 1 - currCascade.fineScaleArr[ii].sz.h;

		scaledROI.xlo = ctrX - halfW;
		scaledROI.xhi = ctrX + halfW;
		scaledROI.ylo = ctrY - halfH;
		scaledROI.yhi = ctrY + halfH;
		if(scaledROI.xlo >= 1 && scaledROI.ylo >= 1 &&
			scaledROI.xhi < imgSz.w-1 &&
			scaledROI.yhi < imgSz.h-1 )
		{
			double best4Scale = refineLoc
				(&scaledROI, currCascade.fineScaleArr[ii].dipoleVect, extents);

			Match_t * pMatch = new Match_t;
			pMatch->roi = scaledROI;
			pMatch->pScaleData = &(currCascade.fineScaleArr[ii]);
			matchVect.push_back(pMatch);

			if(best4Scale > maxDiff)
				maxDiff = best4Scale;
		}
	}

	return maxDiff;
}


double FaceSearch::refineLoc(
	ImgROI_t * pROIStart,
	vector<CascadeDipole_t *> & dipoleVect,
	ImgROI_t & extents
) {
	double maxDiff = 0;
	double diff, diff2;
	double dxPlus, dxMinus, dyPlus, dyMinus;
	Pixel_t px;
	bool atLocalMax = true;

	px.x = pROIStart->xlo;
	px.y = pROIStart->ylo;
	diff = getTotDiff(px, dipoleVect);

	// determine next x
	++px.x;
	if(px.x <= extents.xhi)
	{
		diff2 = getTotDiff(px, dipoleVect);
		dxPlus = diff2 - diff;
	}
	else
		dxPlus = 1; // an arbitrary value > 0
	if(dxPlus <= 0)
	{
		// look at dxMinus
		px.x -= 2;
		if(px.x >= extents.xlo)
		{
			diff2 = getTotDiff(px, dipoleVect);
			dxMinus = diff2 - diff;
		}
		else
			dxMinus = 1; // an arbitrary value > 0
		if(dxMinus <= 0)
			++px.x; // restore original x position
		else
			atLocalMax = false;
	}
	else
		atLocalMax = false;

	// determine next y
	++px.y;
	if(px.y <= extents.yhi)
	{
		diff2 = getTotDiff(px, dipoleVect);
		dyPlus = diff2 - diff;
	}
	else
		dyPlus = 1; // an arbitrary value > 0
	if(dyPlus <= 0)
	{
		// look at dyMinus
		px.y -= 2;
		if(px.y >= extents.ylo)
		{
			diff2 = getTotDiff(px, dipoleVect);
			dyMinus = diff2 - diff;
		}
		else
			dyMinus = 1; // an arbitrary value > 0
		if(dyMinus <= 0)
			++px.y; // restore y position
		else