hlines.cpp

机器人程序

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

//////////////////////////////////////////////////////////////
// Implementation of the HLines class
//

#include "HLines.h"
#include "Mavis.h"
#include "../Logger.h"
#include "../Params.h"
#include "../mavistypes.h"
#include <math.h>


//////////////////
//  constructor
//
HLines::HLines(Mavis * pM)
{
	pMavis = pM;
	w = (double)pM->getImgWidth();
	h = (double)pM->getImgHeight();
	pLogger = pMavis->getLogger();

	pGVPosLinesVector1 = &GVPosLinesVectorA;
	pGVPosLinesVector2 = &GVPosLinesVectorB;
	pGVNegLinesVector1 = &GVNegLinesVectorA;
	pGVNegLinesVector2 = &GVNegLinesVectorB;

	bmpNum = 1;
	seriesStarted = false;


	// set parameters
	Params * pParams = pMavis->getParams();
	bool isValid;

	minLineLen = pParams->getIntValue("HLines", "minLineLen", &isValid);
	if( !isValid || minLineLen < 0 )
	{
		minLineLen = 60;
		pLogger->writelnToLog(
			"\tMissing or invalid value for minLineLen prameter. "
			"Using default."
		);
	}
	minLenSq  = minLineLen * minLineLen;      // comparisons use squared value
	minLineDX = (int)( (double)minLineLen / sqrt(2.0) ); // RegSearch filter


	maxLeastSquaresErr = pParams->getFloatPtValue("HLines", "maxLeastSquaresErr", &isValid);
	if( !isValid || maxLeastSquaresErr < 0 )
	{
		maxLeastSquaresErr = 2.25;
		pLogger->writelnToLog(
			"\tMissing or invalid value for maxLeastSquaresErr prameter. "
			"Using default."
		);
	}


	gyThreshold = pParams->getIntValue("HLines", "gyThreshold", &isValid);
	if( !isValid || gyThreshold < 0 )
	{
		gyThreshold = 22;
		pLogger->writelnToLog(
			"\tMissing or invalid value for gyThreshold prameter. "
			"Using default."
		);
	}


	maxEndPtShift = pParams->getIntValue("HLines", "maxEndPtShift", &isValid);
	if( !isValid || maxEndPtShift < 0 )
	{
		maxEndPtShift = 60;
		pLogger->writelnToLog(
			"\tMissing or invalid value for maxEndPtShift prameter. "
			"Using default."
		);
	}


	maxSlopeChange = pParams->getFloatPtValue("HLines", "maxSlopeChange", &isValid);
	if( !isValid || maxSlopeChange < 0 )
	{
		maxSlopeChange = 0.25;
		pLogger->writelnToLog(
			"\tMissing or invalid value for maxSlopeChange prameter. "
			"Using default."
		);
	}


	maxPctError = pParams->getFloatPtValue("HLines", "maxPctError", &isValid);
	if( !isValid || maxPctError < 0 )
	{
		maxPctError = 10.0;
		pLogger->writelnToLog(
			"\tMissing or invalid value for maxPctError prameter. "
			"Using default."
		);
	}


	gausHalfWidth = pParams->getIntValue("HLines", "gausHalfWidth", &isValid);
	if( !isValid || gausHalfWidth < 0 )
	{
		gausHalfWidth = 2;
		pLogger->writelnToLog(
			"\tMissing or invalid value for gausHalfWidth prameter. "
			"Using default."
		);
	}


	robotWidth = pParams->getFloatPtValue("robot", "width", &isValid);
	if( !isValid || robotWidth < 0 )
	{
		robotWidth = 0;
		pLogger->writelnToLog(
			"\tMissing or invalid value for robot#width prameter. "
			"Using default."
		);
	}


	// reset the index position
	iHLinePtr = 0;


	// get camera data and precompte things that don't change
	CameraData_t cameraData;
	pMavis->getCameraData(&cameraData);
	f = cameraData.f;
	hc = cameraData.cameraHt;
	double theta = PI*cameraData.tiltAngle / 180.0;
	cosTheta = cos(theta);
	sinTheta = sin(theta);
	t5 = f * cosTheta;
	t6 = f * sinTheta;
	halfHeight = h / 2.0;
	halfWidth = w / 2.0;

	pLogger->writelnToLog("\n\tHLines initialized:");
	pLogger->writelnToLog(
		"\t\tfocal length = %.2f pixels,"
		 " theta = %.2f radians",
		 f, theta
	);
	pLogger->writelnToLog("\t\tminLineLen = %d, maxLeastSquaresErr = %.2f", minLineLen, maxLeastSquaresErr);
	pLogger->writelnToLog("\t\tmaxPctError = %.2f", maxPctError);
	pLogger->writelnToLog("\t\tmaxEndPtShift = %d, maxSlopeChange = %.2f", maxEndPtShift, maxSlopeChange);
	pLogger->writelnToLog("\t\tgausHalfWidth = %d, gyThreshold = %d", gausHalfWidth, gyThreshold);
	pLogger->writelnToLog("\t\trobotWidth = %.0f", robotWidth);
}


