root / trunk / code / projects / colonet / server / vision / vision.c @ 1428
History | View | Annotate | Download (3.11 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 | 938 | rcahoon | * @author Ryan Cahoon
|
7 | 286 | chihsiuh | * @date 11/18/2007
|
8 | 250 | chihsiuh | */
|
9 | 938 | rcahoon | |
10 | //TODO: Add camera stabilization
|
||
11 | 250 | chihsiuh | |
12 | 936 | rcahoon | #include "vision.h" |
13 | 431 | emarinel | |
14 | #include <cv.h> |
||
15 | #include <highgui.h> |
||
16 | |||
17 | 250 | chihsiuh | #include <stdio.h> |
18 | #include <stdlib.h> |
||
19 | |||
20 | 939 | rcahoon | #include "shmimgserve.h" |
21 | |||
22 | 937 | rcahoon | #define MIN_PADDING 70 |
23 | 534 | jknichel | |
24 | 939 | rcahoon | #define DEBUG 0 //Debug to find threshold level |
25 | 250 | chihsiuh | |
26 | struct CenterP {
|
||
27 | 430 | emarinel | CvPoint center; |
28 | int count;
|
||
29 | 250 | chihsiuh | }; |
30 | |||
31 | 936 | rcahoon | /*static char* filename;*/
|
32 | 250 | chihsiuh | |
33 | 936 | rcahoon | static CvCapture *capture;
|
34 | |||
35 | int vision_init(/*char* filename_*/) { |
||
36 | /*filename = filename_;*/
|
||
37 | |||
38 | 938 | rcahoon | printf("Vision init. %s\n", DEBUG ? "Debugging on" : ""); |
39 | |||
40 | capture = cvCaptureFromCAM(0);
|
||
41 | 936 | rcahoon | if( !capture ) {
|
42 | fprintf( stderr, "ERROR: capture is NULL \n" );
|
||
43 | return -1; |
||
44 | } |
||
45 | 939 | rcahoon | |
46 | openShM(); |
||
47 | 937 | rcahoon | |
48 | if (DEBUG)
|
||
49 | 938 | rcahoon | { |
50 | cvNamedWindow("mywindow1", CV_WINDOW_AUTOSIZE);
|
||
51 | } |
||
52 | 936 | rcahoon | |
53 | 467 | emarinel | return 0; |
54 | } |
||
55 | |||
56 | 936 | rcahoon | void vision_close()
|
57 | { |
||
58 | if (DEBUG)
|
||
59 | 938 | rcahoon | cvDestroyWindow("mywindow1");
|
60 | 936 | rcahoon | |
61 | 939 | rcahoon | releaseShM(); |
62 | |||
63 | 936 | rcahoon | cvReleaseCapture(&capture); |
64 | 938 | rcahoon | |
65 | printf("Vision closed.\n");
|
||
66 | 936 | rcahoon | } |
67 | |||
68 | 467 | emarinel | int vision_get_robot_positions(VisionPosition** positions) {
|
69 | 937 | rcahoon | IplImage *image03; |
70 | |||
71 | 936 | rcahoon | if(!cvGrabFrame(capture)){ // capture a frame |
72 | 939 | rcahoon | printf("Could not grab a frame\n");
|
73 | 937 | rcahoon | return -1; |
74 | 936 | rcahoon | } |
75 | 937 | rcahoon | IplImage *im_capture=cvRetrieveFrame(capture); // retrieve the captured frame
|
76 | 939 | rcahoon | |
77 | sendImage(im_capture); |
||
78 | |||
79 | 936 | rcahoon | image03 = cvCreateImage(cvGetSize(im_capture), im_capture->depth, 1);
|
80 | cvCvtColor(im_capture, image03, CV_RGB2GRAY); |
||
81 | 467 | emarinel | |
82 | 937 | rcahoon | struct CenterP centers[100] = {0}; |
83 | int count = 0; |
||
84 | 250 | chihsiuh | |
85 | 937 | rcahoon | cvSmooth( image03, image03, CV_GAUSSIAN, 9, 9 ); // smooth it, otherwise a lot of false circles may be detected |
86 | CvMemStorage* storage = cvCreateMemStorage(0);
|
||
87 | 1428 | rcahoon | CvSeq* circles = cvHoughCircles( image03, storage, CV_HOUGH_GRADIENT, 1, 50, 100, 43 ); |
88 | 286 | chihsiuh | |
89 | 937 | rcahoon | int i;
|
90 | for( i = 0; i < circles->total && count < 100; i++ ) |
||
91 | { |
||
92 | //TODO: Augment with Harr cascade second level detecting orbs + identify robot with orb color
|
||
93 | 250 | chihsiuh | |
94 | 937 | rcahoon | float* p = (float*)cvGetSeqElem( circles, i ); |
95 | CvPoint center = cvPoint(cvRound(p[0]), cvRound(p[1])); |
||
96 | int r = cvRound(p[2]); |
||
97 | 430 | emarinel | |
98 | 939 | rcahoon | if (DEBUG) cvCircle( image03, center, 3, CV_RGB(255,255,255), 3, 8, 0 ); |
99 | 430 | emarinel | |
100 | 938 | rcahoon | int dist2 = MIN_PADDING;
|
101 | 937 | rcahoon | int loc = count;
|
102 | 430 | emarinel | |
103 | 937 | rcahoon | int j;
|
104 | for (j = 0; j < count; j++) |
||
105 | { |
||
106 | int dx = centers[j].center.x-center.x;
|
||
107 | int dy = centers[j].center.y-center.y;
|
||
108 | 938 | rcahoon | int d2 = dx*dx + dy*dy;
|
109 | 430 | emarinel | |
110 | 938 | rcahoon | if (d2 < dist2)
|
111 | 937 | rcahoon | { |
112 | 938 | rcahoon | dist2 = d2; |
113 | 937 | rcahoon | loc = j; |
114 | 430 | emarinel | } |
115 | 937 | rcahoon | } |
116 | 430 | emarinel | |
117 | 937 | rcahoon | int c = centers[loc].count;
|
118 | centers[loc].center.x = (centers[loc].center.x * c + center.x) / (c + 1);
|
||
119 | centers[loc].center.y = (centers[loc].center.y * c + center.y) / (c + 1);
|
||
120 | centers[loc].count++; |
||
121 | if (loc == count) count++;
|
||
122 | } |
||
123 | 430 | emarinel | |
124 | 939 | rcahoon | if (DEBUG) cvShowImage( "mywindow1", image03 ); |
125 | 430 | emarinel | |
126 | 937 | rcahoon | cvReleaseMemStorage(&storage); |
127 | 431 | emarinel | |
128 | 937 | rcahoon | cvReleaseImage(&image03); |
129 | 431 | emarinel | |
130 | 466 | emarinel | VisionPosition* pos_array = (VisionPosition*)malloc(sizeof(VisionPosition) * count);
|
131 | if (pos_array == NULL) { |
||
132 | fprintf(stderr, "malloc failed\n");
|
||
133 | return -1; |
||
134 | } |
||
135 | |||
136 | int c = 0; |
||
137 | 937 | rcahoon | for (i = 0; i < count; i++) { |
138 | pos_array[c].x = centers[i].center.x; |
||
139 | pos_array[c].y = centers[i].center.y; |
||
140 | c++; |
||
141 | 430 | emarinel | } |
142 | |||
143 | 466 | emarinel | *positions = pos_array; |
144 | 431 | emarinel | return count;
|
145 | 250 | chihsiuh | } |