root / vision / src / v2v3_converter.cpp @ 14c71a7d
History | View | Annotate | Download (5.91 KB)
1 | 14c71a7d | Nick Stanley | #include <ros/ros.h> |
---|---|---|---|
2 | f3966b0d | Nick Stanley | #include <stdio.h> |
3 | 80eadc1e | Tom Mullins | #include <vision/TargetDescriptor.h> |
4 | 06738945 | Nick Stanley | #include <image_transport/image_transport.h> |
5 | 80eadc1e | Tom Mullins | #include <geometry_msgs/Point.h> |
6 | #include <tf/transform_listener.h> |
||
7 | |||
8 | 91a0bfa9 | Nick Stanley | ros::Publisher pub; // publishes Point
|
9 | ros::Publisher img_pub; //publishes Image with only blob showing
|
||
10 | 80eadc1e | Tom Mullins | |
11 | 14c71a7d | Nick Stanley | int hk = 216; // target hsv values |
12 | 880aeb43 | Nick Stanley | int sk = 255; |
13 | int vk = 255; |
||
14 | |||
15 | 91a0bfa9 | Nick Stanley | /* Given index p and image pointer img,
|
16 | * determines whether or not the pixel
|
||
17 | * in the indicated vicinity is equal to it.
|
||
18 | */
|
||
19 | 06738945 | Nick Stanley | bool isRight(sensor_msgs::Image * img, int p){ |
20 | 880aeb43 | Nick Stanley | return (img->data[p + 1] == img->data[p]); |
21 | } |
||
22 | |||
23 | 06738945 | Nick Stanley | bool isLeft(sensor_msgs::Image * img, int p){ |
24 | 880aeb43 | Nick Stanley | return (img->data[p - 1] == img->data[p]); |
25 | } |
||
26 | |||
27 | 06738945 | Nick Stanley | bool isUp(sensor_msgs::Image * img, int p){ |
28 | return (img->data[p - img->width] == img->data[p]);
|
||
29 | 880aeb43 | Nick Stanley | } |
30 | |||
31 | 06738945 | Nick Stanley | bool isDown(sensor_msgs::Image * img, int p){ |
32 | return (img->data[p + img->width] == img->data[p]);
|
||
33 | 880aeb43 | Nick Stanley | } |
34 | |||
35 | 14c71a7d | Nick Stanley | int min(int a, int b){ |
36 | if (a <= b) return a; |
||
37 | else return b; |
||
38 | } |
||
39 | |||
40 | int max(int a, int b){ |
||
41 | if (a >= b) return a; |
||
42 | else return b; |
||
43 | } |
||
44 | |||
45 | 91a0bfa9 | Nick Stanley | /* Checks for any adjacent pixel, ensuring a continuous blob.
|
46 | */
|
||
47 | 06738945 | Nick Stanley | bool hasAnAdjacent(sensor_msgs::Image * image, int j){ |
48 | 880aeb43 | Nick Stanley | int nl = image->height; // number of lines |
49 | 06738945 | Nick Stanley | int nc = image->width;
|
50 | f3966b0d | Nick Stanley | int nChannels = image->step / image->width;
|
51 | 06738945 | Nick Stanley | int pos = j / nChannels;
|
52 | 880aeb43 | Nick Stanley | if ((pos % nc) != 0){ |
53 | 06738945 | Nick Stanley | if (isLeft(image, pos)) return true; |
54 | 880aeb43 | Nick Stanley | } |
55 | if ((pos / nc) != 0){ |
||
56 | 06738945 | Nick Stanley | if (isUp(image, pos)) return true; |
57 | 880aeb43 | Nick Stanley | } |
58 | if ((pos % nc) != nc - 1){ |
||
59 | 06738945 | Nick Stanley | if (isRight(image, pos)) return true; |
60 | 880aeb43 | Nick Stanley | } |
61 | if ((pos / nc) != nc - 1){ |
||
62 | 06738945 | Nick Stanley | if (isDown(image, pos)) return true; |
63 | 880aeb43 | Nick Stanley | } |
64 | else return false; |
||
65 | } |
||
66 | |||
67 | 13b764cd | Tom Mullins | /* Detects the blobs in an image within above defined constraints. Transforms image into 255
|
68 | 880aeb43 | Nick Stanley | * for pixels in the blob, 0 for not in the blob.
|
69 | */
|
||
70 | 06738945 | Nick Stanley | void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){ |
71 | 880aeb43 | Nick Stanley | int nl = image->height; // number of lines |
72 | 06738945 | Nick Stanley | int nc = image->step; // number of columns |
73 | 91a0bfa9 | Nick Stanley | int nChannels = image->step / image->width; // effective width of each pixel |
74 | 880aeb43 | Nick Stanley | |
75 | // process image
|
||
76 | f3966b0d | Nick Stanley | for (int i = 0; i < nl; i++){ |
77 | 06738945 | Nick Stanley | for (int j = 0; j < nc; j+= nChannels){ |
78 | 91a0bfa9 | Nick Stanley | // if within bounds...
|
79 | if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){ |
||
80 | image->data[i * image->width + j] = 255; // set each to 255 |
||
81 | 13b764cd | Tom Mullins | image->data[i * image->width + j+1] = 255; |
82 | image->data[i * image->width + j+2] = 255; |
||
83 | 880aeb43 | Nick Stanley | } |
84 | 91a0bfa9 | Nick Stanley | // otherwise set to 0
|
85 | 880aeb43 | Nick Stanley | else {
|
86 | 06738945 | Nick Stanley | image->data[i * image->width + j] = 0;
|
87 | image->data[i * image->width + j+1] = 0; |
||
88 | image->data[i * image->width + j+2] = 0; |
||
89 | 880aeb43 | Nick Stanley | } |
90 | } |
||
91 | } |
||
92 | 13b764cd | Tom Mullins | |
93 | 91a0bfa9 | Nick Stanley | |
94 | // The following is to find the average
|
||
95 | int comXSum = 0; |
||
96 | int comYSum = 0; |
||
97 | int blobPixelCount = 0; |
||
98 | 06738945 | Nick Stanley | for (int i = 0; i < image->width * image->height; i+=nChannels){ |
99 | 91a0bfa9 | Nick Stanley | // if the pixel is not both the right hue and has adjacents, set to 0
|
100 | 13b764cd | Tom Mullins | if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){ |
101 | 06738945 | Nick Stanley | image->data[i] = 0;
|
102 | image->data[i + 1] = 0; |
||
103 | image->data[i + 2] = 0; |
||
104 | 880aeb43 | Nick Stanley | } |
105 | 91a0bfa9 | Nick Stanley | else {
|
106 | blobPixelCount++; |
||
107 | comXSum += image->data[i] / 255;
|
||
108 | comYSum += image->data[i] / 255;
|
||
109 | } |
||
110 | 880aeb43 | Nick Stanley | } |
111 | 13b764cd | Tom Mullins | if (blobPixelCount != 0){ |
112 | f3966b0d | Nick Stanley | // get average
|
113 | 13b764cd | Tom Mullins | comXSum /= blobPixelCount; |
114 | comYSum /= blobPixelCount; |
||
115 | } |
||
116 | 91a0bfa9 | Nick Stanley | // output results
|
117 | 880aeb43 | Nick Stanley | *area = blobPixelCount; |
118 | 91a0bfa9 | Nick Stanley | *comX = comXSum; |
119 | *comY = comYSum; |
||
120 | 13b764cd | Tom Mullins | |
121 | 91a0bfa9 | Nick Stanley | img_pub.publish(*image); // publish new image of only black and white
|
122 | 880aeb43 | Nick Stanley | } |
123 | |||
124 | 13b764cd | Tom Mullins | void convertToHSV(sensor_msgs::Image * orig){
|
125 | f3966b0d | Nick Stanley | |
126 | int nChannels = orig->step / orig->width;
|
||
127 | printf("Converting to HSV \n");
|
||
128 | 06738945 | Nick Stanley | |
129 | // get the max, set it as v
|
||
130 | |||
131 | for (int i = 0; i < orig->height * orig->width; i += nChannels){ |
||
132 | int h;
|
||
133 | int s;
|
||
134 | int v;
|
||
135 | 14c71a7d | Nick Stanley | int low;
|
136 | 06738945 | Nick Stanley | |
137 | // get min, max
|
||
138 | 91a0bfa9 | Nick Stanley | v = max(max(orig->data[i], orig->data[i+1]), orig->data[i+2]); |
139 | 14c71a7d | Nick Stanley | low = min(min(orig->data[i], orig->data[i+1]), orig->data[i+2]); |
140 | 06738945 | Nick Stanley | |
141 | // set s
|
||
142 | if (v != 0){ |
||
143 | 14c71a7d | Nick Stanley | s = (v - low) / v; |
144 | 06738945 | Nick Stanley | } |
145 | else s = 0; |
||
146 | |||
147 | // set h
|
||
148 | if (s == 0) h = 0; |
||
149 | else if (v == orig->data[i]){ |
||
150 | h = 60 * (orig->data[i+1] - orig->data[i+2]) / s; |
||
151 | } |
||
152 | else if (v == orig->data[i+1]){ |
||
153 | h = 120 + 60 * (orig->data[i+2] - orig->data[i]) / s; |
||
154 | } |
||
155 | else {
|
||
156 | h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s; |
||
157 | } |
||
158 | |||
159 | 91a0bfa9 | Nick Stanley | // set values
|
160 | 13b764cd | Tom Mullins | orig->data[i] = h; |
161 | orig->data[i+1] = s;
|
||
162 | orig->data[i+2] = v;
|
||
163 | 06738945 | Nick Stanley | |
164 | } |
||
165 | f3966b0d | Nick Stanley | printf("Conversion complete\n");
|
166 | 06738945 | Nick Stanley | } |
167 | |||
168 | void target_cb(const sensor_msgs::Image orig) { |
||
169 | f3966b0d | Nick Stanley | |
170 | printf("beginning callback \n");
|
||
171 | 05e1c990 | Tom Mullins | tf::TransformListener listener; |
172 | 80eadc1e | Tom Mullins | geometry_msgs::Point point; |
173 | tf::StampedTransform transform; |
||
174 | 13b764cd | Tom Mullins | /*try {
|
175 | 80eadc1e | Tom Mullins | listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);
|
176 | }
|
||
177 | catch (tf::TransformException ex) {
|
||
178 | ROS_ERROR("%s", ex.what());
|
||
179 | 13b764cd | Tom Mullins | }*/
|
180 | 880aeb43 | Nick Stanley | |
181 | 91a0bfa9 | Nick Stanley | // image types
|
182 | 880aeb43 | Nick Stanley | |
183 | 06738945 | Nick Stanley | sensor_msgs::Image copy = orig; |
184 | sensor_msgs::Image * source = © // pointer to the image
|
||
185 | 13b764cd | Tom Mullins | sensor_msgs::Image hsvImage = orig; // May be time
|
186 | 06738945 | Nick Stanley | sensor_msgs::Image * hsvPtr = &hsvImage; |
187 | 880aeb43 | Nick Stanley | |
188 | // convert to HSV
|
||
189 | 06738945 | Nick Stanley | |
190 | 13b764cd | Tom Mullins | convertToHSV(source); |
191 | 880aeb43 | Nick Stanley | |
192 | // detect blob
|
||
193 | |||
194 | int comX;
|
||
195 | int comY;
|
||
196 | int area;
|
||
197 | |||
198 | 06738945 | Nick Stanley | detectBlobs(hsvPtr, &comX, &comY, &area); |
199 | 880aeb43 | Nick Stanley | |
200 | f3966b0d | Nick Stanley | int distance;
|
201 | |||
202 | 880aeb43 | Nick Stanley | // give distance as inversely proportional to area, for (x,y,z)
|
203 | 91a0bfa9 | Nick Stanley | if (area > 1){ |
204 | 13b764cd | Tom Mullins | |
205 | f3966b0d | Nick Stanley | distance = 100.0 / (float) area; |
206 | |||
207 | } |
||
208 | else {
|
||
209 | distance = -1;
|
||
210 | } |
||
211 | |||
212 | // fill point based on target and tf
|
||
213 | 880aeb43 | Nick Stanley | |
214 | f3966b0d | Nick Stanley | geometry_msgs::Point origPoint; |
215 | 880aeb43 | Nick Stanley | |
216 | f3966b0d | Nick Stanley | // TODO: Change coordinate axes
|
217 | 880aeb43 | Nick Stanley | |
218 | f3966b0d | Nick Stanley | origPoint.x = comX; |
219 | origPoint.y = comY; |
||
220 | origPoint.z = distance; |
||
221 | 880aeb43 | Nick Stanley | |
222 | f3966b0d | Nick Stanley | // TODO transform into point
|
223 | 06738945 | Nick Stanley | |
224 | f3966b0d | Nick Stanley | // for now just publish that
|
225 | |||
226 | pub.publish(origPoint); |
||
227 | 880aeb43 | Nick Stanley | |
228 | f3966b0d | Nick Stanley | printf("Ending callback");
|
229 | |||
230 | } |
||
231 | |||
232 | void target_cb_test(const sensor_msgs::Image image){ |
||
233 | |||
234 | geometry_msgs::Point point; |
||
235 | point.x = 1;
|
||
236 | point.y = 2;
|
||
237 | point.z = 3;
|
||
238 | pub.publish(point); |
||
239 | 80eadc1e | Tom Mullins | } |
240 | |||
241 | int main(int argc, char **argv) { |
||
242 | ros::init(argc, argv, "v2v3_converter");
|
||
243 | ros::NodeHandle n; |
||
244 | f3966b0d | Nick Stanley | pub = n.advertise<geometry_msgs::Point>("vision/target_3d", 1000); |
245 | img_pub = n.advertise<sensor_msgs::Image>("vision/blob_image", 1000); |
||
246 | 13b764cd | Tom Mullins | ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb); |
247 | 80eadc1e | Tom Mullins | ros::spin(); |
248 | 05e1c990 | Tom Mullins | return 0; |
249 | 80eadc1e | Tom Mullins | } |