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; } } }
void CVisionTab::updateColorStream() { if(WAIT_OBJECT_0 == WaitForSingleObject(kinect.m_cFrameReadyEvent, 0)) { //ResetEvent(m_GetCimgEvent); if(pBufColor == NULL && pColor != NULL ) { pBufColor = pColor->CopyCDib(); SetEvent(m_GetCimgEvent); } if(pColor != NULL) {delete pColor; pColor=NULL;} CDib* temp = kinect.getCDibColorImage(); if(!temp) return; CDib *pDibRGB = temp->CopyCDib(); //pColor = pDibRGB->CopyCDib(); pColor = pDibRGB->GetDistortionCorrectedImage(532.45, 531.82, 0, 331.7, 264.18, 0.16657, -0.29178); // 카메라 렌즈 왜곡 보정 이미지 static bool flag = FALSE; // 처음 한 번만 Global image의 중심 좌표 계산 if(!flag && pDibRGB) { // Image의 중심 좌표 m_GlobalCenter.x = pDibRGB->Width()/2; m_GlobalCenter.y = pDibRGB->Height()/2; // Calibration을 통해 구한 Principal point 저장 //m_GlobalCenter.x = 331; //m_GlobalCenter.y = 264; flag = TRUE; } if(pDibRGB) { // Display update if(clip) { // 원본 이미지도 같이 clip 하려면 주석 반대로.. /*CDib *pRGB = pDibRGB->ClipCDib(m_ClipRect); m_DispRGB.SetDib(pRGB);*/ m_DispRGB.SetDib(pDibRGB->CopyCDib()); CalcTime(); // Calc FPS } else { m_DispRGB.SetDib(pDibRGB->CopyCDib()); CalcTime(); // Calc FPS } delete pDibRGB; } //SetEvent(m_GetCimgEvent); } }
// ------------------------------------------------------------------ // ------------------ 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: { } } } } }