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