hlines.cpp

机器人程序

	double z1 = z2 + distTraveled;
	//pLogger->writelnToLog("\t\ty = %.2f", y);
	//pLogger->writelnToLog("\t\tz2 = %.2f, z1 = %.2f", z2, z1);


	// Precompute data for phi1 and phi2
	double t7 = y * sinTheta;  // C
	double t8 = y * cosTheta;  // D
	double phi1;
	double phi2;
	double m1, hypot1;
	double m2, hypot2;

	// Compute phi1
	{
		       m1          = pLine1->getSlope();
		double xEdgeImg    = (m1 > 0)? 0 : w;
		double vEdgeImg    = pLine1->getYAtX(xEdgeImg) - halfHeight;
		double zEdgeWorld  = f*t8 - vEdgeImg*t7;
			   zEdgeWorld /= (vEdgeImg*cosTheta + f*sinTheta);
		double dx          = (xEdgeImg-halfWidth)/f;  // = du/f
			   dx         *= (zEdgeWorld*cosTheta + t7);
		double dz          = z1 - zEdgeWorld;
		       hypot1      = sqrt(dx*dx + dz*dz);
			   phi1        = asin(dx/hypot1);

		//pLogger->writelnToLog("\t\tdx=%.4f, dz=%.4f", dx, dz);
	}

	// Compute phi2
	{
		       m2          = pLine2->getSlope();
		double xEdgeImg    = (m2 > 0)? 0 : w;
		double vEdgeImg    = pLine2->getYAtX(xEdgeImg) - halfHeight;
		double zEdgeWorld  = f*t8 - vEdgeImg*t7;
			   zEdgeWorld /= (vEdgeImg*cosTheta + f*sinTheta);
		double dx          = (xEdgeImg-halfWidth)/f;  // = du/f
			   dx         *= (zEdgeWorld*cosTheta + t7);
		double dz          = z2 - zEdgeWorld;
		       hypot2      = sqrt(dx*dx + dz*dz);
			   phi2        = asin(dx/hypot2);

		//pLogger->writelnToLog("\t\tdx=%.4f, dz=%.4f", dx, dz);
	}


	// validate
	double err = fabs( hypot1*sin(phi1) - hypot2*sin(phi2) );
	double pctErr = 200.0 * err/(z1+z2);
	pLogger->writelnToLog("\n\t\tpctErr = %.3g, maxPctError = %.2f", pctErr, maxPctError);
	if(pctErr > maxPctError) return (HLine_t *)NULL;


	// fill in 3D line data
	pHLine = new HLine_t;


	// do the In-Path anaylsis
	WorldLine_t * pWorldLine = new WorldLine_t;
	pWorldLine->phi1 = phi1;
	pWorldLine->phi2 = phi2;
	pWorldLine->y = y;
	pWorldLine->z1 = z1;
	pWorldLine->z2 = z2;

	getInPathStatus(pLine1, pLine2, pWorldLine);
	pHLine->inPathVis = pWorldLine->inPathVis;
	pHLine->inPathProb   = pWorldLine->inPathProb;

	delete pWorldLine;


	pHLine->pctError = pctErr;

	double halfErr = err/2.0;
	pHLine->distance.lo = z2 - halfErr;
	pHLine->distance.hi = z2 + halfErr;

	pHLine->height.lo = (hc + y) - halfErr;
	pHLine->height.hi = (hc + y) + halfErr;


	// from phi1 and phi2, find the rotation angle to face toward line
	// return values for phi are in degrees
	phi1        = 180.0 * phi1 / PI;
	phi2        = 180.0 * phi2 / PI;
	phi1        = (m1 > 0)? -(90+phi1) : 90-phi1;
	phi2        = (m2 > 0)? -(90+phi2) : 90-phi2;

	//pLogger->writelnToLog("\t\tphi1=%.4f", phi1);
	//pLogger->writelnToLog("\t\tphi2=%.4f", phi2);


	if( fabs(phi1) > fabs(phi2) )
	{
		pHLine->angle.lo = phi2;
		pHLine->angle.hi = phi1;
	}
	else
	{
		pHLine->angle.lo = phi1;
		pHLine->angle.hi = phi2;
	}


	return pHLine;
}


