imageprocess.cpp

用于系统跟踪的程序

#include "stdafx.h"
#include "cv.h"
#include <string.h>

#include "ImageProcess.h"

#include "draw.h"
CConfigFile cfg("config.xml") ;//打开程序配置文件
 

#define  PI 3.1415926

//判断附近是否存在亮点
bool isNearBrightness(unsigned char xy1,unsigned char xy2,unsigned char xy3,unsigned char xy4,int ThresholdSmall)
{
	if(xy1>ThresholdSmall) return true;
	if(xy2>ThresholdSmall) return true;
	if(xy3>ThresholdSmall) return true;
	if(xy4>ThresholdSmall) return true;
	return false;
}
CImageProc::CImageProc()
{
	IsError=false;
	  
	pointForComputeCount=0;
	isFirst=true;
	line_type = 8; // change it to 8 to see non-antialiased graphic
	DrawLineCount=0;

	rng=cvRNG((unsigned)-1);
 
	ImageNumber=0;
}

//处理图像
IplImage* CImageProc::proc(IplImage* img,double& ScrX,double& ScrY,int ScreenWidth,int ScreenHeight)
{

	int LineCount=0;//线性特征的个数
	int PointCount=0;//点形特征的个数
	int ConnCount=0;
	int LineMarkerCount=0;
	float SumX=0;
	float SumY=0;
	double CountPixel=0;

	bool isFinded=false;

	IsError=false;
	char   ErrStr[100]="";
	//初始化
	if(isFirst)
	{
		pt1.x=20;
		pt1.y=20;
		cvInitFont(&font,10,0.4,0.4,0.2,0, line_type);
		isFirst=false;
		//将彩色图像转换成为8-bit灰度图象
		if(gray==0)gray = cvCreateImage( cvGetSize(img), 8, 1 ); 
		if(ImgTemp==0)ImgTemp = cvCreateImage(cvGetSize(img), 8, 3 ); 
		
	}
  
 
	


	////=====================================================================
	////标志点处理流程1
	////搜索4连同区域作为blob
	////在寻找标志点过程中完成合并距离相差不大的光斑
	////=====================================================================


	DrawLineCount=0;
 	for(int i=0;i<gray->width;i=i+1)
	{
		for(int j=0;j<gray->height-2;j=j+1)
		{
			if((unsigned char)(*(img->imageData+(img->width*j+i)*4+2))>cfg.ImageProcessPara_Threshold)
			{
				NewPoint.Clear();
				SumX=0;
				SumY=0;
				CountPixel=0;

				FindPoint1(img,i,j,SumX,SumY,CountPixel,cfg.ImageProcessPara_Threshold,NewPoint.ptT,NewPoint.ptB,NewPoint.ptL,NewPoint.ptR);
	
				if(CountPixel>cfg.ImageProcessPara_MinRefuse)
				{
					NewPoint.PointCount=(int)CountPixel;//为了计算CountPixel是double型，需要转换
					NewPoint.X=SumX/CountPixel;
					NewPoint.Y=SumY/CountPixel;
					NewPoint.Index=ConnCount;
					NewPoint.type=Point;
					NewPoint.groupID=ConnCount;
					NewPoint.FirstPoint.x=i;
					NewPoint.FirstPoint.y=j;

 

					if (NewPoint.PointCount>cfg.ImageProcessPara_MinRefuse && NewPoint.PointCount<cfg.ImageProcessPara_MaxRefuse )
					{
						points[ConnCount]=NewPoint;
						ConnCount++;
					}
					
				}
				//*(gray->imageData+(gray->width*j+i))=255;
			}else
			{
				//(*(img->imageData+(img->width*j+i)*4+0))=0;
				//(*(img->imageData+(img->width*j+i)*4+1))=0;
				//(*(img->imageData+(img->width*j+i)*4+2))=0;
				//(*(img->imageData+(img->width*j+i)*4+3))=0;
				//*(gray->imageData+(gray->width*j+i))=9;
			}
		}
	}



	
	//对线特征进行识别
	for(int i=0;i<ConnCount;i++)
	{
		points[i].isUsed=false;
		points[i].refreshLine(cfg.ImageAnalysePara_LineLength); 
	}

	int Limt=10;
	//设置安全区，过滤掉接近边界的点
	for(int i=0;i<ConnCount;i++)
	{
		if (points[i].ptL.x<Limt || points[i].ptT.y<Limt || points[i].ptB.y>img->height-Limt || points[i].ptR.x>img->width-Limt)
		{
			points[i].type=Err;
			points[i].isUsed=true;
		}
	}



	//根据找到的直线，构造线形标志点
	for(int i=0;i<ConnCount;i++)
	{
		if (points[i].type==Line)
		{
			linemarkers[LineMarkerCount].ptLineBegin=points[i].ptBegin;
			linemarkers[LineMarkerCount].ptLineEnd=points[i].ptEnd;
			getKB(points[i].wx,points[i].wy,points[i].x0,points[i].y0,linemarkers[LineMarkerCount].k,linemarkers[LineMarkerCount].b);
			points[i].isUsed=true;
			LineMarkerCount=LineMarkerCount+1;
		}
	}


	//寻找线形标志点，并计算交比
	for(int j=0;j<LineMarkerCount;j++)
	{
		DrawLine(img,linemarkers[j].ptLineBegin,linemarkers[j].ptLineEnd);
		
		float distanceToBegin=9999;
		float distanceToEnd=9999;
		int iBegin=-1;
		int iEnd=-1;

		for(int i=0;i<ConnCount;i++)
		{
			if (!points[i].isUsed)
			{
				float distanceTolinemarker=getDistanceToLine(points[i].X,points[i].Y,linemarkers[j].k,linemarkers[j].b);
				if (distanceTolinemarker<cfg.ImageAnalysePara_DistanceToLineLength)
				{
					float d1=getDistance(linemarkers[j].ptLineBegin.x,linemarkers[j].ptLineBegin.y, points[i].X,points[i].Y);
					float d2=getDistance(linemarkers[j].ptLineEnd.x,linemarkers[j].ptLineEnd.y, points[i].X,points[i].Y);

					if (d1<d2)
					{
						if(d1<distanceToBegin)
						{
							distanceToBegin=d1;
							iBegin=i;
						}
					}else
					{
						if(d2<distanceToEnd)
						{
							distanceToEnd=d2;
							iEnd=i;
						}
					}
				}
			}	
		}


		//获得了，线型标志点的两个端点，将数据填充到标志点的数据结构中，并清理原有点数组
		linemarkers[j].ptMarkerBegin.x	=-1;
		linemarkers[j].ptMarkerBegin.y	=-1;
		linemarkers[j].ptMarkerEnd.x	=-1;
		linemarkers[j].ptMarkerEnd.y	=-1;
		if(iBegin!=-1 && iEnd!=-1 && iBegin!=iEnd)
		{
			linemarkers[j].ptMarkerBegin.x=points[iBegin].X;
			linemarkers[j].ptMarkerBegin.y=points[iBegin].Y;
			 

			linemarkers[j].ptMarkerEnd.x=points[iEnd].X;
			linemarkers[j].ptMarkerEnd.y=points[iEnd].Y;

			linemarkers[j].MarkerBeginIndex=iBegin;
			linemarkers[j].MarkerEndIndex=iEnd;
			 
		}else
		{
			iBegin=-1;
			iEnd=-1;
		}
		linemarkers[j].iBegin=iBegin;
		linemarkers[j].iEnd=iEnd;


		linemarkers[j].ComputCrossRatio();
		//DrawCross(img,linemarkers[j].ptMarkerBegin ,2);	
		//DrawCross(img,linemarkers[j].ptMarkerEnd,1);
		DrawPoint(img,linemarkers[j].ptMarkerBegin,linemarkers[j].CrossRatio);

	}

	//清理原有点列中的数据，标注已使用的点
	//for(int i=0;i<LineMarkerCount;i++)
	//{
	//	points[linemarkers[i].iBegin].isUsed=true;
	//	points[linemarkers[i].iEnd].isUsed=true;
	//}


	//寻找到中心距离最近的线型标志点，记录为当前线型标志点
	float distanceToCenter=9999;
	CvPoint ptCenter;
	ptCenter.x=img->width/2;
	ptCenter.y=img->height/2;
	CLineMarker lmCurrent;

	for(int i=0;i<LineMarkerCount;i++)
	{	
		CvPoint pt;
		pt.x=(linemarkers[i].ptMarkerBegin.x+linemarkers[i].ptMarkerEnd.x)/2;
		pt.y=(linemarkers[i].ptMarkerBegin.y+linemarkers[i].ptMarkerEnd.y)/2;

		float d=getDistance(pt,ptCenter);
		if(d<distanceToCenter)
		{
			distanceToCenter=d;
			lmCurrent=linemarkers[i];
			isFinded=true;
		}
	}
	if (!isFinded)
	{
		return img;
	}

//	DrawCross(img,lmCurrent.ptMarkerBegin ,2);	
//	DrawCross(img,lmCurrent.ptMarkerEnd,1);
//	DrawPoint(img,lmCurrent.ptMarkerBegin,lmCurrent.CrossRatio);


	CvPoint Point1;
	CvPoint Point2;
	CvPoint Point3;
	CvPoint Point4;

	Point1.x=lmCurrent.ptMarkerBegin.x;
	Point1.y=lmCurrent.ptMarkerBegin.y;
	Point2.x=lmCurrent.ptMarkerEnd.x;
	Point2.y=lmCurrent.ptMarkerEnd.y;

	isFinded=false;

	float MinAng=9999;

	if(Point1.x==-1 || Point2.x==-1)
		return img;
	//在剩下的两个点中选择与当前线型标志点匹配【组成多边形最接近矩形的】点
	for(int i=0;i<ConnCount;i++)
	{
		if(!points[i].isUsed)
		{
			for(int j=i+1;j<ConnCount;j++)
			{
				if(!points[j].isUsed && !(i==lmCurrent.MarkerBeginIndex || i==lmCurrent.MarkerEndIndex || j==lmCurrent.MarkerBeginIndex ||j==lmCurrent.MarkerEndIndex))
				{
					//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
					//TODO：根据四个角度判断是否最接近矩形代码
					//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
					CvPoint Point3temp;
					CvPoint Point4temp;
					Point3temp.x=points[i].X;
					Point3temp.y=points[i].Y;
					Point4temp.x=points[j].X; 
					Point4temp.y=points[j].Y;

					//根据三角形面积的正负，确定1号和2号点的顺序
					double s1=getAreaOfTriangle(Point1,Point2,Point3temp);
					if (s1<0)
					{
						int tempX=Point1.x;
						int tempY=Point1.y;
						Point1.x=Point2.x;
						Point1.y=Point2.y;
						Point2.x=tempX;
						Point2.y=tempY;
					}

					double s2=getAreaOfTriangle(Point3temp,Point2,Point4temp);
					if (s2<0)
					{
						int tempX=Point3temp.x;
						int tempY=Point3temp.y;
						Point3temp.x=Point4temp.x;
						Point3temp.y=Point4temp.y;
						Point4temp.x=tempX;
						Point4temp.y=tempY;
					}

					 
					float ang=0;
					float ang1=abs(getAngle(Point3temp,Point1,Point2)-PI/2); 
					float ang2=abs(getAngle(Point1,Point2,Point4temp)-PI/2); 
					float ang3=abs(getAngle(Point2,Point4temp,Point3temp)-PI/2); 
					float ang4=abs(getAngle(Point4temp,Point3temp,Point1)-PI/2);

					if (ang1>ang)ang=ang1;
					if (ang2>ang)ang=ang2;
					if (ang3>ang)ang=ang3;
					if (ang4>ang)ang=ang4;

					if (ang<MinAng)
					{
						MinAng=ang;
						isFinded=true;
						Point3.x=Point3temp.x;
						Point3.y=Point3temp.y;
						Point4.x=Point4temp.x;
						Point4.y=Point4temp.y;
						 
					}
					
				}

			}
		}
		
	}

	if (!isFinded)
	{
		return img;
	}
	//DrawCross(img,Point3,3);	
	//DrawCross(img,Point4,1);	

	
	


	DrawPoint(img,Point1.x,Point1.y,1,1);
	DrawPoint(img,Point2.x,Point2.y,2,1);
	DrawPoint(img,Point3.x,Point3.y,3,1);
	DrawPoint(img,Point4.x,Point4.y,4,1);




	double Screen_x=-1;
	double Screen_y=-1;
	CaculateXY(Screen_x,Screen_y,
				ScreenWidth,ScreenHeight,
				img->width/2,img->height/2,
				Point1.x,Point1.y,
				Point2.x,Point2.y,
				Point4.x,Point4.y, 
				Point3.x,Point3.y);
 
	ScrX=Screen_x;
	ScrY=Screen_y;

	 //绘制========================================================================
	//for(int i=0;i<ConnCount;i++)
	//{
	//	if (!points[i].isUsed)
	//	{ 
	//		DrawCross(img,points[i].ptBegin,3);	
	//	}
	//}
	 //for(int i=0;i<ConnCount;i++)
	 //{
		//if (points[i].type==Line)
		//{
		//		DrawCross(img,points[i].ptBegin,1);	
		//		DrawCross(img,points[i].ptEnd,1);	
		//}

		//if (points[i].type==Point)
		//{
		//	DrawCross(img,points[i].X,points[i].Y,3);	
		//}

	 //}


	DrawPoint(img ,10,10,(float)ScrX);
	DrawPoint(img ,10,40,(float)ScrY);
	DrawCross(img,img->width/2,img->height/2,0);
	return img;
 


 
}