예제 #1
0
void CVisionTab::updateDepthStream()
{
	if(WAIT_OBJECT_0 == WaitForSingleObject(kinect.m_dFrameReadyEvent, 0))
	{

	if(pBufDepth == NULL && pDepth != NULL) {
		pBufDepth = pDepth->CopyCDib();
		SetEvent(m_GetDimgEvent);
	}

	if(pDepth != NULL) {delete pDepth; pDepth=NULL;}

	CDib *pDibDepth = kinect.getCDibDepthImage()->CopyCDib();
	pDepth = pDibDepth->CopyCDib();
	
	
	if(pDibDepth) {
		// Display update
		if(clip) {
			CDib *pDepth = pDibDepth->ClipCDib(m_ClipRect);	// memory leak when using median filter... 
			m_DispDepth.SetDib(pDepth);

		} else {
			m_DispDepth.SetDib(pDibDepth->CopyCDib());
		}

		delete pDibDepth;
	}

	}
}
예제 #2
0
CDib* CVisionTab::GetDepthClipImage(RECT *pRect)
{
	CDib* pTmp;
	CDib* pDib;
	
	if(WAIT_OBJECT_0 == WaitForSingleObject(m_GetDimgEvent, INFINITE))
		pTmp = GetDepthImage();
	if(pTmp) pDib = pTmp->ClipCDib(pRect);
	delete pTmp; pTmp=NULL;

	return pDib;
}
예제 #3
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;
}