//////////////////
//  getInPathStatus
//
void HLines::getInPathStatus(YofXLineSeg * pLine1, YofXLineSeg * pLine2, WorldLine_t * pWorldLine)
{
	// Start by assuming the worst
	pWorldLine->inPathVis = TRUE;
	pWorldLine->inPathProb = 100;

	if(pLine1->getXPixelLo() <= halfWidth && pLine1->getXPixelHi() >= halfWidth)
		return;

	if(pLine2->getXPixelLo() <= halfWidth && pLine2->getXPixelHi() >= halfWidth)
		return;


	// line segment is to one side of the centerline
	double robotImgWidth;
	int robotHalfWidthImg;
	double zCamera;


	// Look for a crossing in frame 2
	    zCamera = pWorldLine->z2*cosTheta - pWorldLine->y*sinTheta;
	    robotImgWidth = f*robotWidth / zCamera;
	    robotHalfWidthImg = (int)((robotImgWidth + 0.5)/2.0);
	int xLoRobot2 = halfWidth - robotHalfWidthImg;
	int xHiRobot2 = halfWidth + robotHalfWidthImg;

	if(pLine2->getXPixelLo() < xLoRobot2 && pLine2->getXPixelHi() > xLoRobot2)
		return;

	if(pLine2->getXPixelLo() < xHiRobot2 && pLine2->getXPixelHi() > xHiRobot2)
		return;


	// Look for a crossing in frame 1
	    zCamera = pWorldLine->z1*cosTheta - pWorldLine->y*sinTheta;
	    robotImgWidth = f*robotWidth / zCamera;
	    robotHalfWidthImg = (int)((robotImgWidth + 0.5)/2.0);
	int xLoRobot1 = halfWidth - robotHalfWidthImg;
	int xHiRobot1 = halfWidth + robotHalfWidthImg;

	if(pLine1->getXPixelLo() < xLoRobot1 && pLine1->getXPixelHi() > xLoRobot1)
		return;

	if(pLine1->getXPixelLo() < xHiRobot1 && pLine1->getXPixelHi() > xHiRobot1)
		return;

	// See if crossing would be outside the image frame
	double yCrossing;

	// above the top in frame 1
	if(pLine1->getSlope() > 0)
		yCrossing = h - pLine1->getYPixelAtX(xLoRobot1);
	else
		yCrossing = h - pLine1->getYPixelAtX(xHiRobot1);

	//if(yCrossing > h - gausKernSize - 2)
	if(yCrossing > h - 2)
	{
		pWorldLine->inPathVis = FALSE;
		return;
	}

	// below the bottom in frame 2
	if(pLine2->getSlope() > 0)
		yCrossing = pLine2->getYPixelAtX(xHiRobot2);
	else
		yCrossing = pLine2->getYPixelAtX(xLoRobot2);

	//if(yCrossing < gausKernSize+1)
	if(yCrossing < 1)
	{
		pWorldLine->inPathVis = FALSE;
		return;
	}


	// Line does not visibly extend into the robot's path
	// For now, stop at this and say the path is clear.

	//todo: analyze intersection region for evidence that the
	// detected line segment extends further.

	pWorldLine->inPathProb = 0;
}


//////////////////
//  matchLines
//
void HLines::matchLines (
	vector<YofXLineSeg *> * pLinesVector1,
	vector<YofXLineSeg *> * pLinesVector2,
	vector<LineMatch_t *> * pPairsVector
) {
	int nLines = pLinesVector1->size();
	for(int i=0; i<nLines; i++)
	{
		YofXLineSeg * pLine1 = (*pLinesVector1)[i];
		int j = bestLineMatch(pLine1, pLinesVector2);
		if(j > -1)
		{
			YofXLineSeg * pLine2 = (*pLinesVector2)[j];
			int i2 = bestLineMatch(pLine2, pLinesVector1);
			if(i2 == i)
			{
				LineMatch_t * pLineMatch = new LineMatch_t;
				pLineMatch->i = i;
				pLineMatch->j = j;
				pPairsVector->push_back(pLineMatch);
			}
		}
	}
}


//////////////////
//  bestLineMatch
//
int HLines::bestLineMatch
	(YofXLineSeg * pLine1, vector<YofXLineSeg *> * pLineVector)
{
	int iBest    = -1;
	int d1Best   = maxEndPtShift + 1;
	int d2Best   = maxEndPtShift + 1;
	int dTotBest = d1Best + d2Best;
	int nLines   = pLineVector->size();

	for(int i=0; i<nLines; i++)
	{
		YofXLineSeg * pLine2 = (*pLineVector)[i];
		int d1 = abs( pLine1->getXPixelLo() - pLine2->getXPixelLo() );
		int d2 = abs( pLine1->getXPixelHi() - pLine2->getXPixelHi() );
		double slopeChange = fabs( pLine1->getSlope() - pLine2->getSlope() );
		if(d1 <= maxEndPtShift && d2 <= maxEndPtShift && slopeChange <= maxSlopeChange)
		{
			if( (d1+d2) < dTotBest )
			{
				iBest = i;
				d1Best = d1;
				d2Best = d2;
				dTotBest = d1 + d2;
			}
		}
	}

	return iBest;
}


//////////////////
//  drawLinesOnImage
//
void HLines::drawLinesOnImage (
	VideoFrame * pFrame,
	vector<YofXLineSeg *> * pLinesVector,
	int rgb
) {
	vector<YofXLineSeg *>::iterator p;
	p = pLinesVector->begin();
	while( p != pLinesVector->end() )
	{
		int x1 = (*p)->getXPixelLo();
		int y1 = (*p)->getYPixelAtX(x1);
		int x2 = (*p)->getXPixelHi();
		int y2 = (*p)->getYPixelAtX(x2);

		MVImgUtils::Vis::line (
			*pFrame,
			x1, y1, x2, y2,
			rgb
		);

		p++;
	}
}


