queuearea.c

jiance jichang paidui de moxing.

/**
* @file queuearea.c
* @brief
* Copyright (c) 2007,SICE
* All rights reserved.
*
* 揈梫丗暲傃僄儕傾柾宆傪峔憿偡傞丅
*
* Ver : 0.6
* 嶌幰 : 錖愯摀
* 峏怴帪娫 : 2007/11/23
*/
#include "common.h"
#include "queuearea.h"

#define DEBUG_ON 0
#define DEL_LIN_GRAY_ON 0

static char buff[100];
static int combuf[100][100];
static int combCnt = 0;
static CmQueueAreaModel * m_qAreaModel = NULL;


/**
 * @fn cmInitQueueAreaModule
 * 
 * @brief
 * 揈梫丗create a memory type of CmQueueAreaModule, and set several 
 *       varaibal value with zero.
 * 
 * @return void
 */
CmQueueAreaModel* cmInitQueueAreaModel()
{
	int i, j;
	CmQueueAreaModel * queueAreaMdl = 
		(CmQueueAreaModel*)malloc(sizeof(CmQueueAreaModel));

	queueAreaMdl->imgOrigin = 0;
	queueAreaMdl->lineCount = 0;
	queueAreaMdl->ratioOfSpaceBtwn = 0.0;
	queueAreaMdl->slope = 0.0;

	for(i=0; i<100; i++)
	{
		queueAreaMdl->iArrLineCntPerRow[i] = 0;
		queueAreaMdl->fArrDistOfRowiToRow0[i] = 0.0;

		for(j=0; j<100; j++)
		{
			queueAreaMdl->iArrLineDataPerRow[i][j] = 0;
		}
	}

	return queueAreaMdl;
}

/**
 * @fn cmInitQueueAreaModule2
 * 
 * @brief
 * 揈梫丗create a memory type of CmQueueAreaModule, and set several 
 *       varaibal value with provided by user.
 * @param[in] fLeft
 * @param[in] fRight
 * @param[in] lLeft
 * @param[in] lRight
 * @param[in] lineCnt
 * @return void
 */
CmQueueAreaModel* cmInitQueueAreaModule2(int origin, CvPoint fLeft, CvPoint fRight, 
										  CvPoint lLeft, CvPoint lRight,
										  int lineCnt)
{
	float tmp1, tmp2;
	int i, j;

	CmQueueAreaModel * queueAreaMdl = cmInitQueueAreaModel();

	queueAreaMdl->imgOrigin = origin;
	//暲傃懸偪僄儕傾偺巐偮僐僫乕億僀儞僩傪愝掕偡傞丅
	queueAreaMdl->firstLineLeft = fLeft;
	queueAreaMdl->firstLineRight = fRight;
	queueAreaMdl->lastLineLeft = lLeft;
	queueAreaMdl->lastLineRight = lRight;

	queueAreaMdl->lineCount = lineCnt;
	
	queueAreaMdl->ratioOfSpaceBtwn = 0.0;

	tmp1 = (float)(fRight.y-fLeft.y) / (fRight.x-fLeft.x);
	tmp2 = (float)(lRight.y-lLeft.y) / (lRight.x-lLeft.x);

	//戞堦偺楍慄偲嵟屻偺楍慄偼暯峴偟側偄応崌丄僄儔乕偲偟偰廔傢傞丅
	//if (fabs(tmp1-tmp2) >= FLOAT_ZERO_DIFF)
	//{
	//	printf("CmQueueAreaModule format error.\n");
	//	return NULL;
	//}

	queueAreaMdl->slope = (tmp1+tmp2)/2;
	
	return queueAreaMdl;
}

/**
 * @fn cmReleaseQueueAreaModule
 * 
 * @brief
 * 揈梫丗Release the space of queueAreaMdl.
 * 
 * @param[in] queueAreaMdl
 *
 * @return void
 */
void cmReleaseQueueAreaModule(CmQueueAreaModel **queueAreaMdl)
{
	free(*queueAreaMdl);
	//...... adding
	*queueAreaMdl = NULL;
}


/**
 * @fn cmHoughLine
 * 
 * @brief
 * 揈梫丗HoughLine傪峴偆丅
 * 
 * @param[in] pImgSrc
 * @param[in] pStorage
 *
 * @return void
 */