//////////////////
//  destructor
//
HLines::~HLines()
{

	// free any HLine_t objects
	clearWorldLines();

	// free any ImgHLine_t objects
	clearVector(pGVPosLinesVector1);
	clearVector(pGVPosLinesVector2);
	clearVector(pGVNegLinesVector1);
	clearVector(pGVNegLinesVector2);


	// free any LineMatch_t objects
	clearVector(&GVPosPairsVector);
	clearVector(&GVNegPairsVector);
}


//////////////////
//  nextLine
//
int HLines::nextLine(HLine_t * pHLine)
{
	int nLines = worldLinesVector.size();

	if( !nLines )
	{
		memset( pHLine, 0, sizeof(HLine_t) );
		return -1;
	}
	else if( iHLinePtr < nLines )
	{
		memcpy( pHLine, worldLinesVector[iHLinePtr], sizeof(HLine_t) );
		iHLinePtr++;
		return nLines - iHLinePtr;
	}
	else
	{
		memcpy( pHLine, worldLinesVector[nLines-1], sizeof(HLine_t) );
		return -1;
	}
}


//////////////////
//  locateHLines
//
int HLines::locateHLines(int d, HLineOrder_t ordering, HLineMetadata_t *pHLineMetadata)
{
	// logging and visualization
	pLogger->writelnToLog("\n\tlocateHLines(), d = %d mm, ordering = %d", d, ordering);
	char filename[20];

	// capture one frame
	VideoFrame * pFrame = pMavis->getNextFrame();

	// save the original bitmap
	sprintf(filename, "rgb_%d", bmpNum);
	pLogger->writeFrame(pFrame->getData(), "hlines", filename);


	// metadata prep
	pMetaData = pHLineMetadata;
	memset( pMetaData, 0, sizeof(HLineMetadata_t) );
	iHLinePtr = 0;
	sortOrder = ordering;

	// convert to a grayscale image
	MVImg<int> * pImg = 0;
	MVImgUtils::frame2grayscaleImg(*pFrame, &pImg);

	// visualization: prepare to show the grayscale image
	MVImgUtils::Vis::grayscale(*pFrame);


	// add blur
	MVImgUtils::ImgProc::gaus(pImg, gausHalfWidth);

	if( seriesStarted && d )
	{
		distTraveled = d;

		// prepare to process frame 2
		clearVector(&GVPosPairsVector);
		clearVector(&GVNegPairsVector);
		clearWorldLines();


		// find prominent, non-vertical image lines in frame 2
		findProminentHLines(pImg, pGVPosLinesVector2, pGVNegLinesVector2);

		// visualization: draw lines on the image
		drawLinesOnImage (pFrame, pGVPosLinesVector2, 0x00ffff);
		drawLinesOnImage (pFrame, pGVNegLinesVector2, 0x00ff00);


		// do line matching
		matchLines(pGVPosLinesVector1, pGVPosLinesVector2, &GVPosPairsVector);
		matchLines(pGVNegLinesVector1, pGVNegLinesVector2, &GVNegPairsVector);


		// visualization: draw the matching lines in red and yellow
		int nLines = GVPosPairsVector.size();
		int i;
		for(i=0; i<nLines; i++)
		{
			LineMatch_t * pMatch = GVPosPairsVector[i];
			YofXLineSeg * pLine = (*pGVPosLinesVector2)[pMatch->j];
			int x1 = pLine->getXPixelLo();
			int x2 = pLine->getXPixelHi();
			MVImgUtils::Vis::line (
				*pFrame,
				x1, pLine->getYPixelAtX(x1), x2, pLine->getYPixelAtX(x2),
				0xff0000
			);
		}
		nLines = GVNegPairsVector.size();
		for(i=0; i<nLines; i++)
		{
			LineMatch_t * pMatch = GVNegPairsVector[i];
			YofXLineSeg * pLine = (*pGVNegLinesVector2)[pMatch->j];
			int x1 = pLine->getXPixelLo();
			int x2 = pLine->getXPixelHi();
			MVImgUtils::Vis::line (
				*pFrame,
				x1, pLine->getYPixelAtX(x1), x2, pLine->getYPixelAtX(x2),
				0xffff00
			);
		}


		// extract 3D data
		addWorldLines(pGVPosLinesVector1, pGVPosLinesVector2, &GVPosPairsVector, GVPOS);
		addWorldLines(pGVNegLinesVector1, pGVNegLinesVector2, &GVNegPairsVector, GVNEG);

		pHLineMetadata->nLines = worldLinesVector.size();


		// set up for next frame:
		//    swap pointers for pGVPosLinesVector1 & 2
		//    clear pGVPosLinesVector2
		vector<YofXLineSeg *> * pTemp;
		pTemp = pGVPosLinesVector1;
		pGVPosLinesVector1 = pGVPosLinesVector2;
		pGVPosLinesVector2 = pTemp;
		clearVector(pGVPosLinesVector2);

		//    swap pointers for pGVNegLinesVector1 & 2
		//    clear pGVNegLinesVector2
		pTemp = pGVNegLinesVector1;
		pGVNegLinesVector1 = pGVNegLinesVector2;
		pGVNegLinesVector2 = pTemp;
		clearVector(pGVNegLinesVector2);
	}
	else
	{
		// find prominent, non-vertical image lines in frame 1
		findProminentHLines(pImg, pGVPosLinesVector1, pGVNegLinesVector1);


		// visualization aid: draw lines on the image
		drawLinesOnImage (pFrame, pGVPosLinesVector1, 0x00ffff);
		drawLinesOnImage (pFrame, pGVNegLinesVector1, 0x00ff00);


		pHLineMetadata->nLines =
			pGVPosLinesVector1->size() + pGVNegLinesVector1->size();
		pHLineMetadata->pivotWasEstimated = false;


		seriesStarted = true;
	}

	// write the modified video frame to the hlines directory
	sprintf(filename, "frame_%d", bmpNum);
	pLogger->writeFrame(pFrame->getData(), "hlines", filename);
	bmpNum++;


	// free memory
	if(pImg) delete pImg;

	return 0;
}


