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