예제 #1
0
///////////// Vision Class Implementation //////////////
CDib* CVisionTab::ColorBoxRecog(CDib* pImg)
{
	
	// ------------------------------------------------------
	// ------------------- Color Slicing &-------------------
	// ------------- Kernel density estimation --------------
	// ------------------------------------------------------

	//CalcTime();
	
	// input image 영상 처리 및 복사
	CDib* src = pImg->GetSingleScaleRetinexImage(1.5);
	//CDib* src = pImg->CopyCDib();

	int width = src->Width();
	int height = src->Height();
	//int bitcount = src->BitCount();
	int bitcount = 8;
	
	
	
	CDib* pRed = new CDib;
	pRed->Allocate(width, height, bitcount);
	pRed->SetGrayPalette();

	CDib* pGreen = new CDib;
	pGreen->Allocate(width, height, bitcount);
	pGreen->SetGrayPalette();

	CDib* pBlue = new CDib;
	pBlue->Allocate(width, height, bitcount);
	pBlue->SetGrayPalette();

	CDib* pYellow = new CDib;
	pYellow->Allocate(width, height, bitcount);
	pYellow->SetGrayPalette();

	
	register int i, j;
	double h, s, v;
	int step = (int)src->BitCount()/8;
	RGBQUAD quad;


	CKde* kdemat[NUMBOX];
	for(i=0; i<NUMBOX; i++) {
		kdemat[i] = new CKde;
		kdemat[i]->initKDEmask(width, height, 4);	// initialize KDE function
	}

	

	for(j=0; j<src->Height(); j++)
	{
		unsigned char *ptr = src->GetPointer(0,j);
		for(i=0; i<src->Width(); i++, ptr+=step)
		{
			quad.rgbBlue = *(ptr+0);	quad.rgbGreen = *(ptr+1);	quad.rgbRed = *(ptr+2);
			src->RGBtoHSV(quad, &h, &s, &v);

			// normalize Hue
			h /= 360;
			
			
			// Red Box: 0.93, 0.97
			if(h > 0.91 && h < 0.99 && s > TS && v > Ti) {
				unsigned char *pr = pRed->GetPointer(i,j);
				*(pr+0) = 255;
				kdemat[RED]->setSumKDEmask(i,j);
			}

			// Green Box: 0.25, 0.37
			if(h > 0.24 && h < 0.38 && s > TS && v > Ti) {	// default: 0.27, 0.36
				unsigned char *pg = pGreen->GetPointer(i,j);
				*(pg+0) = 255;
				kdemat[GREEN]->setSumKDEmask(i,j);
			}

			// Blue: 0.59, 0.65
			if(h > 0.59 && h < 0.65 && s > TS  && v > Ti) {
				unsigned char *pb = pBlue->GetPointer(i,j);
				*(pb+0) = 255;
				kdemat[BLUE]->setSumKDEmask(i,j);
			}

			// Yellow: 0.12, 0.14
			if(h > 0.11 && h < 0.15 && s > TS  && v > Ti) {
				unsigned char *py = pYellow->GetPointer(i,j);
				*(py+0) = 255;
				kdemat[YELLOW]->setSumKDEmask(i,j);
			}
			
		}
	}
	

	// *****************************************************************
	//
	// 본 프로그램의 좌표는 크게 아래 3가지로 나뉜다. 
	// Clip 좌표(ci_pt):			clip image를 기준으로 한 좌표
	// Global image 좌표(mpt):	640x480의 전체 이미지를 기준으로 한 좌표
	// World 좌표:			Camera calibration을 통한 월드 좌표계
	//
	// *****************************************************************
	
	static Coordi prePoint[NUMBOX];	// Low pass filter구현을 위한 이전 좌표 저장 변수
	static Coordi preGiPt[NUMBOX];	
	double alpha = 0.3;

	Coordi gi_pt[NUMBOX];		// global image point
	unsigned char val = 255;

	CDib* bDib[NUMBOX];

	// RGBY in order
	for(i=0; i<NUMBOX; i++) {
		// Color Box Position ------------------
		CPoint cp = kdemat[i]->GetMaxCoordi();	// Clip image에서의 KDE max 좌표
		CPoint ci_pt = kdemat[i]->GetThresMeanCoordiClip(0.2, cp, 60);				// depth 정보에 따른 Area 계산식 구현할 것!
		//bDib[i] = kdemat[i]->GetThresBinaryClip(0.1, cp, 60);						// Binary image 사용


		// Color Box Orientation ----------------
		CDib* pTmp[10] = {NULL,};	// Image processing 후 지우기 위한 배열
		int sub=20;
		pTmp[0] = src->ClipCDib(ci_pt.x-sub, ci_pt.y-sub, ci_pt.x+sub, ci_pt.y+sub);		// Clipped Grayscale 이미지 사용
		pTmp[1] = pTmp[0]->GetGrayCDib();
		//pTmp[2] = pTmp[1]->BrightnessG(25);
		pTmp[3] = pTmp[1]->ContrastG(3);
		pTmp[10] = pTmp[3]->GaussianSmoothingG(2);

		bDib[i] = pTmp[10];
		
		for(int k=0; k<10; k++){
			if(pTmp[k]) { delete pTmp[k]; pTmp[k]=NULL;}
		}
		
		double maxkde = kdemat[i]->GetMaxValue();
		
		double currx = (ci_pt.x + m_ClipRect.left);
		double curry = (ci_pt.y + m_ClipRect.top);
		double currz = (double)kinect.getDepthValue(currx, curry);
		double currRz = bDib[i]->GetSobelOrientation();	// 여기서 실제 orientation 계산
		

		// Get global x, y, z-axis coordinate
		// Global image 좌표, Low pass filter
		gi_pt[i].x = currx*alpha + (1-alpha)*preGiPt[i].x;
		gi_pt[i].y = curry*alpha + (1-alpha)*preGiPt[i].y;
		gi_pt[i].z = currz*alpha + (1-alpha)*preGiPt[i].z;
		gi_pt[i].rz = currRz*alpha + (1-alpha)*preGiPt[i].rz;

		// depth noise 처리
		if(currz == 0.0) {
			gi_pt[i].z = preGiPt[i].z;	// 현재 depth 값이 0일 경우 이전 값을 사용
		}
		
		
		// 각 Color Box의 pixel 개수가 일정 이상이어야 World 좌표 업데이트
		// Insert Kalman or any other filter code
		if(maxkde > 0.6) {

			
			MatrixXd StaubliPoint = MatrixXd::Zero(4,1);
			MatrixXd CamPoint = MatrixXd::Zero(4,1);

			// Camera World좌표계로 변환
			CamPoint(0,0) = ((gi_pt[i].x - (double)m_GlobalCenter.x)*gi_pt[i].z)/K(0,0) + OFFSETX;
			CamPoint(1,0) = -((gi_pt[i].y - (double)m_GlobalCenter.y)*gi_pt[i].z)/K(1,1) + OFFSETY;
			CamPoint(2,0) = 940 - gi_pt[i].z;	// 940 mm: Kinect와 World Frame 사이의 거리
			CamPoint(3,0) = 1.0;

			//Staubli 기준 좌표로 변환
			StaubliPoint = M_st * CamPoint;	// Transformation Matrix 곱해줌


			//m_cbWorldPoint[i].x = CamPoint(0,0);
			//m_cbWorldPoint[i].y = CamPoint(1,0);
			//m_cbWorldPoint[i].z = (CamPoint(2,0) < -370 ? -370 : CamPoint(2,0));	// 땅에 부딪히지 않도록 limit 걸어줌
			//m_cbWorldPoint[i].rx = 0.0;
			//m_cbWorldPoint[i].ry = 0.0;
			//m_cbWorldPoint[i].rz = gi_pt[i].rz;

			
			m_cbWorldPoint[i].x = StaubliPoint(0,0);
			m_cbWorldPoint[i].y = StaubliPoint(1,0);
			//m_cbWorldPoint[i].z = (StaubliPoint(2,0) < -305 ? -305 : StaubliPoint(2,0));	// 땅에 부딪히지 않도록 limit 걸어줌
			m_cbWorldPoint[i].z = -305;	// 땅에 부딪히지 않도록 limit 걸어줌
			m_cbWorldPoint[i].rx = 0.0;
			m_cbWorldPoint[i].ry = 0.0;
			m_cbWorldPoint[i].rz = gi_pt[i].rz;
			
			
			// 현재 데이터를 이전 데이터로 저장
			preGiPt[i].x = gi_pt[i].x;
			preGiPt[i].y = gi_pt[i].y;
			preGiPt[i].z = gi_pt[i].z;
			preGiPt[i].rx = gi_pt[i].rx;
			preGiPt[i].ry = gi_pt[i].ry;
			preGiPt[i].rz = gi_pt[i].rz;

			double px = gi_pt[i].x-m_ClipRect.left;
			double py = gi_pt[i].y-m_ClipRect.top;

			//pRed->DrawCross(mpt[i].x, mpt[i].y, val);
			src->DrawCross(px, py, val);	// using local(clip) coordinate


			double rad = gi_pt[i].rz;
			double len = 15;
			src->DrawLine(px-len*cos(rad), py-len*sin(rad), px+len*cos(rad), py+len*sin(rad), RGB(255,100,128));
		}
	}

	


	// Region growing은 버리자...
	//CDib* pDib = pRed->RegionGrowing(mpt[BLUE].x, mpt[BLUE].y);
	//CDib* pDib = src->RegionGrowing(mpt[RED].x, mpt[RED].y, 80);
	
	if(m_flag) {
		int width = kdemat[RED]->Width();
		int height = kdemat[RED]->Height();

		// 파일로 저장
		FILE *f;
		f = fopen("c:\\kde.dat","wb");
		if(f!=NULL) {
					
			for(int j=0; j<height; j++)
			{
				for(int i=0; i<width; i++)
				{
					fprintf(f, "%lf ", kdemat[RED]->GetValue(i,j));
				}
				fprintf(f, "\n");
			}
	
			fclose(f);
		}
	
		m_flag=0;
		AfxMessageBox("kde map is saved");
	}



	////////////////////////////////////////////
	// Copy an image for monitoring
	pTestImage = bDib[GREEN]->CopyCDib();
	//pTestImage = src->GetGrayCDib();



	////////////////////////////////////////////
	//// delete images.... 
	delete pRed;
	delete pGreen;
	delete pBlue;
	delete pYellow;

	for(i=0; i<NUMBOX; i++) {
		delete bDib[i];
		kdemat[i]->releaseKDEMask();
	}

	return src;
}
예제 #2
0
// ------------------------------------------------------------------
// ------------------ Image processing ------------------------------
// ------------------------------------------------------------------
void CVisionTab::doingImageProcess()
{
	//cout << "image processing..." << endl;
	// Calculate the Frame Rate

	CalcTimeIP();

	if(clip) {
		//if(pGColorClip && pGDepthClip)	// Clipped image에 대해서만 영상 처리 적용
		{
			
			switch(m_visionMode) {
				case None:
					{
					CDib* pDib = GetColorClipImage(m_ClipRect);
					if(pDib) m_imgProcess.SetDib(pDib);
					break;
					}
				case Color_box:
					{
					CDib* pRes = NULL;
					CDib* pDib = NULL;

					pDib = GetColorClipImage(m_ClipRect);
					if(pDib) {
						pRes = ColorBoxRecog(pDib);	// DLL로 만들 함수..
						m_imgProcess.SetDib(pRes);
					}
					delete pDib;
					

					// Display the coordinate of each color box
					CString str;	// RGBY
					str.Format("Red      x: %.1lf y: %.1lf z: %.1lf rz: %.1lf \nGreen   x: %.1lf y: %.1lf z: %.1lf rz: %.1lf \nBlue     x: %.1lf y: %.1lf z: %.1lf rz: %.1lf \nYellow  x: %.1lf y: %.1lf z: %.1lf rz: %.1lf", 
					m_cbWorldPoint[RED].x, m_cbWorldPoint[RED].y, m_cbWorldPoint[RED].z, m_cbWorldPoint[RED].rz*(180/PI),
					m_cbWorldPoint[GREEN].x, m_cbWorldPoint[GREEN].y, m_cbWorldPoint[GREEN].z, m_cbWorldPoint[GREEN].rz*(180/PI),
					m_cbWorldPoint[BLUE].x, m_cbWorldPoint[BLUE].y, m_cbWorldPoint[BLUE].z, m_cbWorldPoint[BLUE].rz*(180/PI),
					m_cbWorldPoint[YELLOW].x, m_cbWorldPoint[YELLOW].y, m_cbWorldPoint[YELLOW].z, m_cbWorldPoint[YELLOW].rz*(180/PI));

					/*********************************************************/
					/*T_SW에 Vision data 저장 */
					for(int i=0; i<NUMBOX; i++){
						theApp.T_SW->T_ND.recv_vision[4*i + 0] = m_cbWorldPoint[i].x;
						theApp.T_SW->T_ND.recv_vision[4*i + 1] = m_cbWorldPoint[i].y;
						theApp.T_SW->T_ND.recv_vision[4*i + 2] = m_cbWorldPoint[i].z;
						theApp.T_SW->T_ND.recv_vision[4*i + 3] = m_cbWorldPoint[i].rz;
					}
					/*********************************************************/

					SetDlgItemTextA(IDC_COORDTARGETOBJ, str);


					// TODO, Monitor
					if(pTestImage) {
						m_testView.SetDib(pTestImage->CopyCDib());
						delete pTestImage; pTestImage=NULL;
					}
					

					break;
					}
				case Calibration:
					{
						CDib* pDib = NULL;
						pDib = GetColorClipImage(m_ClipRect);
						if(pDib) { 
							m_imgProcess.SetDib(pDib->CopyCDib());
						}
						//m_testView.SetDib(pDib->GaussianSmoothing(2));
						delete pDib;
					}
					break;
				case Test:
					{
						CDib* pDib = NULL;
						CDib* pTmp = NULL;
						pDib = GetColorClipImage(m_ClipRect);
						if(pDib) { 
							pTmp = pDib->GlobalBinFixed(m_binThres)->ReverseG()->Opening(3); 	// clip image에 대해서..

							// KDE, image point
							TargetObject obj = CellPhonePartRecog(pTmp);
							
							if(obj.density > 0.6) {

								// Orientation
								double rad = pTmp->GetSobelOrientation();
							
								//center = pTmp->GetMeanCoordinateB(0.0);

								MatrixXd StaubliPoint = MatrixXd::Zero(4,1);
								MatrixXd CamPoint	  = MatrixXd::Zero(4,1);

								// Image -> Camera point 변환
								obj.wz = kinect.getDepthValue(obj.ix, obj.iy);
								CamPoint(0,0) = ((obj.ix - (double)m_GlobalCenter.x)*obj.wz)/K(0,0) + OFFSETX;
								CamPoint(1,0) = -((obj.iy - (double)m_GlobalCenter.y)*obj.wz)/K(1,1) + OFFSETY;
								//CamPoint(2,0) = 940 - obj.wz;	// 940 mm: Kinect와 World Frame 사이의 거리
								CamPoint(3,0) = 1.0;

								// Camera -> Staubli point 변환
								StaubliPoint = M_st * CamPoint;	// Transformation Matrix 곱해줌

			
								obj.rox = StaubliPoint(0,0);
								obj.roy = StaubliPoint(1,0);
								obj.roz = -305;	// 땅에 부딪히지 않도록 limit 걸어줌
								obj.Rz = rad;

								
								// Print & Visualization
								CString str;
								int len = 30;
								//char* CellPhoneParts[] = {"크래들", "플러그", "USB커넥터", "이어폰"};
								const CString CellPhoneParts[] = {"크래들", "플러그", "USB커넥터", "이어폰"};
								str.Format("rox: %.2f, roy: %.2f, roz: %.2f, ori: %.2lf, cnt: %d, Obj Class: %s \n", obj.rox, obj.roy, obj.roz, obj.Rz*RtoD, obj.cnt, CellPhoneParts[obj.ID]);
								SetDlgItemTextA(IDC_COORDTARGETOBJ, str);
								unsigned char val=255;
							
								pDib->DrawCross(obj.cix, obj.ciy, val);
								pDib->DrawLine(obj.cix-len*cos(obj.Rz), obj.ciy-len*sin(obj.Rz), obj.cix+len*cos(obj.Rz), obj.ciy+len*sin(obj.Rz), RGB(255,100,128));
							}
							

							m_imgProcess.SetDib(pDib->CopyCDib());
							m_testView.SetDib(pTmp->CopyCDib());
						}
						delete pDib;
						delete pTmp;
					}
					break;
			}
			
		}
	} else {	// 전체 이미지 모드일땐 그냥 출력만...
		if(pColor)
		{
			CDib* pDib = GetColorImage();
			if(pDib != NULL) m_imgProcess.SetDib(pDib);


			switch(m_visionMode) 
			{
				case Test:{
					//CDib* pDib = GetColorImage();
					//m_testView.SetDib(pDib->GaussianSmoothing(3));
					//delete pDib;
					break;
						  }
				default: {
					
						 }

			}
		}
	}
}