CvSeq * cmHoughLine(IplImage *pImgSrc, CvMemStorage **pStorage)
{
	IplImage *pImgDst = NULL;

	CvSeq *pLines = NULL;

	int i;

	pImgDst = cvCreateImage(cvGetSize(pImgSrc), IPL_DEPTH_8U, 1 );
	pImgDst->origin = pImgSrc->origin;
	cvSmooth(pImgSrc, pImgSrc, CV_GAUSSIAN, 3, 0, 0, 0);
	cvDilate(pImgSrc, pImgSrc, 0, 1);
	cvErode(pImgSrc, pImgSrc, 0, 1);

#if 1
	cvCanny(pImgSrc, pImgDst, 50, 200, 3);
#else
	cvLaplace(pImgSrc1, pImgDst, 3);
#endif

    pLines = cvHoughLines2(pImgDst, *pStorage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 60, 30, 10);
	//@param3 method: CV_HOUGH_STANDARD,CV_HOUGH_PROBABILISTIC,CV_HOUGH_MULTI_SCALE 

	cvReleaseImage(&pImgDst);

	return pLines;
}

int cmIsTheLineRealObject(IplImage *pImgGray, CmLine line, int iCriticalGray)
{
	int x, y, validPixelCnt = 0, gray;
	float validPercent;

	for(x=line.pt1.x; x<=line.pt2.x; x++)
	{
		y = cmCalCoordinateYWithX(x, line);
		gray = (unsigned char)pImgGray->imageData[y*pImgGray->widthStep+x];

		if(gray < iCriticalGray)
		{
			validPixelCnt++;
		}
	}

	validPercent = validPixelCnt / (line.pt2.x-line.pt1.x);

	if(validPercent<0.6)
	{
		return 0;
	}

	return 1;
}

/**
 * @fn cmFilterLines
 * 
 * @brief
 * 揈梫丗儘僕僢僋曽朄偵傛偭偰丄晄昁梫側慄傪饪偄偵偐偗傞丅
 * 
 * @param[in] pLines
 * @param[in] fMinSlope
 * @param[in] fMaxSlope
 *
 * @return void
 */
