Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / vision / vision.c @ 256

History | View | Annotate | Download (7.44 KB)

1
/**
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
            cvNamedWindow("Result", 1);
53
        // Create toolbars. HighGUI use.
54
            cvCreateTrackbar( "Threshold", "Result", &slider_pos, 255, process_image );
55
        }
56

    
57
        if (!DEBUG) loop_image();
58
        if (DEBUG) process_image(0);
59

    
60
    // Wait for a key stroke; the same function arranges events processing
61
    if (DEBUG) cvWaitKey(0);
62
    cvReleaseImage(&image02);
63
    cvReleaseImage(&image03);
64

    
65
            if (DEBUG){
66
                    cvDestroyWindow("Source");
67
                    cvDestroyWindow("Result");
68
        }
69

    
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
                        //cvCircle(image04,bestc[i].center, 20, CV_RGB(0,0,0),5,8,0);
166
                }
167
        }
168
    // Show image. HighGUI use.
169
    //cvShowImage( "Result", image04 );
170
}
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
260