//////////////////
//  findProminentHLines
//
void HLines::findProminentHLines(
		MVImg<int> * pGrayImg,
		vector<YofXLineSeg *> * pGVPos, vector<YofXLineSeg *> * pGVNeg
) {
	int w = pGrayImg->getWidth();
	int h = pGrayImg->getHeight();

	// vertical gradients
	MVImg<int> * pGY = pGrayImg->clone();
	MVImgUtils::ImgProc::gy(pGY);

	// filter GY by GX
	MVImg<int> * pGX = pGrayImg->clone();
	MVImgUtils::ImgProc::gx(pGX);
	filterByGX(pGY, pGX);
	delete pGX;

	// threshold GY
	thresholdGY(pGY);


	// find GYPos lines
	findLines(pGY, 1, pGVPos);


	// find GYNeg lines
	findLines(pGY, -1, pGVNeg);
}


//////////////////
//  findLines
//
void HLines::findLines(
	MVImg<int> * pGY, int gradDir, vector<YofXLineSeg *> * pLinesVector
) {
	// find regions
	vector<Region *> regVector;
	RegSearch regSearch;
	regSearch.setRegionValue(gradDir);
	regSearch.setMinW(minLineDX);
	regSearch.search(pGY, &regVector);
	regSearch.setXStart(1);
	regSearch.setXEnd(w - 2);
	regSearch.setYStart(1);
	regSearch.setYEnd(h - 2);

	// find best-fit lines for each region
	vector<Region *>::iterator p;
	p = regVector.begin();
	while( p != regVector.end() )
	{
		// create a line segment for each region
		YofXLineSeg * pImgLineSeg = new YofXLineSeg(*p);

		// make sure region is a straight line
		if(pImgLineSeg->getLineErr() > maxLeastSquaresErr)
			delete pImgLineSeg;
		else
		{
			// make sure it's not too short
			double x1 = pImgLineSeg->getXLo();
			double x2 = pImgLineSeg->getXHi();
			double lenSq  = pImgLineSeg->getYAtX(x2) - pImgLineSeg->getYAtX(x1);
				   lenSq *= lenSq;  // lenSq = dy^2
				   lenSq += (x2-x1)*(x2-x1); // lenSq = dx^2 + dy^2
			if((int)(0.5+lenSq) < minLenSq)
				delete pImgLineSeg;
			else
				pLinesVector->push_back(pImgLineSeg);
		}

		delete (*p);
		p = regVector.erase(p);
	}
}


//////////////////
//  thresholdGY
//
void HLines::thresholdGY(MVImg<int> * pGY)
{
	int nPixels = pGY->getNPixels();
	int * pGyData = pGY->getData();

	for(int iPx=0; iPx<nPixels; iPx++)
		if( pGyData[iPx] < 0 )
			if(pGyData[iPx] > -gyThreshold)
				pGyData[iPx] = 0;
			else
				pGyData[iPx] = -1;
		else
			if(pGyData[iPx] < gyThreshold)
				pGyData[iPx] = 0;
			else
				pGyData[iPx] = 1;
}


//////////////////
//  filterByGX
//
void HLines::filterByGX(MVImg<int> * pGY, MVImg<int> * pGX)
{
	int nPixels = pGY->getNPixels();
	int * pGyData = pGY->getData();
	int * pGxData = pGX->getData();

	for(int iPx=0; iPx<nPixels; iPx++)
		if( abs(pGxData[iPx]) > abs(pGyData[iPx]) )
			pGyData[iPx] = 0;
}


//////////////////
//  clearWorldLines
//
void HLines::clearWorldLines()
{
	// delete all 3D lines
	vector<HLine_t *>::iterator p;
	p = worldLinesVector.begin();
	while( p != worldLinesVector.end() )
	{
		delete (*p);
		p = worldLinesVector.erase(p);
	}
}


//////////////////
//  clearVector(<YofXLineSeg *>)
//
void HLines::clearVector(vector<YofXLineSeg *> * pImgHLineVector)
{
	vector<YofXLineSeg *>::iterator p;
	p = pImgHLineVector->begin();
	while( p != pImgHLineVector->end() )
	{
		delete (*p);
		p = pImgHLineVector->erase(p);
	}
}


//////////////////
//  clearVector(<LineMatch_t *>)
//
void HLines::clearVector(vector<LineMatch_t *> *  pLinePairsVector)
{
	vector<LineMatch_t *>::iterator p;
	p = pLinePairsVector->begin();
	while( p != pLinePairsVector->end() )
	{
		delete (*p);
		p = pLinePairsVector->erase(p);
	}
}

///////////////////////////////////////////////////////////////////////////////////////
// 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.
///////////////////////////////////////////////////////////////////////////////////////