//////////////////
//  addWorldLines
//
void HLines::addWorldLines(
		vector<YofXLineSeg *> * pLinesVector1,
		vector<YofXLineSeg *> * pLinesVector2,
		vector<LineMatch_t *> * pPairsVector,
		HLineType_t lineType
) {
	int  nPairs = pPairsVector->size();
	int sort  = sortOrder & 0x1;  // sort requested? (xxx1 -> yes)
	int field = sortOrder & 0x6;  // x01x -> height, x00x -> distance
	int hi2lo = sortOrder & 0x8;  // 1xxx -> descending order, 0xxx, ascending

	for(int iPair=0; iPair<nPairs; iPair++)
	{
		int i = (*pPairsVector)[iPair]->i;
		int j = (*pPairsVector)[iPair]->j;
		YofXLineSeg * pLine1 = (*pLinesVector1)[i];
		YofXLineSeg * pLine2 = (*pLinesVector2)[j];
		HLine_t * pHLine = compute3DHLine(pLine1, pLine2);
		if(pHLine)
		{
			pHLine->lineType = lineType;
			char type[50];
			if(GVPOS == lineType) sprintf(type, "GVPOS");
			else sprintf(type, "GVNEG");

			// add new HLine to worldLinesVector
			if( sort && !worldLinesVector.empty() )
			{
				// find where to insert it
				DataRange_t range2 = (field)? pHLine->height : pHLine->distance;
				double valAdd = (range2.lo + range2.hi)/2.0;
				if(hi2lo)  // descending order
					valAdd  = -valAdd;
				vector<HLine_t *>::iterator p;
				p = worldLinesVector.begin();
				while( p != worldLinesVector.end() )
				{
					DataRange_t range1 = (field)? (*p)->height : (*p)->distance;
					double valHere = (range1.lo + range1.hi)/2.0;
					if(hi2lo)  // descending order
						valHere = -valHere;

					// insert point is based on ascending order
					if(valAdd < valHere)
					{
						worldLinesVector.insert(p, pHLine);
						goto addedHLine;
					}

					p++;
				}

			}
			// This line is reached if the if-stmt is skipped (no sorting) or
			// the while loop inside it falls through (item belongs at the end).
			worldLinesVector.push_back(pHLine);

			// If the HLine was added in the while loop above, control jumps
			// to here, skipping the push_back() above.
			addedHLine:


			pLogger->writelnToLog(
				"\n\tHLine:\n"
				"\t\tpctError = %.2f,\n"
				"\t\tdistance.lo = %.2f mm,\n"
				"\t\tdistance.hi = %.2f mm,\n"
				"\t\theight.lo = %.2f mm,\n"
				"\t\theight.hi = %.2f mm,\n"
				"\t\tangle.lo = %.2f degrees,\n"
				"\t\tangle.hi = %.2f degrees,",
				pHLine->pctError,
				pHLine->distance.lo,
				pHLine->distance.hi,
				pHLine->height.lo,
				pHLine->height.hi,
				pHLine->angle.lo,
				pHLine->angle.hi
			);
			pLogger->writelnToLog(
				"\t\tline type = %s,\n"
				"\t\tinPathVis = %d,\n"
				"\t\tinPathProb = %d.",
				type,
				pHLine->inPathVis,
				pHLine->inPathProb
			);
		}
	}
}


