homebase.cpp

机器人程序

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

//////////////////////////////////////////////////////////////
// Implementation of Homebase and HomebaseSighting classes
//
// This is a completely hand-coded recognition strategy. It relies
// somewhat on the expected contest conditions, and it specifically
// looks for features known to be distinctive in the finish circle.
// This means it's fairly well optimized for the contest, but will
// not generalize well for other objects. Also, this approach is not
// amenable to being learned.
//

#include "Homebase.h"
#include "Mavis.h"
#include "../camera/Camera.h"
#include "SubpixelEdge.h"
#include <memory.h>
#include <math.h>
#include "../Params.h"
#include "../Logger.h"
#include "../mavistypes.h"


//////////////////////////////////////////////////////////////
// HomebaseSighting class
//

HomebaseSighting::HomebaseSighting()
{
	pTopCurve  = pBottomCurve  = NULL;
	pTopRegion = pBottomRegion = NULL;
	confidence = 0;
}

HomebaseSighting::~HomebaseSighting()
{
	if(pTopCurve)     delete pTopCurve;
	if(pBottomCurve)  delete pBottomCurve;
	if(pTopRegion)    delete pTopRegion;
	if(pBottomRegion) delete pBottomRegion;
}


//////////////////////////////////////////////////////////////
// Homebase class
//

//////////////////
//  constructor
//
Homebase::Homebase(Mavis * pM) : MVObjBase(pM)
{
	pGyImg = NULL;

	// defaults
	int w = pM->getImgWidth();
	minWidth = 5;
	maxWidth = (int)( (double)w/3.0 );

	// if possible, refine these based on camera params
	Camera * pCamera = pMavis->getCamera();
	if( pCamera->getIsCalibrated() )
	{
		double scale;
		bool isValid;
		Pixel_t pix; pix.x = 0; // x value doesn't matter for this
		whiteDiam = pMavis->getParams()->
			getFloatPtValue("homebase", "whiteDiam", &isValid);
		if(!isValid) whiteDiam = 0;

		// max value (closest in image)
		pix.y = -5;
		if( isValid && pCamera->getScale(&pix, 0, HEIGHT_CONSTRAINT, &scale) )
			maxWidth = (int)(0.5 + scale*whiteDiam*1.1); // allow 10% over

		// min value (furthest in image)
		pix.y = pMavis->getImgHeight() + 5;
		if( isValid && pCamera->getScale(&pix, 0, HEIGHT_CONSTRAINT, &scale) )
			minWidth = (int)(0.5 + scale*whiteDiam*0.8); // allow 20% under
	}

	pMavis->getLogger()->writelnToLog(
		"Homebase object:\n"
		"\tminWidth = %d, maxWidth = %d", minWidth, maxWidth);
}


