homebase.cpp

机器人程序

	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;
}


//////////////////
//  storeValidTops()
//
void Homebase::storeValidTops(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) && isAValidTop(pCurve) )
		{
			// create a sighting object using pCurve
			// and add it to sightings vector
			HomebaseSighting * pSighting = new HomebaseSighting();
			pSighting->pTopCurve  = pCurve;
			pSighting->pTopRegion = pRegion;
			pSighting->confidence += 50;
			SightingsVector.push_back(pSighting);
		}
		else
		{
			delete pCurve;
			delete pRegion;
		}
	}
}


//////////////////
//  scaleIsCorrect()
//
bool Homebase::scaleIsCorrect(Region * pReg)
{
	Camera * pCamera = pMavis->getCamera();
	int isCalibrated = pCamera->getIsCalibrated();

	bool scaleIsOk = true;
	if(isCalibrated)
	{
		// validate scale based on image location
		double scale;
		if(whiteDiam > 0)
		{
			Pixel_t pix;
			pix.x = (int)(0.5 +
				( (double)pReg->getMinX() + (double)pReg->getMaxX() )
				/ 2.0);
			pix.y = pReg->getMinY();
			if( pCamera->getScale(&pix, 0, HEIGHT_CONSTRAINT, &scale) )
			{
				double w = (double)( pReg->getWidth() );
				if( w < 0.8*whiteDiam*scale ) scaleIsOk = false;
				else if( w > 1.1*whiteDiam*scale) scaleIsOk = false;
			}
		}
	}

	return scaleIsOk;
}


//////////////////
//  setTopSearch()
//
void Homebase::setTopSearch() {

	regSearch.setMinW(minWidth);
	regSearch.setMaxW(maxWidth);
	regSearch.setMinH(3);
	regSearch.setMaxH(maxWidth/2);
	regSearch.setRegionValue(-1);
}


//////////////////
//  setBottomSearch()
//
void Homebase::setBottomSearch() {

	regSearch.setMinW(minWidth);
	regSearch.setMaxW(maxWidth);
	regSearch.setMinH(3);
	regSearch.setMaxH(maxWidth/2);
	regSearch.setRegionValue(1);
}


//////////////////
//  curveMax()
//
int Homebase::curveMax(YofXCurve * pCurve) {
	double avgX = 0;
	double nX = 0;
	int nPts = pCurve->getNPoints();
	int yMax = pCurve->getYMax();
	int xLo = pCurve->getXLo();

	for(int i=0; i<nPts; i++)
	{
		int y = pCurve->getYAtIndex(i);
		if( y == yMax )
		{
			avgX += (double)(xLo + i);
			nX++;
		}
	}

	return (int)(0.5 + avgX/nX);
}


//////////////////
//  curveMin()
//
int Homebase::curveMin(YofXCurve * pCurve) {
	double avgX = 0;
	double nX = 0;
	int nPts = pCurve->getNPoints();
	int yMin = pCurve->getYMin();
	int xLo = pCurve->getXLo();

	for(int i=0; i<nPts; i++)
	{
		int y = pCurve->getYAtIndex(i);
		if( y == yMin )
		{
			avgX += (double)(xLo + i);
			nX++;
		}
	}

	return (int)(0.5 + avgX/nX);
}


//////////////////
//  setPixel()
//
void Homebase::setPixel(HomebaseSighting * pSighting)
{
	int x, y;

	if( pSighting->pTopCurve && pSighting->pBottomCurve )
	{
		x = (( curveMax(pSighting->pTopCurve) + curveMin(pSighting->pBottomCurve) )>>1);
		y = ( pSighting->pTopCurve->getYMax() + pSighting->pBottomCurve->getYMin() )>>1;
	}
	else if( pSighting->pTopCurve )
	{
		x = (pSighting->pTopCurve->getXHi() + pSighting->pTopCurve->getXLo())>>1;
		y = pSighting->pTopCurve->getYMin();
	}
	else if( pSighting->pBottomCurve )
	{
		x = (pSighting->pBottomCurve->getXHi() + pSighting->pBottomCurve->getXLo())>>1;
		y = pSighting->pBottomCurve->getYMax();
	}
	else
	{
		x = y = 0;
	}

	pSighting->px.x = x;
	pSighting->px.y = y;
}


//////////////////
//  setObjLoc()
//
void Homebase::setObjLoc(ObjLoc_t * pObjLoc)
{
	Camera * pCamera = pMavis->getCamera();
	WorldCoord wc;

	vector<HomebaseSighting *>::iterator p = SightingsVector.begin();
	while( p != SightingsVector.end() )
	{
		setPixel(*p);
		wc = pCamera->screen2world( (*p)->px.x, (*p)->px.y );
		pObjLoc->angle = wc.angle;
		pObjLoc->dist  = wc.distance;
		pObjLoc->prob  = (*p)->confidence;
		p++;
	}

	pObjLoc->nFound = SightingsVector.size();
}