HLine_t * HLines::compute3DHLine(YofXLineSeg * pLine1, YofXLineSeg * pLine2)
{
	HLine_t * pHLine = NULL;

	pLogger->writelnToLog("\n\n\tNext pair:");

	// log details of each line
	pLogger->writelnToLog("\t\tLine1:");
	pLogger->writelnToLog("\t\tx2Lo = %d, x2Hi = %d", pLine1->getXPixelLo(), pLine1->getXPixelHi());
	pLogger->writelnToLog("\t\tm = %.10f, b = %.4f", pLine1->getSlope(), pLine1->getIntercept());

	pLogger->writelnToLog("\n\t\tLine2:");
	pLogger->writelnToLog("\t\tx2Lo = %d, x2Hi = %d", pLine2->getXPixelLo(), pLine2->getXPixelHi());
	pLogger->writelnToLog("\t\tm = %.10f, b = %.4f", pLine2->getSlope(), pLine2->getIntercept());

	pLogger->writelnToLog("");


	// Compute y, z2, and z1
	double v1 = pLine1->getYAtX(halfWidth) - halfHeight;
	double v2 = pLine2->getYAtX(halfWidth) - halfHeight;
	//pLogger->writelnToLog("\t\tv1 = %.2f, v2 = %.2f", v1, v2);

	double t1 = v1 * cosTheta;
	double t2 = v1 * sinTheta;
	double t3 = v2 * cosTheta;
	double t4 = v2 * sinTheta;
	double k = (t5-t2) + ( (t1+t6)*(t4-t5) / (t3+t6) );
	//pLogger->writelnToLog("\t\tt1 = %.2f, t2 = %.2f", t1, t2);
	//pLogger->writelnToLog("\t\tt3 = %.2f, t4 = %.2f, k = %.2f", t3, t4, k);

	// validate that |k| > 0
	if( fabs(k) < 1e-6)
	{
		pLogger->writelnToLog("\t\tSmall denominator. Can\'t compute 3D line.");
		return (HLine_t *)NULL;
	}

	double y = distTraveled * (t1+t6) / k;
	double z2 = y * (t5-t4)/(t3+t6);