Mobile-C

<?xml version="1.0"?>

<!DOCTYPE myMessage SYSTEM "gafmessage.dtd">

<GAF_MESSAGE>
  <MESSAGE message="MOBILE_AGENT">
    <MOBILE_AGENT>
     <AGENT_DATA>
       <NAME>visionagent</NAME>
       <OWNER>IEL</OWNER>
       <HOME>mouse2.engr.ucdavis.edu:5051</HOME>
       <TASK task="1" num="0">
	   <DATA name="objPXLoc" 
			 complete="0" 
			 server="fish.engr.ucdavis.edu:5051">
       </DATA>
	  <AGENT_CODE>
          <![CDATA[
#ifdef _CH_
#pragma package <opencv>
#endif
#define DEBUG 0 //change this from 0 or 1.
#define MAXSQUARE 80
#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
short FinalCoord[2][3]; 
short objPXLoc[2]; 
int thresh = 50;
IplImage* img = 0;
IplImage* frame = 0;
CvMemStorage* storage = 0;
int counti=0, countj=1;
int centerpoints[3][100];

// helper function:
// finds a cosine of angle between vectors
// from pt0->pt1 and from pt0->pt2 
double angle( CvPoint* pt1, CvPoint* pt2, CvPoint* pt0 )
{
    double dx1 = pt1->x - pt0->x;
    double dy1 = pt1->y - pt0->y;
    double dx2 = pt2->x - pt0->x;
    double dy2 = pt2->y - pt0->y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

// returns sequence of squares detected on the image.
// the sequence is stored in the specified memory storage
CvSeq* findSquares4( IplImage* img, CvMemStorage* storage )
{
    CvSeq* contours;
    int i, c, l, N = 11;
    CvSize sz = cvSize( img->width & -2, img->height & -2 );
    IplImage* timg = cvCloneImage( img ); // make a copy of input image
    IplImage* gray = cvCreateImage( sz, 8, 1 ); 
    IplImage* pyr = cvCreateImage( cvSize(sz.width/2, sz.height/2), 8, 3 );
    IplImage* tgray;
    CvSeq* result;
    double s, t;
    // create empty sequence that will contain points -
    // 4 points per square (the square's vertices)
    CvSeq* squares = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage );

    // select the maximum ROI in the image
    // with the width and height divisible by 2
    cvSetImageROI( timg, cvRect( 0, 0, sz.width, sz.height ));
    
    // down-scale and upscale the image to filter out the noise
    cvPyrDown( timg, pyr, 7 );
    cvPyrUp( pyr, timg, 7 );
    tgray = cvCreateImage( sz, 8, 1 );

    
    // find squares in every color plane of the image
    for( c = 0; c < 3; c++ )
    {
        // extract the c-th color plane
        cvSetImageCOI( timg, c+1 );
        cvCopy( timg, tgray, 0 );
        
        // try several threshold levels
        for( l = 0; l < N; l++ )
        {
            // hack: use Canny instead of zero threshold level.
            // Canny helps to catch squares with gradient shading   
            if( l == 0 )
            {
                // apply Canny. Take the upper threshold from slider
                // and set the lower to 0 (which forces edges merging) 
            //cvCanny( tgray, gray, 0, thresh, 5 );
                // dilate canny output to remove potential
                // holes between edge segments 
               // cvDilate( gray, gray, 0, 1 );
            }
            else
            {
                // apply threshold if l!=0:
                //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
                cvThreshold( tgray, gray, (l+1)*255/N, 255, CV_THRESH_BINARY );
            }

            // find contours and store them all as a list
            cvFindContours( gray, storage, &contours, sizeof(CvContour),
                CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
            
            // test each contour
            while( contours )
            {
                // approximate contour with accuracy proportional
                // to the contour perimeter
                result = cvApproxPoly( contours, sizeof(CvContour), storage,
                    CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 );
                // square contours should have 4 vertices after approximation
                // relatively large area (to filter out noisy contours)
                // and be convex.
                // Note: absolute value of an area is used because
                // area may be positive or negative - in accordance with the
                // contour orientation
                if( result->total == 4 &&
                    fabs(cvContourArea(result,CV_WHOLE_SEQ)) > 1000 &&
                    cvCheckContourConvexity(result) )
                {
                    s = 0;
                    
                    for( i = 0; i < 5; i++ )
                    {
                        // find minimum angle between joint
                        // edges (maximum of cosine)
                        if( i >= 2 ) //DEFAULT 2
                        {
                            t = fabs(angle(
                            (CvPoint*)cvGetSeqElem( result, i ),
                            (CvPoint*)cvGetSeqElem( result, i-2 ),
                            (CvPoint*)cvGetSeqElem( result, i-1 )));
                            s = s > t ? s : t;
                        }
                    }
                    
                    // if cosines of all angles are small
                    // (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence 
                    if( s < 0.3 )
                        for( i = 0; i < 4; i++ )
                            cvSeqPush( squares,
                                (CvPoint*)cvGetSeqElem( result, i ));
                }
                
                // take the next contour
                contours = contours->h_next;
            }
        }
    }

    // release all the temporary images
    cvReleaseImage( &gray );
    cvReleaseImage( &pyr );
    cvReleaseImage( &tgray );
    cvReleaseImage( &timg );
    
    return squares;
}

int length(CvPoint a, CvPoint b)
{ //finds the distance between two points
    return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}

int centers(CvPoint *pt, CvPoint *center) 
{  //finds the center of a rectange given 4 points
        center->x = ((pt[0].x + pt[2].x)/2 + (pt[1].x + pt[3].x)/2)/2;
        center->y = ((pt[0].y + pt[2].y)/2 + (pt[1].y + pt[3].y)/2)/2;
        return 0;
}
double duplicateDel(CvPoint *pt, CvPoint *oldPt, int Check){
    int i;
    double AvgVal[2], AvgDiff;
    if(Check==1){
        printf("testing");
        return 0;          
    }
    else{
        for(i=0;i<4;++i){
            oldPt[i].x=pt[i].x;
            oldPt[i].y=pt[i].y;
        }
        /*for(i=1;i<4;++i){
            AvgVal[0]+=oldPt[i].x;
            AvgVal[0]+=oldPt[i].y;
            AvgVal[1]+=pt[i].x;
            AvgVal[1]+=pt[i].y;
            printf("====%f\n",Olt1]);
            printf("====%f\n",AvgVal[0]);
        }*/
    }
    return abs(AvgVal[0]-AvgVal[1]);
}

double sortArrayptr(int n, int a1[3][n]){
    int i, j;
    int temp;
    int *a = &a1[0][0];
    int *b = &a1[1][0];
    int *c = &a1[2][0];
    double median;
    for(i=0; i<=(n-2); i++) {
        for(j=i+1; j<=(n-1); j++) {
            if(*(a+i) > *(a+j)) {
                temp = *(a+i);
                *(a+i) = *(a+j);
                *(a+j) = temp;
                temp = *(b+i);
                *(b+i) = *(b+j);
                *(b+j) = temp;
                temp = *(c+i);
                *(c+i) = *(c+j);
                *(c+j) = temp;
            }
        }
    }
    return 0;
}
int orientation(CvPoint *pt, CvPoint *o_pt, CvPoint *center)
{    
    double theta;
    centers(pt, center);
    /*if(length(pt[0],pt[1])>length(pt[0],pt[3])){
        //printf("ver1\n");
        theta = ((atan((pt[0].x-pt[2].x)/(pt[0].y-pt[2].y))-M_PI/2)+atan((pt[1].x-pt[3].x)/(pt[1].y-pt[3].y))-M_PI/2)/2;
        printf("%f\n", theta);
        printf("(pt %d,%d) ", pt[0].x, o_pt[0].y);
        o_pt->x = length(pt[0],pt[1])*cos(theta)+center->x;
        o_pt->y = length(pt[0],pt[1])*sin(theta)+center->y;

    }
    
    else {*/
        theta = (atan(abs(pt[0].x-pt[1].x)/abs(pt[0].y-pt[1].y)) +
                atan(abs(pt[2].x-pt[3].x)/abs(pt[2].y-pt[3].y)))/2;
        o_pt->x = length(pt[0],pt[1])*cos(theta)+center->x;
        o_pt->y = length(pt[0],pt[1])*sin(theta)+center->y;
   // }
    return 0;
}
// the function draws all the squares in the image
void drawSquares( IplImage* img, CvSeq* squares )
{
    CvSeqReader reader;
    IplImage* cpy = cvCloneImage( img );
    int i, check=1;

    // initialize reader of the sequence
    cvStartReadSeq( squares, &reader, 0 );

    // read 4 sequence elements at a time (all vertices of a square)
    for( i = 0; i < squares->total; i += 4 )
    {
        CvPoint pt[4], *rect = pt, center, o_pt, oldPt[4];
        int count = 4;
 

        // read 4 vertices
        double lengths[2]; //length of each side of the square
        
        //duplicateDel(pt, oldPt, check);
        CV_READ_SEQ_ELEM( pt[0], reader );
        CV_READ_SEQ_ELEM( pt[1], reader );
        CV_READ_SEQ_ELEM( pt[2], reader );
        CV_READ_SEQ_ELEM( pt[3], reader );
        int crap;
        for(crap = 0; crap < 4; crap++)
        lengths[0]=length(pt[1],pt[0]);
        lengths[1]=length(pt[3],pt[2]); 
      // lengths[0]=length(pt[1],pt[3]);
       //lengths[1]=length(pt[3],pt[0]); 
        // draw the square as a closed polyline and checks it to see if it is smaller than a certain dimension.
        // printf("%d,%d\n", height, width);
      
        if(lengths[0]*lengths[1]<MAXSQUARE*MAXSQUARE && lengths[0]*lengths[1]>10 && abs(lengths[0]-lengths[1])<2){
            centers(pt, ¢er);
     
			//orientation(pt, &o_pt, ¢er);
           // printf("(o_pt %d,%d) ", o_pt.x, o_pt.y);
            //printf("(center %d,%d) ", center.x, center.y);
            cvPolyLine( cpy, &rect, &count, 1, 1, CV_RGB(0,255,0), .1, CV_AA, 0 );
            cvCircle( cpy, center, 2, CV_RGB(0,255,0), .1, CV_AA, 0 );
       
            cvLine(cpy, center, o_pt, CV_RGB(255,0,0), .1, CV_AA, 0);
        }
       
        if (center.x!=0 || center.y!=0){
            centerpoints[0][counti]=center.x+center.y;
            centerpoints[1][counti]=center.x;
            centerpoints[2][counti]=center.y;
            counti+=1;
        }
   
            
        //
        check=0;
    }
//DEBUG
    
}



int main(int argc, char** argv)
{
    int i, j, c;
    CvCapture* capture = 0;
    IplImage* frame;
	//FinalCoord[0][X},[1][Y] --center points of the final chosen squares.

	printf("Arrived\n");

    if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
        capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
    else if( argc == 2 )
        capture = cvCaptureFromAVI( argv[1] ); 


    if( !capture )
    {
        printf("Could not initialize capturing...\n");
        return -1;
    }

	printf("Start Capturing\n");

    // create memory storage that will contain all the dynamic data
    frame =  cvQueryFrame( capture );
    storage = cvCreateMemStorage(0);

    img = cvCloneImage( frame );

	printf("Draw Squares\n");
	// find and draw the squares
	drawSquares( img, findSquares4( img, storage ) );
	printf("Draw Squares Done\n");

	printf("Sort\n");
	//sort the centerpoints array by x/y sum
	sortArrayptr(counti, centerpoints);
	if(centerpoints[1][countj]!=0 && centerpoints[2][countj]!=0)
	{
	  printf("center %d is at %d %d\n", 
	     countj, centerpoints[1][countj], centerpoints[2][countj]);
	  FinalCoord[0][0]=centerpoints[1][countj];
	  FinalCoord[1][0]=centerpoints[2][countj];
	}
	printf("Sort Done\n");

	// clear out duplicate squares by finding 
	// the difference in positions of their centers (should be >5)
	for (j=1;j<=counti;++j)
	{
	  if(abs(centerpoints[1][j-1]-centerpoints[1][j])>5 
	  || abs(centerpoints[2][j-1]-centerpoints[2][j])>5)
	  {
	    if(centerpoints[1][j]>0 && centerpoints[2][j]>0)
    	{
      	  countj=countj+1;
	      FinalCoord[0][j]=centerpoints[1][countj];
	      FinalCoord[1][j]=centerpoints[2][countj];
	      printf("center %d is at %d %d\n", 
	        countj, centerpoints[1][j], centerpoints[2][j]);
	    }
	  }
	}


    // clear memory storage - reset free space position
    cvReleaseImage( &img );
    cvClearMemStorage( storage );
    cvReleaseCapture(&capture);

	objPXLoc[0] = FinalCoord[0][0];
	objPXLoc[1] = FinalCoord[1][0];
	printf("%d %d\n", objPXLoc[0], objPXLoc[1]);
   
	printf("Done\n");
    return 0;
}

        ]]>
        </AGENT_CODE>
      </TASK>
      </AGENT_DATA>
    </MOBILE_AGENT>
  </MESSAGE>
</GAF_MESSAGE>