root / trunk / code / projects / colonet / vision / vision.c @ 256
History | View | Annotate | Download (7.44 KB)
1 | 250 | chihsiuh | /**
|
---|---|---|---|
2 | * find the locations of each robot
|
||
3 | * based on opencv's sample program fitellipse.c by Denis Burenkov
|
||
4 | *
|
||
5 | * @author Rich Hong
|
||
6 | * @date 11/10/2007
|
||
7 | */
|
||
8 | #ifdef _CH_
|
||
9 | #pragma package <opencv>
|
||
10 | #endif
|
||
11 | |||
12 | #ifndef _EiC
|
||
13 | #include "cv.h" |
||
14 | #include "highgui.h" |
||
15 | #endif
|
||
16 | #include <stdio.h> |
||
17 | #include <stdlib.h> |
||
18 | |||
19 | int slider_pos = 200; |
||
20 | |||
21 | #define MINH 100 //min threshold level |
||
22 | #define MAXH 150 //max threshold level |
||
23 | #define DEBUG 0 //Debug to find threshold level |
||
24 | |||
25 | // Load the source image. HighGUI use.
|
||
26 | IplImage *image02 = 0, *image03 = 0, *image04 = 0; |
||
27 | |||
28 | void process_image(int h); |
||
29 | void loop_image();
|
||
30 | |||
31 | struct CenterP {
|
||
32 | CvPoint center; |
||
33 | int count;
|
||
34 | }; |
||
35 | |||
36 | int main( int argc, char** argv ) { |
||
37 | const char* filename = argc == 2 ? argv[1] : (char*)"colonet.jpg"; |
||
38 | |||
39 | // load image and force it to be grayscale
|
||
40 | if( (image03 = cvLoadImage(filename, 0)) == 0 ) return -1; |
||
41 | |||
42 | // Create the destination images
|
||
43 | image02 = cvCloneImage( image03 ); |
||
44 | image04 = cvCloneImage( image03 ); |
||
45 | |||
46 | if (DEBUG){
|
||
47 | // Create windows.
|
||
48 | cvNamedWindow("Source", 1); |
||
49 | // Show the image.
|
||
50 | cvShowImage("Source", image03);
|
||
51 | |||
52 | 256 | chihsiuh | cvNamedWindow("Result", 1); |
53 | // Create toolbars. HighGUI use.
|
||
54 | 250 | chihsiuh | cvCreateTrackbar( "Threshold", "Result", &slider_pos, 255, process_image ); |
55 | } |
||
56 | |||
57 | if (!DEBUG) loop_image();
|
||
58 | 256 | chihsiuh | if (DEBUG) process_image(0); |
59 | 250 | chihsiuh | |
60 | // Wait for a key stroke; the same function arranges events processing
|
||
61 | 256 | chihsiuh | if (DEBUG) cvWaitKey(0); |
62 | 250 | chihsiuh | cvReleaseImage(&image02); |
63 | cvReleaseImage(&image03); |
||
64 | |||
65 | 256 | chihsiuh | if (DEBUG){
|
66 | cvDestroyWindow("Source");
|
||
67 | cvDestroyWindow("Result");
|
||
68 | } |
||
69 | 250 | chihsiuh | |
70 | return 0; |
||
71 | } |
||
72 | |||
73 | void loop_image(){
|
||
74 | CvMemStorage* stor; |
||
75 | CvSeq* cont; |
||
76 | CvBox2D32f* box; |
||
77 | CvPoint* PointArray; |
||
78 | CvPoint2D32f* PointArray2D32f; |
||
79 | |||
80 | // Create dynamic structure and sequence.
|
||
81 | stor = cvCreateMemStorage(0);
|
||
82 | cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor); |
||
83 | |||
84 | struct CenterP bestc[100]; |
||
85 | int index=0; |
||
86 | |||
87 | int h;
|
||
88 | for (h=MINH;h<MAXH;h++){
|
||
89 | // Threshold the source image. This needful for cvFindContours().
|
||
90 | cvThreshold( image03, image02, h, 255, CV_THRESH_BINARY );
|
||
91 | |||
92 | // Find all contours.
|
||
93 | cvFindContours( image02, stor, &cont, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0)); |
||
94 | |||
95 | // Clear images. IPL use.
|
||
96 | cvZero(image02); |
||
97 | cvZero(image04); |
||
98 | |||
99 | // This cycle draw all contours and approximate it by ellipses.
|
||
100 | for(;cont;cont = cont->h_next) {
|
||
101 | int i; // Indicator of cycle. |
||
102 | int count = cont->total; // This is number point in contour |
||
103 | CvPoint center; |
||
104 | CvSize size; |
||
105 | |||
106 | // Number point must be more than or equal to 6 (for cvFitEllipse_32f).
|
||
107 | if( count < 6 ) continue; |
||
108 | |||
109 | // Alloc memory for contour point set.
|
||
110 | PointArray = (CvPoint*)malloc( count*sizeof(CvPoint) );
|
||
111 | PointArray2D32f= (CvPoint2D32f*)malloc( count*sizeof(CvPoint2D32f) );
|
||
112 | |||
113 | // Alloc memory for ellipse data.
|
||
114 | box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
|
||
115 | |||
116 | // Get contour point set.
|
||
117 | cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); |
||
118 | |||
119 | // Convert CvPoint set to CvBox2D32f set.
|
||
120 | for(i=0; i<count; i++) { |
||
121 | PointArray2D32f[i].x = (float)PointArray[i].x;
|
||
122 | PointArray2D32f[i].y = (float)PointArray[i].y;
|
||
123 | } |
||
124 | |||
125 | // Fits ellipse to current contour.
|
||
126 | cvFitEllipse(PointArray2D32f, count, box); |
||
127 | |||
128 | // Convert ellipse data from float to integer representation.
|
||
129 | center.x = cvRound(box->center.x); |
||
130 | center.y = cvRound(box->center.y); |
||
131 | size.width = cvRound(box->size.width*0.5); |
||
132 | size.height = cvRound(box->size.height*0.5); |
||
133 | box->angle = -box->angle; |
||
134 | |||
135 | if (size.width>10&&size.height>10&&size.width<20&&size.height<20){ |
||
136 | //printf("%d %d %d %d\n",center.x,center.y,size.width,size.height);
|
||
137 | |||
138 | int found=0; |
||
139 | int j;
|
||
140 | for (j=0;j<index;j++) |
||
141 | if (abs(bestc[j].center.x-center.x)<9&&abs(bestc[j].center.y-center.y)<9){ |
||
142 | bestc[j].count++; |
||
143 | found=1;
|
||
144 | break;
|
||
145 | } |
||
146 | if (!found){
|
||
147 | struct CenterP c;
|
||
148 | c.center=center; |
||
149 | c.count=1;
|
||
150 | bestc[index]=c; |
||
151 | index++; |
||
152 | } |
||
153 | } |
||
154 | // Free memory.
|
||
155 | free(PointArray); |
||
156 | free(PointArray2D32f); |
||
157 | free(box); |
||
158 | } |
||
159 | } |
||
160 | image04 = cvCloneImage( image03 ); |
||
161 | int i;
|
||
162 | for (i=0;i<index;i++){ |
||
163 | if (bestc[i].count>7){ |
||
164 | printf("%d,%d\n",bestc[i].center.x,bestc[i].center.y);
|
||
165 | 256 | chihsiuh | //cvCircle(image04,bestc[i].center, 20, CV_RGB(0,0,0),5,8,0);
|
166 | 250 | chihsiuh | } |
167 | } |
||
168 | // Show image. HighGUI use.
|
||
169 | 256 | chihsiuh | //cvShowImage( "Result", image04 );
|
170 | 250 | chihsiuh | } |
171 | |||
172 | // Define trackbar callback functon. This function find contours,
|
||
173 | // draw it and approximate it by ellipses.
|
||
174 | void process_image(int h) |
||
175 | { |
||
176 | CvMemStorage* stor; |
||
177 | CvSeq* cont; |
||
178 | CvBox2D32f* box; |
||
179 | CvPoint* PointArray; |
||
180 | CvPoint2D32f* PointArray2D32f; |
||
181 | |||
182 | // Create dynamic structure and sequence.
|
||
183 | stor = cvCreateMemStorage(0);
|
||
184 | cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor); |
||
185 | |||
186 | // Threshold the source image. This needful for cvFindContours().
|
||
187 | cvThreshold( image03, image02, slider_pos, 255, CV_THRESH_BINARY );
|
||
188 | |||
189 | // Find all contours.
|
||
190 | cvFindContours( image02, stor, &cont, sizeof(CvContour),
|
||
191 | CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0)); |
||
192 | |||
193 | // Clear images. IPL use.
|
||
194 | cvZero(image02); |
||
195 | cvZero(image04); |
||
196 | |||
197 | // This cycle draw all contours and approximate it by ellipses.
|
||
198 | for(;cont;cont = cont->h_next)
|
||
199 | { |
||
200 | int i; // Indicator of cycle. |
||
201 | int count = cont->total; // This is number point in contour |
||
202 | CvPoint center; |
||
203 | CvSize size; |
||
204 | |||
205 | // Number point must be more than or equal to 6 (for cvFitEllipse_32f).
|
||
206 | if( count < 6 ) |
||
207 | continue;
|
||
208 | |||
209 | // Alloc memory for contour point set.
|
||
210 | PointArray = (CvPoint*)malloc( count*sizeof(CvPoint) );
|
||
211 | PointArray2D32f= (CvPoint2D32f*)malloc( count*sizeof(CvPoint2D32f) );
|
||
212 | |||
213 | // Alloc memory for ellipse data.
|
||
214 | box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
|
||
215 | |||
216 | // Get contour point set.
|
||
217 | cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); |
||
218 | |||
219 | // Convert CvPoint set to CvBox2D32f set.
|
||
220 | for(i=0; i<count; i++) |
||
221 | { |
||
222 | PointArray2D32f[i].x = (float)PointArray[i].x;
|
||
223 | PointArray2D32f[i].y = (float)PointArray[i].y;
|
||
224 | } |
||
225 | |||
226 | // Fits ellipse to current contour.
|
||
227 | cvFitEllipse(PointArray2D32f, count, box); |
||
228 | |||
229 | // Draw current contour.
|
||
230 | cvDrawContours(image04,cont,CV_RGB(255,255,255),CV_RGB(255,255,255),0,1,8,cvPoint(0,0)); |
||
231 | |||
232 | // Convert ellipse data from float to integer representation.
|
||
233 | center.x = cvRound(box->center.x); |
||
234 | center.y = cvRound(box->center.y); |
||
235 | size.width = cvRound(box->size.width*0.5); |
||
236 | size.height = cvRound(box->size.height*0.5); |
||
237 | box->angle = -box->angle; |
||
238 | |||
239 | if (size.width>10&&size.height>10&&size.width<20&&size.height<20){ |
||
240 | printf("%d %d %d %d\n",center.x,center.y,size.width,size.height);
|
||
241 | |||
242 | // Draw ellipse.
|
||
243 | cvEllipse(image04, center, size, |
||
244 | box->angle, 0, 360, |
||
245 | CV_RGB(0,0,255), 1, CV_AA, 0); |
||
246 | } |
||
247 | // Free memory.
|
||
248 | free(PointArray); |
||
249 | free(PointArray2D32f); |
||
250 | free(box); |
||
251 | } |
||
252 | |||
253 | // Show image. HighGUI use.
|
||
254 | cvShowImage( "Result", image04 );
|
||
255 | } |
||
256 | |||
257 | #ifdef _EiC
|
||
258 | main(1,"vision.c"); |
||
259 | #endif
|