CmLine* cmFilterLines(CmQueueAreaModel *pModel, CvSeq *pLines, float fMinSlope, float fMaxSlope, IplImage *pImgGray, int iCriticalGray)
{
	int i;
	CmLine *firstLine = NULL;
	CmLine *pCmLine = NULL;
	
	float fMaxDis,fDist1,fDist2;

	CmLine frmLine12, frmLine13, frmLine24, frmLine34;
	CvPoint ptInsec12, ptInsec13, ptInsec24, ptInsec34;

	if(pModel)
	{
		frmLine12.pt1 = pModel->firstLineLeft;
		frmLine12.pt2 = pModel->firstLineRight;
		frmLine13.pt1 = pModel->firstLineLeft;
		frmLine13.pt2 = pModel->lastLineLeft;
		frmLine24.pt1 = pModel->firstLineRight;
		frmLine24.pt2 = pModel->lastLineRight;
		frmLine34.pt1 = pModel->lastLineLeft;
		frmLine34.pt2 = pModel->lastLineRight;
	}

	fMaxDis = cmCalDistanceOfPointToLine(pModel->firstLineRight, 
				cmLine(pModel->lastLineLeft,pModel->lastLineRight));
	for(i = 0; i < pLines->total; i++)
    {
        CvPoint* line = (CvPoint*)cvGetSeqElem(pLines,i);
		
		float dirtX = line->x > line[1].x ? line->x - line[1].x : line[1].x - line->x; 
		float dirtY = line->x > line[1].x ? line->y - line[1].y : line[1].y - line[0].y; 
		float slope;
		CmLine cmtpLine = cmLine(line[0], line[1]);

		if(dirtX < 0.00001)
		{
			continue;
		}
		
		if(pImgGray->origin == 0)
		{
			dirtY = -dirtY;
		}

		slope = dirtY / dirtX;

		if((fMinSlope+FLOAT_ZERO_DIFF) >= slope ||  slope >= fMaxSlope)
		{
			continue;
		}
		fDist1= cmCalDistanceOfPointToLine(line[0], 
				cmLine(pModel->lastLineLeft,pModel->lastLineRight));
		fDist2= cmCalDistanceOfPointToLine(line[1], 
				cmLine(pModel->lastLineLeft,pModel->lastLineRight));
		if(fDist1 > fMaxDis && fDist2 > fMaxDis)
		{
			continue;
		}

		if(pModel)//暲傫偱椞堟埲奜偺捈慄傪庢傝彍偔丅
		{
			int isInsideOfSphere0 = cmIsPointInsideOfSphere(line[0], pModel->firstLineLeft, 
				pModel->firstLineRight, pModel->lastLineLeft, pModel->lastLineRight);
			int isInsideOfSphere1 = cmIsPointInsideOfSphere(line[1], pModel->firstLineLeft, 
				pModel->firstLineRight, pModel->lastLineLeft, pModel->lastLineRight);

			int iInsecNum = 0;
			CvPoint ptTmp1, ptTmp2;
			ptInsec12 = cmCalIntersectionOfTwoLines(&frmLine12, &cmtpLine, 1);
			ptInsec13 = cmCalIntersectionOfTwoLines(&frmLine13, &cmtpLine, 1);
			ptInsec24 = cmCalIntersectionOfTwoLines(&frmLine24, &cmtpLine, 1);
			ptInsec34 = cmCalIntersectionOfTwoLines(&frmLine34, &cmtpLine, 1);

			if (ptInsec12.x > 0)
			{
				iInsecNum++;
				ptTmp1 = ptInsec12;
			}
			if (ptInsec13.x > 0)
			{
				iInsecNum++;
				if (iInsecNum == 1)
				{
					ptTmp1 = ptInsec13;
				}else{
					ptTmp2 = ptInsec13;
				}
			}
			if (ptInsec24.x > 0)
			{
				iInsecNum++;
				if (iInsecNum == 1)
				{
					ptTmp1 = ptInsec24;
				}else{
					ptTmp2 = ptInsec24;
				}
			}
			if (ptInsec34.x > 0)
			{
				iInsecNum++;
				if (iInsecNum == 1)
				{
					ptTmp1 = ptInsec34;
				}else{
					ptTmp2 = ptInsec34;
				}
			}
			
			if (iInsecNum == 2)
			{
				if(ptTmp1.x==ptTmp2.x && ptTmp1.y==ptTmp2.y)
				{
					continue;
				}
				line[0].x = ptTmp1.x;
				line[0].y = ptTmp1.y;
				line[1].x = ptTmp2.x;
				line[1].y = ptTmp2.y;
			}
			else
			{
				if (isInsideOfSphere0 == 0 && isInsideOfSphere1 == 0)
				{
					continue;
				}

				if(isInsideOfSphere0 == 0 && isInsideOfSphere1 == 1 || 
					isInsideOfSphere0 == 1 && isInsideOfSphere1 == 0)
				{
					CvPoint ptInsec = cvPoint(-1,-1);

					if(ptInsec12.x > 0)
					{
						ptInsec = ptInsec12;
					}
					else if(ptInsec13.x > 0)
					{
						ptInsec = ptInsec13;
					}
					else if(ptInsec24.x > 0)
					{
						ptInsec = ptInsec24;
					}
					else if(ptInsec34.x > 0)
					{
						ptInsec = ptInsec34;
					}
					
					if (ptInsec.x > 0 && ptInsec.y > 0)
					{
						if (isInsideOfSphere0 == 0)
						{
							line[0].x = ptInsec.x;
							line[0].y = ptInsec.y;
						}
						else if (isInsideOfSphere1 == 0)
						{
							line[1].x = ptInsec.x;
							line[1].y = ptInsec.y;
						}
					}
				}		
			}
		}

		if(line[0].x == line[1].x && line[0].y == line[1].y)
		{
			continue;
		}

		//According its graycolor is black, filter line
#if DEL_LIN_GRAY_ON
		if(cmIsTheLineRealObject(pImgGray, cmLine(line[0],line[1]), iCriticalGray) == 0)
		{
			continue;
		}
#endif

		pCmLine = (CmLine *)malloc(sizeof(CmLine));
		pCmLine->next=pCmLine->prev=NULL;
		pCmLine->slope = slope;

		if(line[0].x < line[1].x)
		{
			pCmLine->pt1 = line[0];
			pCmLine->pt2 = line[1];
		}
		else
		{
			pCmLine->pt1 = line[1];
			pCmLine->pt2 = line[0];
		}

		if(firstLine)
		{
			pCmLine->next = firstLine;
			firstLine->prev = pCmLine;
		}

		firstLine = pCmLine;
		pCmLine = NULL;
    }

	return firstLine;
}

