root / trunk / code / projects / colonet / vision / vision.c @ 476
History | View | Annotate | Download (4.44 KB)
1 | 250 | chihsiuh | /**
|
---|---|---|---|
2 | 286 | chihsiuh | * Robot Detection
|
3 | 250 | chihsiuh | * based on opencv's sample program fitellipse.c by Denis Burenkov
|
4 | *
|
||
5 | * @author Rich Hong
|
||
6 | 286 | chihsiuh | * @date 11/18/2007
|
7 | 250 | chihsiuh | */
|
8 | |||
9 | 431 | emarinel | #include <vision.h> |
10 | |||
11 | #include <cv.h> |
||
12 | #include <highgui.h> |
||
13 | |||
14 | 250 | chihsiuh | #include <stdio.h> |
15 | #include <stdlib.h> |
||
16 | |||
17 | #define MINH 100 //min threshold level |
||
18 | 465 | jknichel | #define MAXH 220 //max threshold level |
19 | 250 | chihsiuh | #define DEBUG 0 //Debug to find threshold level |
20 | |||
21 | struct CenterP {
|
||
22 | 430 | emarinel | CvPoint center; |
23 | int count;
|
||
24 | 250 | chihsiuh | }; |
25 | |||
26 | 431 | emarinel | static IplImage *image02, *image03, *image04;
|
27 | 467 | emarinel | static char* filename; |
28 | 250 | chihsiuh | |
29 | 467 | emarinel | int vision_init(char* filename_) { |
30 | filename = filename_; |
||
31 | return 0; |
||
32 | } |
||
33 | |||
34 | int vision_get_robot_positions(VisionPosition** positions) {
|
||
35 | CvMemStorage* stor; |
||
36 | CvSeq* cont; |
||
37 | CvBox2D32f* box; |
||
38 | CvPoint* PointArray; |
||
39 | CvPoint2D32f* PointArray2D32f; |
||
40 | |||
41 | 431 | emarinel | if ((image03 = cvLoadImage(filename, 0)) == 0) { |
42 | fprintf(stderr, "Failed to load image.\n");
|
||
43 | return -1; |
||
44 | } |
||
45 | |||
46 | 430 | emarinel | // load image and force it to be grayscale
|
47 | if ((image03 = cvLoadImage(filename, 0)) == 0) { |
||
48 | fprintf(stderr, "Failed to load image.\n");
|
||
49 | return -1; |
||
50 | } |
||
51 | 250 | chihsiuh | |
52 | 430 | emarinel | // Create the destination images
|
53 | 431 | emarinel | image02 = cvCloneImage(image03); |
54 | image04 = cvCloneImage(image03); |
||
55 | 286 | chihsiuh | |
56 | 430 | emarinel | if (DEBUG) cvNamedWindow("Result", 1); |
57 | 250 | chihsiuh | |
58 | 430 | emarinel | // Create dynamic structure and sequence.
|
59 | stor = cvCreateMemStorage(0);
|
||
60 | cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor); |
||
61 | 250 | chihsiuh | |
62 | 430 | emarinel | struct CenterP bestc[100]; |
63 | int index=0; |
||
64 | |||
65 | int h;
|
||
66 | for (h = MINH; h < MAXH; h++) {
|
||
67 | // Threshold the source image. This needful for cvFindContours().
|
||
68 | cvThreshold(image03, image02, h, 255, CV_THRESH_BINARY);
|
||
69 | |||
70 | // Find all contours.
|
||
71 | cvFindContours( image02, stor, &cont, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0)); |
||
72 | |||
73 | // Clear images. IPL use.
|
||
74 | cvZero(image02); |
||
75 | cvZero(image04); |
||
76 | |||
77 | // This cycle draw all contours and approximate it by ellipses.
|
||
78 | for(; cont; cont = cont->h_next) {
|
||
79 | int i; // Indicator of cycle. |
||
80 | int count = cont->total; // This is number point in contour |
||
81 | CvPoint center; |
||
82 | CvSize size; |
||
83 | |||
84 | // Number point must be more than or equal to 6 (for cvFitEllipse_32f).
|
||
85 | if (count < 6) continue; |
||
86 | |||
87 | // Alloc memory for contour point set.
|
||
88 | PointArray = (CvPoint*) malloc(count * sizeof(CvPoint));
|
||
89 | PointArray2D32f= (CvPoint2D32f*) malloc(count * sizeof(CvPoint2D32f));
|
||
90 | |||
91 | // Alloc memory for ellipse data.
|
||
92 | box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
|
||
93 | |||
94 | // Get contour point set.
|
||
95 | cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ); |
||
96 | |||
97 | // Convert CvPoint set to CvBox2D32f set.
|
||
98 | for(i=0; i<count; i++) { |
||
99 | PointArray2D32f[i].x = (float)PointArray[i].x;
|
||
100 | PointArray2D32f[i].y = (float)PointArray[i].y;
|
||
101 | } |
||
102 | |||
103 | // Fits ellipse to current contour.
|
||
104 | cvFitEllipse(PointArray2D32f, count, box); |
||
105 | |||
106 | // Convert ellipse data from float to integer representation.
|
||
107 | center.x = cvRound(box->center.x); |
||
108 | center.y = cvRound(box->center.y); |
||
109 | size.width = cvRound(box->size.width*0.5); |
||
110 | size.height = cvRound(box->size.height*0.5); |
||
111 | box->angle = -box->angle; |
||
112 | |||
113 | if (size.width>10&&size.height>10&&size.width<20&&size.height<20){ |
||
114 | //printf("%d %d %d %d\n",center.x,center.y,size.width,size.height);
|
||
115 | |||
116 | 431 | emarinel | int found=0, j; |
117 | for (j = 0; j < index; j++) { |
||
118 | if (abs(bestc[j].center.x-center.x)<9&&abs(bestc[j].center.y-center.y)<9) { |
||
119 | 430 | emarinel | bestc[j].count++; |
120 | found=1;
|
||
121 | break;
|
||
122 | } |
||
123 | 466 | emarinel | } |
124 | 431 | emarinel | |
125 | 430 | emarinel | if (!found){
|
126 | struct CenterP c;
|
||
127 | c.center=center; |
||
128 | c.count=1;
|
||
129 | bestc[index]=c; |
||
130 | index++; |
||
131 | } |
||
132 | } |
||
133 | 431 | emarinel | |
134 | 430 | emarinel | // Free memory.
|
135 | free(PointArray); |
||
136 | free(PointArray2D32f); |
||
137 | free(box); |
||
138 | } |
||
139 | } |
||
140 | |||
141 | 431 | emarinel | image04 = cvCloneImage(image03); |
142 | |||
143 | int count = 0; |
||
144 | 430 | emarinel | int i;
|
145 | 431 | emarinel | for (i = 0; i < index; i++) { |
146 | if (bestc[i].count > 7){ |
||
147 | count++; |
||
148 | } |
||
149 | } |
||
150 | |||
151 | 466 | emarinel | VisionPosition* pos_array = (VisionPosition*)malloc(sizeof(VisionPosition) * count);
|
152 | if (pos_array == NULL) { |
||
153 | fprintf(stderr, "malloc failed\n");
|
||
154 | return -1; |
||
155 | } |
||
156 | |||
157 | int c = 0; |
||
158 | 431 | emarinel | for (i = 0; i < index; i++) { |
159 | if (bestc[i].count > 7){ |
||
160 | 466 | emarinel | pos_array[c].x = bestc[i].center.x; |
161 | pos_array[c].y = bestc[i].center.y; |
||
162 | c++; |
||
163 | 431 | emarinel | |
164 | 466 | emarinel | if (DEBUG) cvCircle(image04, bestc[i].center, 20, CV_RGB(0,0,0), 5, 8, 0); |
165 | 430 | emarinel | } |
166 | } |
||
167 | |||
168 | 467 | emarinel | if (DEBUG) cvWaitKey(0); |
169 | |||
170 | cvReleaseImage(&image02); |
||
171 | cvReleaseImage(&image03); |
||
172 | |||
173 | if (DEBUG) cvDestroyWindow("Result"); |
||
174 | |||
175 | 430 | emarinel | // Show image. HighGUI use.
|
176 | if (DEBUG) cvShowImage( "Result", image04 ); |
||
177 | 431 | emarinel | |
178 | 466 | emarinel | *positions = pos_array; |
179 | 431 | emarinel | return count;
|
180 | 250 | chihsiuh | } |