//////////////////
//  destructor
//
Homebase::~Homebase()
{
	// free any Sighting objects
	MVUtils::deleteElements(&SightingsVector);

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


//////////////////
//  locatePrecisely()
//
void Homebase::locatePrecisely(ObjSighting_t * pObjSighting)
{
	memset(pObjSighting, 0, sizeof(ObjSighting_t));
	pObjSighting->objId = HOMEBASE_OBJ_ID;
	locate();

	// keep only one sighting
	cullMultipleSightings();

	if( SightingsVector.size() )
	{
		if(SightingsVector[0]->pBottomCurve && SightingsVector[0]->pTopCurve)
		{
			// create an x-gradient image
			VideoFrame * pFrame = pMavis->getNextFrame();
			MVImg<int> * pGxImg = 0;
			MVImgUtils::frame2grayscaleImg(*pFrame, &pGxImg);
			//pMavis->getLogger()->writelnToLog(
			//	"homebase: frameWidth = %d, frameHeight = %d", pFrame->getWidth(), pFrame->getHeight());

			MVImgUtils::ImgProc::gx(pGxImg);
			threshold(pGxImg);

			// find left-side curve
			vector<Region *> * pRegVector = new vector<Region *>();

			regSearch.reset();

			regSearch.setMinW(4);
			regSearch.setMaxW(100);
			regSearch.setMinH(10);
			regSearch.setMaxH(100);
			regSearch.setRegionValue(1);
			regSearch.setYStart( SightingsVector[0]->pBottomCurve->getYMin() );
			regSearch.setYEnd( SightingsVector[0]->pTopCurve->getYMax() );
			regSearch.setXStart( SightingsVector[0]->pTopCurve->getXLo() - 50 );
			regSearch.setXEnd( SightingsVector[0]->pTopCurve->getXLo() + 10 );
			regSearch.search(pGxImg, pRegVector);

			XofYCurve * pLeft = NULL;
			Region * pLeftReg = getFirstRegDeleteOthers(pRegVector);

			if(pLeftReg)
				pLeft = new XofYCurve( pLeftReg );


			// find right-side curve
			regSearch.setRegionValue(-1);
			regSearch.setXStart( SightingsVector[0]->pTopCurve->getXHi() - 10 );
			regSearch.setXEnd( SightingsVector[0]->pTopCurve->getXHi() + 50 );
			regSearch.search(pGxImg, pRegVector);

			XofYCurve * pRight = NULL;
			Region * pRightReg = getFirstRegDeleteOthers(pRegVector);
			if(pRightReg)
				pRight = new XofYCurve( pRightReg );


			// get precision values for x, y, height, and width
			if( pLeftReg && pRightReg && SightingsVector[0]->pTopRegion && SightingsVector[0]->pBottomRegion)
			{
				pObjSighting->prob = 100;

				XofYSubpixelEdge * pPrecLeft = new XofYSubpixelEdge(pLeftReg, pGxImg);
				XofYSubpixelEdge * pPrecRight = new XofYSubpixelEdge(pRightReg, pGxImg);
				pObjSighting->width = pPrecRight->getXMax() - pPrecLeft->getXMin();
				pObjSighting->x = (pPrecRight->getXMax() + pPrecLeft->getXMin()) / 2.0;
				delete pPrecLeft;
				delete pPrecRight;

				YofXSubpixelEdge * pPrecTop = new YofXSubpixelEdge(SightingsVector[0]->pTopRegion, pGxImg);
				YofXSubpixelEdge * pPrecBottom = new YofXSubpixelEdge(SightingsVector[0]->pBottomRegion, pGxImg);
				pObjSighting->height = pPrecTop->getYMax() - pPrecBottom->getYMin();
				pObjSighting->y = (pPrecTop->getYMax() + pPrecBottom->getYMin()) / 2.0;
				delete pPrecTop;
				delete pPrecBottom;

				SightingsVector[0]->px.y = (int)(0.5 + pObjSighting->y);
				SightingsVector[0]->px.x = (int)(0.5 + pObjSighting->x);
			}


			// debug stuff
			//////////////////////
			/*
			IntImg * pTestImg = new IntImg(240, 320, 0);
			addCurveToImg(SightingsVector[0]->pTopCurve, pTestImg);
			addCurveToImg(SightingsVector[0]->pBottomCurve, pTestImg);
			if(pLeft) addCurveToImg(pLeft, pTestImg);
			if(pRight) addCurveToImg(pRight, pTestImg);
			//test(rgbBuf, pTestImg); // old visualization method
			//VisUtil::grayscale2frame(&frame, pTestImg); //whoops, this isn't the same as above!
			RGBTuple rgb(255,0,0);
			pMavis->markLoc(&(SightingsVector[0]->px), &rgb, 20, 20);
			delete pTestImg;
			*/
			//////////////////////
			// end - debug stuff


			// free resources
			if(pLeft) delete pLeft;
			if(pRight) delete pRight;
			if(pLeftReg) delete pLeftReg;
			if(pRightReg) delete pRightReg;
			delete pGxImg;
			delete pRegVector;
		}
	}

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


//////////////////
//  FUNCTION:   lookOnce()
//
int Homebase::lookOnce(ObjLoc_t * pObjLoc)
{
	memset(pObjLoc, 0, sizeof(ObjLoc_t));
	locate();
	if(pGyImg) delete pGyImg;
	pGyImg = NULL;

	// keep only one sighting
	cullMultipleSightings();

	// translate to world coordinates
	setObjLoc(pObjLoc);

	// mark the center
	if( SightingsVector.size() )
		pMavis->markLoc(SightingsVector[0]->px, 0xff0000, 20, 20);


	if(pGyImg) delete pGyImg;
	pGyImg = NULL;

	return 0;
}


//////////////////
//  locate()
//
void Homebase::locate()
{
	// free any Sighting objects leftover in the vector
	vector<HomebaseSighting *>::iterator pSV;
	pSV = SightingsVector.begin();
	while( pSV != SightingsVector.end() )
	{
		delete (*pSV);
		pSV = SightingsVector.erase(pSV);
	}

	vector<Region *> * pRegVector = new vector<Region *>();
	regSearch.reset();

	// capture a video frame
	VideoFrame * pFrame = pMavis->getNextFrame();
	MVImg<int> * pGyImg = 0;
	MVImgUtils::frame2grayscaleImg(*pFrame, &pGyImg);
	//pMavis->getLogger()->writelnToLog(
	//			"homebase: frameWidth = %d, frameHeight = %d", pFrame->getWidth(), pFrame->getHeight());

	// use only the vertical gradient for this
	MVImgUtils::ImgProc::gy(pGyImg);

	// threshold the gradient
	threshold(pGyImg);

	// find regions that might be the top of the circle
	setTopSearch();
	regSearch.search(pGyImg, pRegVector);

	storeValidTops(pRegVector);

	// set up to search for bottom of circle
	setBottomSearch();

	if( SightingsVector.size() )
	{
		// search each sighting for a matching bottom
		for(int iS=0; iS<SightingsVector.size(); iS++)
		{
			HomebaseSighting * pSighting = SightingsVector[iS];

			regSearch.setYStart( pSighting->pTopCurve->getYMin() - 40 );
			regSearch.setYEnd( pSighting->pTopCurve->getYMin() );
			regSearch.setXStart( pSighting->pTopCurve->getXLo() );
			regSearch.setXEnd( pSighting->pTopCurve->getXHi() );
			regSearch.search(pGyImg, pRegVector);

			storeValidBottomMatch(pRegVector, pSighting);
		}
	}
	else
	{
		// search entire image for bottoms
		regSearch.search(pGyImg, pRegVector);
		storeValidBottoms(pRegVector);
	}


	// free resources
	vector<Region *>::iterator p = pRegVector->begin();
	while( p != pRegVector->end() )
	{
		Region * pRegion = *p;
		p = pRegVector->erase(p);
		delete pRegion;
	}
	delete pRegVector;
}


//////////////////
//  getFirstRegDeleteOthers()
//
Region * Homebase::getFirstRegDeleteOthers(vector<Region *> * pRegVector)
{
	Region * pRegion = NULL;
	vector<Region *>::iterator p;

	if(pRegVector->size())
	{
		p = pRegVector->begin();
		pRegion = *p;
		p = pRegVector->erase(p);
		while( p != pRegVector->end() )
		{
			Region * pRegion = *p;
			p = pRegVector->erase(p);
			delete pRegion;
		}
	}

	return pRegion;
}


//////////////////
//  storeValidBottomMatch()
//
void Homebase::storeValidBottomMatch(vector<Region *> * pRegVector, HomebaseSighting * pSighting) {
	if( !pRegVector->size() ) return;

	// look only at first region in vector
	Region * pRegion = getFirstRegDeleteOthers(pRegVector);
	// use first one with correct scale
	bool scaleIsOk = scaleIsCorrect(pRegion);

	// see if it's a valid bottom curve
	YofXCurve * pCurve = new YofXCurve(pRegion);
	if( scaleIsOk && isAValidBottom(pCurve) )
	{
		pSighting->pBottomCurve  = pCurve;
		pSighting->pBottomRegion = pRegion;
		pSighting->confidence += 50;
	}
	else
	{
		delete pCurve;
		delete pRegion;
	}
}


//////////////////
//  FUNCTION:   storeValidBottoms()
//
void Homebase::storeValidBottoms(vector<Region *> * pRegVector)
{
	if( !pRegVector->size() ) return;

	vector<Region *>::iterator p = pRegVector->begin();
	while( p != pRegVector->end() )
	{
		Region * pRegion = *p;
		YofXCurve *  pCurve  = new YofXCurve(pRegion);
		p = pRegVector->erase(p);

		if( scaleIsCorrect(pRegion) && isAValidBottom(pCurve) )
		{
			// create a sighting object using pCurve
			// and add it to sightings vector
			HomebaseSighting * pSighting = new HomebaseSighting();
			pSighting->pBottomCurve  = pCurve;
			pSighting->pBottomRegion = pRegion;
			pSighting->confidence += 50;
			SightingsVector.push_back(pSighting);
		}
		else
		{
			delete pCurve;
			delete pRegion;
		}
	}
}


//////////////////
//  isAValidBottom()
//
bool Homebase::isAValidBottom(YofXCurve * pCurve)
{
	int nPts   = pCurve->getNPoints();
	int yleft  = pCurve->getYAtIndex(0);
	int yright = pCurve->getYAtIndex(nPts-1);
	int ymid   = pCurve->getYAtIndex(nPts>>1);

	if(ymid < yleft && ymid < yright)
	{
		if( pCurve->jags() > 5 ) return false;
		return true;
	}
	else
		return false;
}


//////////////////
//  isAValidTop()
//
bool Homebase::isAValidTop(YofXCurve * pCurve) {