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