Revision 91a0bfa9
ID | 91a0bfa9fd4c7c4b078b623fab8a9da5c7558195 |
Commented code!
vision/src/v2v3_converter.cpp | ||
---|---|---|
1 |
#include <ros/ros.h>
|
|
1 |
#nclude <ros/ros.h> |
|
2 | 2 |
#include <stdio.h> |
3 | 3 |
#include <vision/TargetDescriptor.h> |
4 | 4 |
#include <image_transport/image_transport.h> |
5 | 5 |
#include <geometry_msgs/Point.h> |
6 | 6 |
#include <tf/transform_listener.h> |
7 | 7 |
|
8 |
ros::Publisher pub; |
|
9 |
ros::Publisher img_pub; |
|
8 |
ros::Publisher pub; // publishes Point
|
|
9 |
ros::Publisher img_pub; //publishes Image with only blob showing
|
|
10 | 10 |
|
11 |
int hk = 15; |
|
11 |
int hk = 15; // target hsv values
|
|
12 | 12 |
int sk = 255; |
13 | 13 |
int vk = 255; |
14 | 14 |
|
15 |
/* Given index p and image pointer img, |
|
16 |
* determines whether or not the pixel |
|
17 |
* in the indicated vicinity is equal to it. |
|
18 |
*/ |
|
15 | 19 |
bool isRight(sensor_msgs::Image * img, int p){ |
16 | 20 |
return (img->data[p + 1] == img->data[p]); |
17 | 21 |
} |
... | ... | |
28 | 32 |
return (img->data[p + img->width] == img->data[p]); |
29 | 33 |
} |
30 | 34 |
|
35 |
/* Checks for any adjacent pixel, ensuring a continuous blob. |
|
36 |
*/ |
|
31 | 37 |
bool hasAnAdjacent(sensor_msgs::Image * image, int j){ |
32 | 38 |
int nl = image->height; // number of lines |
33 | 39 |
int nc = image->width; |
... | ... | |
54 | 60 |
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){ |
55 | 61 |
int nl = image->height; // number of lines |
56 | 62 |
int nc = image->step; // number of columns |
57 |
int nChannels = image->step / image->width; // effective width |
|
63 |
int nChannels = image->step / image->width; // effective width of each pixel
|
|
58 | 64 |
|
59 | 65 |
// process image |
60 | 66 |
for (int i = 0; i < nl; i++){ |
61 | 67 |
for (int j = 0; j < nc; j+= nChannels){ |
62 |
if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){ |
|
63 |
image->data[i * image->width + j] = 255; |
|
68 |
// if within bounds... |
|
69 |
if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){ |
|
70 |
image->data[i * image->width + j] = 255; // set each to 255 |
|
64 | 71 |
image->data[i * image->width + j+1] = 255; |
65 | 72 |
image->data[i * image->width + j+2] = 255; |
66 | 73 |
} |
74 |
// otherwise set to 0 |
|
67 | 75 |
else { |
68 | 76 |
image->data[i * image->width + j] = 0; |
69 | 77 |
image->data[i * image->width + j+1] = 0; |
... | ... | |
72 | 80 |
} |
73 | 81 |
} |
74 | 82 |
|
75 |
int comXSum = 0; |
|
76 |
int comYSum = 0; |
|
77 |
int blobPixelCount = 0; |
|
83 |
|
|
84 |
// The following is to find the average |
|
85 |
int comXSum = 0; |
|
86 |
int comYSum = 0; |
|
87 |
int blobPixelCount = 0; |
|
78 | 88 |
for (int i = 0; i < image->width * image->height; i+=nChannels){ |
89 |
// if the pixel is not both the right hue and has adjacents, set to 0 |
|
79 | 90 |
if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){ |
80 | 91 |
image->data[i] = 0; |
81 | 92 |
image->data[i + 1] = 0; |
82 | 93 |
image->data[i + 2] = 0; |
83 |
continue; |
|
84 | 94 |
} |
85 |
blobPixelCount++; |
|
86 |
comXSum += image->data[i] / 255; |
|
87 |
comYSum += image->data[i] / 255; |
|
95 |
else { |
|
96 |
blobPixelCount++; |
|
97 |
comXSum += image->data[i] / 255; |
|
98 |
comYSum += image->data[i] / 255; |
|
99 |
} |
|
88 | 100 |
} |
89 | 101 |
if (blobPixelCount != 0){ |
90 | 102 |
// get average |
91 | 103 |
comXSum /= blobPixelCount; |
92 | 104 |
comYSum /= blobPixelCount; |
93 | 105 |
} |
94 |
|
|
106 |
// output results |
|
95 | 107 |
*area = blobPixelCount; |
96 |
*comX = comXSum;
|
|
97 |
*comY = comYSum;
|
|
108 |
*comX = comXSum;
|
|
109 |
*comY = comYSum;
|
|
98 | 110 |
|
99 |
img_pub.publish(*image);
|
|
111 |
img_pub.publish(*image); // publish new image of only black and white
|
|
100 | 112 |
} |
101 | 113 |
|
102 | 114 |
void convertToHSV(sensor_msgs::Image * orig){ |
... | ... | |
113 | 125 |
int min; |
114 | 126 |
|
115 | 127 |
// 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 |
} |
|
128 |
v = max(max(orig->data[i], orig->data[i+1]), orig->data[i+2]); |
|
129 |
min = min(min(orig->data[i], orig->data[i+1]), orig->data[i+2]); |
|
137 | 130 |
|
138 | 131 |
// set s |
139 | 132 |
if (v != 0){ |
... | ... | |
153 | 146 |
h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s; |
154 | 147 |
} |
155 | 148 |
|
149 |
// set values |
|
156 | 150 |
orig->data[i] = h; |
157 | 151 |
orig->data[i+1] = s; |
158 | 152 |
orig->data[i+2] = v; |
... | ... | |
174 | 168 |
ROS_ERROR("%s", ex.what()); |
175 | 169 |
}*/ |
176 | 170 |
|
177 |
// convert to OpenCV
|
|
171 |
// image types
|
|
178 | 172 |
|
179 | 173 |
sensor_msgs::Image copy = orig; |
180 | 174 |
sensor_msgs::Image * source = © // pointer to the image |
... | ... | |
196 | 190 |
int distance; |
197 | 191 |
|
198 | 192 |
// give distance as inversely proportional to area, for (x,y,z) |
199 |
if (area > 10){
|
|
193 |
if (area > 1){ |
|
200 | 194 |
|
201 | 195 |
distance = 100.0 / (float) area; |
202 | 196 |
|
Also available in: Unified diff