#ifdef _CH_
#pragma package
#endif
#define DEBUG 0 //change this from 0 or 1.
#define MAXSQUARE 80
#include "cv.h"
#include "highgui.h"
#include
#include
#include
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]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;
}
