int main( int argc, char* argv[] ) { //IplImage* img = cvCreateImage(imSize,IPL_DEPTH_8U,3); IplImage* img = cvLoadImage(imcd0,CV_LOAD_IMAGE_UNCHANGED); IplImage* imgA = cvLoadImage(imcd0,CV_LOAD_IMAGE_GRAYSCALE); IplImage* imgB = cvLoadImage(imcd1,CV_LOAD_IMAGE_GRAYSCALE); imSize = cvSize(img->width,img->height); rmax=0.8*((imSize.width>imSize.height)?imSize.height/2:imSize.width/2); rmin=0.2*((imSize.width>imSize.height)?imSize.height/2:imSize.width/2); lx=0.5*imSize.width; ly=0.5*imSize.height; int win_siz = 7; int arr_siz = NUMX*NUMY; CvPoint2D32f p0 = cvPoint2D32f(imSize.width/2,imSize.height/2); IplImage* pyr = cvCreateImage(imSize,8,1); IplImage* pyr_old = cvCreateImage(imSize,8,1); char* status =0; status = (char*)cvAlloc(arr_siz); cvNamedWindow("testWindow"); cvNamedWindow("ImgA"); cvShowImage("ImgA", imgA); cvNamedWindow("ImgB"); cvShowImage("ImgB", imgB); CvPoint2D32f* arrg = new CvPoint2D32f[arr_siz]; CvPoint2D32f* arrg_old = new CvPoint2D32f[arr_siz]; int counter=0; for(int x=0; x<NUMX; x++) { for(int y=0; y<NUMY; y++) { arrg_old[counter].x = p0.x + (-lx/2) + lx*x/NUMX; arrg_old[counter].y = p0.y + (-ly/2) + lx*y/NUMY; counter++; } } cout << "f**k-0" << endl; for(int i=0; i<arr_siz; i++) { cvLine(img,cvPointFrom32f(arrg_old[i]),cvPointFrom32f(arrg_old[i]),CV_RGB(0,0,0),4); } cvShowImage("testWindow",img); cvWaitKey(100); cout << "f**k-1" << endl; cvFindCornerSubPix(imgA, arrg_old, arr_siz, cvSize(win_siz,win_siz), cvSize(2,2), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); //cvReleaseImage(&img); //img = cvLoadImage(imcd0,CV_LOAD_IMAGE_UNCHANGED); cout << "f**k-2" << endl; for(int i=0; i<arr_siz; i++) { cvLine(img,cvPointFrom32f(arrg_old[i]),cvPointFrom32f(arrg_old[i]),CV_RGB(255,0,255),4); } cvShowImage("testWindow",img); cvWaitKey(100); cout << "f**k-3" << endl; float errors[arr_siz]; cvCalcOpticalFlowPyrLK(imgA,imgB, pyr_old, pyr, arrg_old, arrg, arr_siz, cvSize(win_siz,win_siz), 5, status, errors, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.3), 0); CvPoint2D32f dp, dp2; CvPoint2D32f center = cvPoint2D32f(0., 0.); bool arr_draw[arr_siz]; int count = 0; for(int i=0; i<arr_siz; i++) { cvLine(img,cvPointFrom32f(arrg[i]),cvPointFrom32f(arrg[i]),CV_RGB(0,255,0),4); CvScalar color = CV_RGB(255,0,0); dp = getDp(arrg[i],arrg_old[i]); double len = getLength(dp); // if(errors[i]<50) { if(getLength(dp)>3) { color = CV_RGB(255,0,0); } else { color = CV_RGB(100,255,100); } int nc = i+1; arr_draw[i] = false; if((nc>-1) && (nc<arr_siz) && len>3) { dp2=getDp(arrg[nc],arrg_old[nc]); if(getLength(dp2)>2) { CvPoint2D32f ctmp = getCrossPoint(arrg_old[i],getOrtoVec(dp), arrg_old[nc],getOrtoVec(dp2)); // cvLine(img,cvPointFrom32f(arrg_old[i]),cvPointFrom32f(ctmp),CV_RGB(0,0,0),1); // cvLine(img,cvPointFrom32f(arrg[i]),cvPointFrom32f(ctmp),CV_RGB(0,0,0),1); center = getSum(center,ctmp); count++; arr_draw[i] = true; } } drawArrow(img,arrg_old[i],arrg[i],color,2,15.); cout << "status=[" << (int)status[i] << "], error=[" << errors[i] << "]" << endl; // cout << "[" << arrg[i].x << "," << arrg[i].y << "]" << endl; } center=getDiv(center,count); cvCircle(img,cvPointFrom32f(center),10,CV_RGB(0,200,0),1); double df = 0; for(int i=0; i<arr_siz; i++) { if(arr_draw[i]) { cvLine(img, cvPointFrom32f(center), cvPointFrom32f(arrg_old[i]),CV_RGB(0,0,0),1); cvLine(img, cvPointFrom32f(center), cvPointFrom32f(arrg[i]),CV_RGB(0,0,0),1); df += 180.0*(getLength(getDel(arrg[i],arrg_old[i]))) /(CV_PI*getLength(getDel(arrg_old[i],center))); } } CvFont font, fontbg; cvInitFont(&font,CV_FONT_HERSHEY_PLAIN, 2, 2, 0.0, 2, CV_AA); cvInitFont(&fontbg,CV_FONT_HERSHEY_PLAIN, 2, 2, 0.0, 8, CV_AA); char buff[100]; bzero(buff,sizeof(buff)); sprintf(buff,"angle=%0.1f degres",(df/count)); cvPutText(img,buff,cvPoint(10,25),&fontbg,CV_RGB(0,0,0)); cvPutText(img,buff,cvPoint(10,25),&font,CV_RGB(255,0,0)); /* for(int r=0; r<NUMR; r++) { for(int f=0; f<NUMF; f++) { double pfi = 2*CV_PI*f/NUMF; double ro = rmin + (rmax-rmin)*r/NUMR; p1.x = p0.x + ro*cos(pfi); p1.y = p0.y + ro*sin(pfi); //cvLine(img,cvPointFrom32f(p1),cvPointFrom32f(p1),CV_RGB(0,0,255),2); drawArrow(img,p0,p1,CV_RGB(255,0,0)); } } */ cvShowImage("testWindow",img); cvWaitKey(0); cvDestroyWindow("testWindow"); cvReleaseImage(&img); cout << "Shutdown" << endl; return 0; }
void KinectHandler::calculateTriangles( mcCube *cubes, mcPoint *points, unsigned short *pixelData) { int numCubes =(sw-1)*(sh-1); int i; for(i=0;i<numCubes;i++) { mcCube cube =cubes[i]; int pos=0; if(cube.tl->isHit )pos+=1; if(cube.tr->isHit )pos+=2; if(cube.bl->isHit )pos+=4; if(cube.br->isHit )pos+=8; if(pos == 0) { continue; } mcPoint point1; mcPoint point2 ; mcPoint point3 ; if(pos == 15) { addTriangle(cube.tl,cube.tr,cube.bl); addTriangle(cube.tr,cube.br,cube.bl); } else if(pos == 1) { point1 = getCrossPoint(cube.tl,cube.tr,true,pixelData); point2 = getCrossPoint(cube.tl,cube.bl,false,pixelData); addTriangle(&point1,&point2,cube.tl); // delete &point1; //delete &point2; } else if(pos == 2) { point1 = getCrossPoint(cube.tr,cube.br,false,pixelData); point2 = getCrossPoint(cube.tr,cube.tl,true,pixelData); addTriangle(&point1,&point2,cube.tr); } else if(pos == 3) { point1 = getCrossPoint(cube.tl,cube.bl,false ,pixelData); point2 = getCrossPoint(cube.tr,cube.br,false,pixelData ); addTriangle(cube.tl,cube.tr,&point1); addTriangle(cube.tr,&point2,&point1); } else if(pos == 4) { point1 = getCrossPoint(cube.bl,cube.tl,false,pixelData); point2= getCrossPoint(cube.bl,cube.br,true,pixelData); addTriangle(&point1,&point2,cube.bl); } else if(pos == 5) { point1 = getCrossPoint(cube.tl,cube.tr,true,pixelData); point2 = getCrossPoint(cube.bl,cube.br,true,pixelData); addTriangle(cube.bl,&point1,&point2); addTriangle(cube.tl,&point1,cube.bl); } else if(pos == 8) { point1 = getCrossPoint(cube.br,cube.tr,false,pixelData); point2 = getCrossPoint(cube.br,cube.bl,true,pixelData); addTriangle(&point1, cube.br,&point2); } else if(pos == 7) { addTriangle( cube.tl, cube.tr,cube.bl); point1 = getCrossPoint(cube.tr,cube.br,false,pixelData); point2 = getCrossPoint(cube.bl,cube.br,true,pixelData); addTriangle(&point1, &point2,cube.tr); addTriangle(&point2, cube.bl,cube.tr); } else if(pos == 10) { point1 = getCrossPoint(cube.tr,cube.tl,true,pixelData); point2 = getCrossPoint(cube.br,cube.bl,true,pixelData); addTriangle(&point1,cube.tr ,&point2); addTriangle(cube.tr,cube.br,&point2); } else if(pos == 7) { addTriangle( cube.tl, cube.tr,cube.bl); point1 = getCrossPoint(cube.tr,cube.br,false,pixelData); point2 = getCrossPoint(cube.bl,cube.br,true,pixelData); addTriangle(&point1, &point2,cube.tr); addTriangle(&point2, cube.bl,cube.tr); } else if(pos == 11) { addTriangle( cube.tl, cube.tr,cube.br); point1 = getCrossPoint(cube.tl,cube.bl,false,pixelData); point2 = getCrossPoint(cube.br,cube.bl,true ,pixelData); addTriangle(&point1,cube.br ,&point2); addTriangle(&point1, cube.tl,cube.br); } else if(pos == 12) { point2 = getCrossPoint(cube.br,cube.tr,false,pixelData); point1 = getCrossPoint(cube.bl,cube.tl,false,pixelData); addTriangle(&point1, &point2,cube.bl); addTriangle(&point2, cube.br,cube.bl); } else if(pos == 13) { addTriangle( cube.br, cube.bl,cube.tl); point1 = getCrossPoint(cube.tl,cube.tr,true,pixelData); point2 = getCrossPoint(cube.br,cube.tr,false,pixelData); addTriangle(&point1,&point2,cube.br ); addTriangle(&point1, cube.br,cube.tl); } else if(pos == 14) { addTriangle( cube.bl, cube.tr,cube.br); point1 = getCrossPoint(cube.tr,cube.tl,true,pixelData); point2 = getCrossPoint(cube.bl,cube.tl,false,pixelData); addTriangle(&point1,cube.bl ,&point2); addTriangle(&point1, cube.tr,cube.bl); } else if(pos == 6) { point1 = getCrossPoint(cube.tr,cube.tl,true,pixelData); point2 = getCrossPoint(cube.bl,cube.tl,false ,pixelData); addTriangle( cube.tr, &point1,cube.bl); addTriangle( &point1,cube.bl,&point2); point1 = getCrossPoint(cube.tr,cube.br,false ,pixelData); point2 = getCrossPoint(cube.bl,cube.br,true,pixelData); addTriangle( cube.tr, &point1,cube.bl); addTriangle( &point1,&point2,cube.bl); } else if(pos == 9) { point1 = getCrossPoint(cube.tl,cube.tr,true,pixelData); point2 = getCrossPoint(cube.br,cube.tr,false ,pixelData); addTriangle( cube.tl, &point1,cube.br); addTriangle( &point1,&point2,cube.br); point1 = getCrossPoint(cube.tl,cube.bl,false ,pixelData); point2 = getCrossPoint(cube.br,cube.bl,true,pixelData); addTriangle( cube.tl,cube.br, &point1); addTriangle( &point1,cube.br,&point2); } } }