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: {
}
}
}
}
}