//////////////////
//  cullMultipleSightings()
//
void Homebase::cullMultipleSightings()
{
	if( SightingsVector.size() < 2 ) return;

	vector<HomebaseSighting *>::iterator p;
	int iS;

	// eliminate all but highest-confidence sightings
	int bestProb = 0;
	for(iS=0; iS<SightingsVector.size(); iS++)
		if(SightingsVector[iS]->confidence > bestProb)
			bestProb = SightingsVector[iS]->confidence;

	p = SightingsVector.begin();
	while( p != SightingsVector.end() )
	{
		HomebaseSighting * pSighting = *p;
		if( pSighting->confidence < bestProb)
		{
			delete pSighting;
			p = SightingsVector.erase(p);
		}
		else
			p++;
	}
	if( SightingsVector.size() < 2 ) return;

	// eliminate sightings furthest from center
	int w = pMavis->getImgWidth();
	int h = pMavis->getImgHeight();
	int halfW = w>>1;
	int halfH = h>>1;
	int leastDist2 = w*w + h*h;
	for(iS=0; iS<SightingsVector.size(); iS++)
	{
		setPixel(SightingsVector[iS]);
		int x = SightingsVector[iS]->px.x - halfW;
		int y = SightingsVector[iS]->px.y - halfH;
		int dist2 = x*x + y*y;
		if(dist2 < leastDist2) leastDist2 = dist2;
	}
	p = SightingsVector.begin();
	while( p != SightingsVector.end() )
	{
		HomebaseSighting * pSighting = *p;
		int x = pSighting->px.x - halfW;
		int y = pSighting->px.y - halfH;
		int dist2 = x*x + y*y;
		if(dist2 < leastDist2)
		{
			delete pSighting;
			p = SightingsVector.erase(p);
		}
		else
			p++;
	}
	if( SightingsVector.size() < 2 ) return;

	// if there's still more than one, arbitrarily select the first one
	p = SightingsVector.begin();
	p++;
	while( p != SightingsVector.end() )
	{
		HomebaseSighting * pSighting = *p;
		delete pSighting;
		p = SightingsVector.erase(p);
	}
}


//////////////////
// threshold()
//
void Homebase::threshold(MVImg<int> * pImg)
{
	double avgMag = 0;
	int sum = 0;
	int nVals = 0;
	int max = 0;
	int * data = pImg->getData();
	int w = pImg->getWidth();
	int h = pImg->getHeight();
	int nPixels = w*h;
	int iPx;

	// Find the average magnitude of pixels that have
	// a non-zero gradient.
	for(iPx=0; iPx<nPixels; iPx++)
	{
		if(data[iPx] > 0)
		{
			sum += data[iPx];
			nVals++;
			if(data[iPx] > max) {max = data[iPx];}
		}
		else if(data[iPx] < 0)
		{
			sum -= data[iPx];
			nVals++;
			if(-data[iPx] > max) {max = -data[iPx];}
		}
	}
	avgMag = (double)sum / (double)nVals;

	// tHiParam is a magic number. It gives us a crude but quick
	// thresholding based on the average computed above
	double tHiParam = 0.2;
	int th = avgMag + tHiParam*((double)max - avgMag);

	// Apply the threshold
	//
	// There are 3 categories: 1) magnitude is above threshold and sign
	//    is positive, 2) magnitude is above threshold and sign is negative,
	//    3) magnitude is below threshold.
	for(iPx=0; iPx<nPixels; iPx++) {
		if(data[iPx] > th) data[iPx] = 1;
		else if(-data[iPx] > th) data[iPx] = -1;
		else data[iPx] = 0;
	}
}

////////////// some debug stuff ////////////////

void Homebase::addCurveToImg(YofXCurve * pCurve, MVImg<int> * pImg, int val)
{
	int ** yxData = pImg->getYXData();
	int xlo = pCurve->getXLo();

	for(int ipt=0; ipt<pCurve->getNPoints(); ipt++)
	{
		int x = xlo + ipt;
		int y = pCurve->getYAtIndex(ipt);
		yxData[y][x] = val;
	}
}

void Homebase::addCurveToImg(XofYCurve * pCurve, MVImg<int> * pImg, int val)
{
	int ** yxData = pImg->getYXData();
	int ylo = pCurve->getYLo();

	for(int ipt=0; ipt<pCurve->getNPoints(); ipt++)
	{
		int y = ylo + ipt;
		int x = pCurve->getXAtIndex(ipt);
		yxData[y][x] = val;
	}
}

// todo: use VisUtil for this
//
void Homebase::test(BYTE * rgbData, MVImg<int> * pImg)
{
	int w = pImg->getWidth();
	int h = pImg->getHeight();
	int nPixels = w*h;
	int * data = pImg->getData();
	int iBuf, iPx;
	int red, blue, green;

	for(iPx=0; iPx<nPixels; iPx++)
	{
		// rgbData uses 3 bytes per pixel
		iBuf = (iPx<<1) + iPx;

		if(data[iPx] > 0)
		{
			red = green = 255;
			blue = 0;
		}
		else if(data[iPx] < 0)
		{
			blue = green = 255;
			red = 0;
		}
		else
		{
			red = blue = green =
				(int)((0.5 +
				(double)rgbData[iBuf+RED_OFFSET_24] +
				(double)rgbData[iBuf+BLUE_OFFSET_24] +
				(double)rgbData[iBuf+GREEN_OFFSET_24]) / 3.0);
		}

		rgbData[iBuf+RED_OFFSET_24] = red;
		rgbData[iBuf+BLUE_OFFSET_24] = blue;
		rgbData[iBuf+GREEN_OFFSET_24] = green;
	}
}

///////////////////////////////////////////////////////////////////////////////////////
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this
// license. If you do not agree to this license, do not download, install, copy or
// use the software.
//
//
//                        Mavis License Agreement
//
// Copyright (c) 2004-2005, Robin Hewitt (http://www.robin-hewitt.com).
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistributions in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
// This software is provided "as is" and any express or implied warranties, including,
// but not limited to, the implied warranties of merchantability and fitness for a
// particular purpose are disclaimed. In no event shall the authors or contributors be
// liable for any direct, indirect, incidental, special, exemplary, or consequential
// damages (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused and on any
// theory of liability, whether in contract, strict liability, or tort (including
// negligence or otherwise) arising in any way out of the use of this software, even
// if advised of the possibility of such damage.
///////////////////////////////////////////////////////////////////////////////////////