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