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