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; } } }
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; }
///////////// 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; }