/**
 * @fn cmUniteSameQueueLines
 * 
 * @brief
 * 揈梫丗摨偠楍偺椬偵専弌偝傟偨慄傪堦偮慄偵摑崌偡傞丅
 * 
 * @param[in] pCmLines
 * @param[in] fSlopeErr
 * @param[in] fDistErr
 *
 * @return CmLine*
 */
CmLine * cmUniteSameQueueLines(CmLine *pCmLines, float fSlopeErr, float fDistErr)
{
	CmLine *pLine = NULL;
	CmLine *pTmpLine = NULL, *pNext = NULL;

	CmLine *pHead = (CmLine*)malloc(sizeof(CmLine));

	float d1,d2, dirtH;
	float tmpY;
	int iIsUp, iDirtH, iRepTimes = 0;
	float dirtX ;
	float dirtY ;
	float slope;

	pHead->next = pCmLines;
	if (pCmLines)
	{
		pCmLines->prev = pHead;
	}

	while(iRepTimes < 3)
	{
		pLine = pHead->next;

		while(pLine)
		{
			pTmpLine = pLine->next;
			while(pTmpLine)
			{
				if(fabs(pLine->slope - pTmpLine->slope) <= fSlopeErr && 
					((pLine->pt1.x >= pTmpLine->pt1.x && pLine->pt1.x <= pTmpLine->pt2.x) || 
					 (pTmpLine->pt1.x >= pLine->pt1.x && pTmpLine->pt1.x <= pLine->pt2.x)))
				{
					d1 = cmCalDistanceOfPointToLine(pLine->pt1, *pTmpLine);
					d2 = cmCalDistanceOfPointToLine(pLine->pt2, *pTmpLine);

					if(d1 <= fDistErr && d2 <= fDistErr)
					{
						if((pLine->pt1.x >= pTmpLine->pt1.x && pLine->pt1.x <= pTmpLine->pt2.x) ||
							(pTmpLine->pt1.x >= pLine->pt1.x && pTmpLine->pt1.x <= pLine->pt2.x))
						{//廳側傝偁偆丅
							tmpY = (pTmpLine->pt2.x*pTmpLine->pt1.y - pTmpLine->pt1.x*pTmpLine->pt2.y -
								(pTmpLine->pt1.y-pTmpLine->pt2.y)*pLine->pt1.x) / (pTmpLine->pt2.x-pTmpLine->pt1.x);
							iIsUp = pLine->pt1.y <= tmpY ? 1 : 0; 
							dirtH = d1  / 2;
							iDirtH = (int)(dirtH + 0.5);

							if (pTmpLine->pt1.x < pLine->pt1.x)
							{
								pLine->pt1.x = pTmpLine->pt1.x;
								pLine->pt1.y = iIsUp == 1 ? pTmpLine->pt1.y - iDirtH : pTmpLine->pt1.y + iDirtH ;
							}
							else
							{
								pLine->pt1.y = iIsUp == 1 ? pLine->pt1.y + iDirtH : pLine->pt1.y - iDirtH ;
							}

							//reset pLine.pt2
							tmpY = (pTmpLine->pt2.x*pTmpLine->pt1.y - pTmpLine->pt1.x*pTmpLine->pt2.y -
								(pTmpLine->pt1.y-pTmpLine->pt2.y)*pLine->pt2.x) / (pTmpLine->pt2.x-pTmpLine->pt1.x);
							iIsUp = pLine->pt2.y <= tmpY ? 1 : 0; 
							dirtH = d2  / 2;
							iDirtH = (int)(dirtH + 0.5);

							if (pTmpLine->pt2.x > pLine->pt2.x)
							{
								pLine->pt2.x = pTmpLine->pt2.x;
								pLine->pt2.y = iIsUp == 1 ? pTmpLine->pt2.y - iDirtH : pTmpLine->pt2.y + iDirtH ;