root / trunk / code / projects / colonet / server / vision / capturetest.c @ 936
History | View | Annotate | Download (7.53 KB)
1 |
#include "vision.h" |
---|---|
2 |
|
3 |
#include <cv.h> |
4 |
#include <highgui.h> |
5 |
|
6 |
#include <stdio.h> |
7 |
|
8 |
#define MINH 50 //min threshold level |
9 |
#define MAXH 75 //max threshold level |
10 |
|
11 |
#define MIN_BOX_WIDTH 10 |
12 |
#define MAX_BOX_WIDTH 20 |
13 |
#define MIN_BOX_HEIGHT 10 |
14 |
#define MAX_BOX_HEIGHT 20 |
15 |
|
16 |
#define BEST_CENTER_PADDING_X 9 |
17 |
#define BEST_CENTER_PADDING_Y 9 |
18 |
|
19 |
#define ELLIPSE_COUNT_TO_BE_OBJECT 7 |
20 |
|
21 |
struct CenterP {
|
22 |
CvPoint center; |
23 |
int count;
|
24 |
}; |
25 |
|
26 |
static CvCapture *capture;
|
27 |
|
28 |
void compute(aTYPE *a, int length, IplImage* image, IplImage* out) |
29 |
{ |
30 |
int *values = (int*)out->imageData; |
31 |
|
32 |
for(int y = 0; y < image->height; y++) |
33 |
{ |
34 |
for(int x = 0; x < image->width; x++) |
35 |
{ |
36 |
bool d = ((uchar*)(image->imageData + image->widthStep*y))[x] > 0; |
37 |
if(!d) continue; |
38 |
for(int i = 0; i < length; i++) |
39 |
{ |
40 |
int centerj = y + a[i].rhoj;
|
41 |
int centeri = x + a[i].thetaj;
|
42 |
if(centerj < 0 || centerj >= image->height || centeri < 0 || centeri >= image->width) |
43 |
continue;
|
44 |
values[centerj * image->width + centeri]++; |
45 |
} |
46 |
} |
47 |
} |
48 |
} |
49 |
void circleTransform(IplImage* image, IplImage* out, int radius) |
50 |
{ |
51 |
int amax = 8 * radius; |
52 |
aTYPE *a = (aTYPE*)malloc(sizeof(aTYPE) * amax);
|
53 |
int i = 0; |
54 |
for(int j = 0; j < amax; j++) |
55 |
{ |
56 |
double theta = (6.2831853071795862 * ((double)j)) / ((double)amax); |
57 |
int rhoj = (int)cvRound(radius * cos(theta)); |
58 |
int thetaj = (int)cvRound(radius * sin(theta)); |
59 |
if(i == 0 || rhoj != a[i].rhoj && thetaj != a[i].thetaj) |
60 |
{ |
61 |
a[i].rhoj = rhoj; |
62 |
a[i].thetaj = thetaj; |
63 |
i++; |
64 |
} |
65 |
} |
66 |
|
67 |
compute(a, amax, image, out); |
68 |
free(a); |
69 |
} |
70 |
|
71 |
// A Simple Camera Capture Framework
|
72 |
int main() {
|
73 |
|
74 |
unsigned char buffer[150000]; |
75 |
int written;
|
76 |
|
77 |
CvCapture* capture = cvCaptureFromCAM( -1 );
|
78 |
if( !capture ) {
|
79 |
fprintf( stderr, "ERROR: capture is NULL \n" );
|
80 |
getchar(); |
81 |
return -1; |
82 |
} |
83 |
|
84 |
// Create a window in which the captured images will be presented
|
85 |
cvNamedWindow( "mywindow1", CV_WINDOW_AUTOSIZE );
|
86 |
cvNamedWindow( "mywindow2", CV_WINDOW_AUTOSIZE );
|
87 |
//cvNamedWindow( "mywindow3", CV_WINDOW_AUTOSIZE );
|
88 |
//cvNamedWindow( "mywindow4", CV_WINDOW_AUTOSIZE );
|
89 |
//cvNamedWindow( "mywindow5", CV_WINDOW_AUTOSIZE );
|
90 |
//cvNamedWindow( "mywindow6", CV_WINDOW_AUTOSIZE );
|
91 |
//cvNamedWindow( "mywindow7", CV_WINDOW_AUTOSIZE );
|
92 |
|
93 |
|
94 |
// Show the image captured from the camera in the window and repeat
|
95 |
while( 1 ) { |
96 |
IplImage *image03, *image04; |
97 |
IplImage *im_capture; |
98 |
|
99 |
if(!cvGrabFrame(capture)){ // capture a frame |
100 |
printf("Could not grab a frame\n\7");
|
101 |
exit(0);
|
102 |
} |
103 |
im_capture=cvRetrieveFrame(capture); // retrieve the captured frame
|
104 |
|
105 |
image03 = cvCreateImage(cvGetSize(im_capture), im_capture->depth, 1);
|
106 |
cvCvtColor(im_capture, image03, CV_RGB2GRAY); |
107 |
|
108 |
/*/ load image and force it to be grayscale
|
109 |
if ((image03 = cvLoadImage(filename, 0)) == 0) {
|
110 |
fprintf(stderr, "Failed to load image.\n");
|
111 |
return -1;
|
112 |
}*/
|
113 |
|
114 |
// Create the destination images
|
115 |
IplImage* image02 = cvCloneImage(image03); |
116 |
|
117 |
// Create dynamic structure and sequence.
|
118 |
CvMemStorage* stor = cvCreateMemStorage(0);
|
119 |
CvSeq* cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor); |
120 |
|
121 |
struct CenterP bestc[100]; |
122 |
int index=0; |
123 |
|
124 |
// Threshold the source image. This needful for cvFindContours().
|
125 |
cvCanny(image03, image02, 50, 100); |
126 |
cvDilate(image02, image02); |
127 |
|
128 |
cvShowImage( "mywindow2", image02 );
|
129 |
|
130 |
// Find all contours.
|
131 |
cvFindContours(image02, stor, &cont, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0)); |
132 |
|
133 |
// Clear images. IPL use.
|
134 |
cvZero(image02); |
135 |
|
136 |
// This cycle draw all contours and approximate it by ellipses.
|
137 |
for(; cont; cont = cont->h_next) {
|
138 |
int i; // Indicator of cycle. |
139 |
int count = cont->total; // This is number point in contour |
140 |
CvPoint center; |
141 |
CvSize size; |
142 |
|
143 |
// Number point must be more than or equal to 6 (for cvFitEllipse_32f).
|
144 |
if (count < 6) continue; |
145 |
|
146 |
// Alloc memory for contour point set.
|
147 |
CvPoint* PointArray = (CvPoint*) malloc(count * sizeof(CvPoint));
|
148 |
CvPoint2D32f* PointArray2D32f= (CvPoint2D32f*) malloc(count * sizeof(CvPoint2D32f));
|
149 |
|
150 |
// Alloc memory for ellipse data.
|
151 |
CvBox2D32f* box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
|
152 |
|
153 |
/* Use Hough Transform to check circularity of contours, threshold Hough space
|
154 |
Second gen (?): Use Harr cascade on bot shape */
|
155 |
|
156 |
/*for( ; cont != 0; cont = cont->h_next )
|
157 |
{
|
158 |
CvRect rect = ((CvContour*)cont)->rect;
|
159 |
if (rect.width < RADIUS - R_ERROR || rect.height < RADIUS - R_ERROR) continue;
|
160 |
|
161 |
cvZero(imageBlobs);
|
162 |
cvZero(imageOutlines);
|
163 |
cvDrawContours( imageOutlines, contour, CV_RGB(255,255,255), CV_RGB(255,255,255), -1, 1, 8 );
|
164 |
cvDrawContours( imageBlobs, contour, CV_RGB(255,255,255), CV_RGB(255,255,255), -1, CV_FILLED, 8 );
|
165 |
|
166 |
CvScalar drawcolor = CV_RGB( rand()&255, rand()&255, rand()&255 );
|
167 |
cvDrawContours( imageCircles, contour, drawcolor, drawcolor, -1, CV_FILLED, 8 );
|
168 |
}*/
|
169 |
|
170 |
// Get contour point set.
|
171 |
cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); |
172 |
|
173 |
// Convert CvPoint set to CvBox2D32f set.
|
174 |
for(i=0; i<count; i++) { |
175 |
PointArray2D32f[i].x = (float)PointArray[i].x;
|
176 |
PointArray2D32f[i].y = (float)PointArray[i].y;
|
177 |
} |
178 |
|
179 |
// Fits ellipse to current contour.
|
180 |
cvFitEllipse(PointArray2D32f, count, box); |
181 |
|
182 |
// Convert ellipse data from float to integer representation.
|
183 |
center.x = cvRound(box->center.x); |
184 |
center.y = cvRound(box->center.y); |
185 |
size.width = cvRound(box->size.width*0.5); |
186 |
size.height = cvRound(box->size.height*0.5); |
187 |
box->angle = -box->angle; |
188 |
|
189 |
if (size.width > MIN_BOX_WIDTH && size.height > MIN_BOX_HEIGHT
|
190 |
&& size.width < MAX_BOX_WIDTH && size.height < MAX_BOX_HEIGHT){ |
191 |
//printf("%d %d %d %d\n",center.x,center.y,size.width,size.height);
|
192 |
|
193 |
int found=0, j; |
194 |
for (j = 0; j < index; j++) { |
195 |
if (abs(bestc[j].center.x-center.x) < BEST_CENTER_PADDING_X
|
196 |
&& abs(bestc[j].center.y-center.y) < BEST_CENTER_PADDING_Y) { |
197 |
bestc[j].count++; |
198 |
found=1;
|
199 |
break;
|
200 |
} |
201 |
} |
202 |
|
203 |
if (!found){
|
204 |
struct CenterP c;
|
205 |
c.center=center; |
206 |
c.count=1;
|
207 |
bestc[index]=c; |
208 |
index++; |
209 |
} |
210 |
} |
211 |
|
212 |
// Free memory.
|
213 |
free(PointArray); |
214 |
free(PointArray2D32f); |
215 |
free(box); |
216 |
} |
217 |
|
218 |
image04 = cvCloneImage(image03); |
219 |
cvZero(image04); |
220 |
|
221 |
int i;
|
222 |
for (i = 0; i < index; i++) { |
223 |
//if (bestc[i].count > ELLIPSE_COUNT_TO_BE_OBJECT){
|
224 |
cvCircle(image04, bestc[i].center, 20, CV_RGB(0,0,0), 5, 8, 0); |
225 |
//}
|
226 |
} |
227 |
|
228 |
cvReleaseImage(&image02); |
229 |
cvReleaseImage(&image03); |
230 |
|
231 |
// Show image. HighGUI use.
|
232 |
cvShowImage( "mywindow1", image04 );
|
233 |
|
234 |
cvReleaseImage(&image04); |
235 |
|
236 |
cvReleaseMemStorage(&stor); |
237 |
|
238 |
//If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
|
239 |
//remove higher bits using AND operator
|
240 |
if( (cvWaitKey(0) & 255) == 27 ) break; |
241 |
} |
242 |
|
243 |
// Release the capture device housekeeping
|
244 |
cvReleaseCapture( &capture ); |
245 |
|
246 |
cvDestroyWindow( "mywindow1" );
|
247 |
cvDestroyWindow( "mywindow2" );
|
248 |
cvDestroyWindow( "mywindow3" );
|
249 |
cvDestroyWindow( "mywindow4" );
|
250 |
cvDestroyWindow( "mywindow5" );
|
251 |
cvDestroyWindow( "mywindow6" );
|
252 |
cvDestroyWindow( "mywindow7" );
|
253 |
return 0; |
254 |
} |