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