Revision 13b764cd
ID | 13b764cd9018697ec9efc216efe51bdc042a1ce2 |
Working more.
vision/src/v2v3_converter.cpp | ||
---|---|---|
5 | 5 |
#include <tf/transform_listener.h> |
6 | 6 |
|
7 | 7 |
ros::Publisher pub; |
8 |
ros::Publisher img_pub; |
|
8 | 9 |
|
9 | 10 |
int hk = 25; |
10 | 11 |
int sk = 255; |
... | ... | |
46 | 47 |
else return false; |
47 | 48 |
} |
48 | 49 |
|
49 |
/* Detects the blobs in an image within above defined constraints. Transforms image into 1
|
|
50 |
/* Detects the blobs in an image within above defined constraints. Transforms image into 255
|
|
50 | 51 |
* for pixels in the blob, 0 for not in the blob. |
51 | 52 |
*/ |
52 | 53 |
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){ |
... | ... | |
58 | 59 |
for (int i = 1; i < nl; i++){ |
59 | 60 |
for (int j = 0; j < nc; j+= nChannels){ |
60 | 61 |
if (image->data[i * image->width + j] < 30 && image->data[j] > 20){ |
61 |
image->data[i * image->width + j] = 1;
|
|
62 |
image->data[i * image->width + j+1] = 1;
|
|
63 |
image->data[i * image->width + j+2] = 1;
|
|
62 |
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;
|
|
64 | 65 |
} |
65 | 66 |
else { |
66 | 67 |
image->data[i * image->width + j] = 0; |
... | ... | |
69 | 70 |
} |
70 | 71 |
} |
71 | 72 |
} |
72 |
|
|
73 |
|
|
73 | 74 |
int comXSum = 0; |
74 | 75 |
int comYSum = 0; |
75 | 76 |
int blobPixelCount = 0; |
76 | 77 |
for (int i = 0; i < image->width * image->height; i+=nChannels){ |
77 |
if ( !(image->data[i] == 1 && hasAnAdjacent(image, i)) ){
|
|
78 |
if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){
|
|
78 | 79 |
image->data[i] = 0; |
79 | 80 |
image->data[i + 1] = 0; |
80 | 81 |
image->data[i + 2] = 0; |
82 |
continue; |
|
81 | 83 |
} |
82 | 84 |
blobPixelCount++; |
83 |
comXSum += image->data[i] % nc + 1;
|
|
84 |
comYSum += image->data[i] / nc + 1;
|
|
85 |
comXSum += image->data[i] / 255;
|
|
86 |
comYSum += image->data[i] / 255;
|
|
85 | 87 |
} |
86 |
comXSum /= blobPixelCount; |
|
87 |
comYSum /= blobPixelCount; |
|
88 |
|
|
88 |
if (blobPixelCount != 0){ |
|
89 |
comXSum /= blobPixelCount; |
|
90 |
comYSum /= blobPixelCount; |
|
91 |
} |
|
92 |
|
|
89 | 93 |
*area = blobPixelCount; |
90 | 94 |
*comX = comXSum; |
91 | 95 |
*comY = comYSum; |
96 |
|
|
97 |
img_pub.publish(*image); |
|
92 | 98 |
} |
93 | 99 |
|
94 |
void convertToHSV(sensor_msgs::Image * orig, sensor_msgs::Image * result){
|
|
100 |
void convertToHSV(sensor_msgs::Image * orig){ |
|
95 | 101 |
int nChannels = orig->width / orig->step; |
96 | 102 |
|
97 | 103 |
// get the max, set it as v |
... | ... | |
143 | 149 |
h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s; |
144 | 150 |
} |
145 | 151 |
|
146 |
result->data[i] = h;
|
|
147 |
result->data[i+1] = s;
|
|
148 |
result->data[i+2] = v;
|
|
152 |
orig->data[i] = h;
|
|
153 |
orig->data[i+1] = s;
|
|
154 |
orig->data[i+2] = v;
|
|
149 | 155 |
|
150 | 156 |
} |
151 | 157 |
|
... | ... | |
155 | 161 |
tf::TransformListener listener; |
156 | 162 |
geometry_msgs::Point point; |
157 | 163 |
tf::StampedTransform transform; |
158 |
try { |
|
164 |
/*try {
|
|
159 | 165 |
listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform); |
160 | 166 |
} |
161 | 167 |
catch (tf::TransformException ex) { |
162 | 168 |
ROS_ERROR("%s", ex.what()); |
163 |
} |
|
169 |
}*/
|
|
164 | 170 |
|
165 | 171 |
// convert to OpenCV |
166 | 172 |
|
167 | 173 |
sensor_msgs::Image copy = orig; |
168 | 174 |
sensor_msgs::Image * source = © // pointer to the image |
169 |
sensor_msgs::Image hsvImage = sensor_msgs::Image();
|
|
175 |
sensor_msgs::Image hsvImage = orig; // May be time
|
|
170 | 176 |
sensor_msgs::Image * hsvPtr = &hsvImage; |
171 | 177 |
|
172 | 178 |
// convert to HSV |
173 | 179 |
|
174 |
convertToHSV(source, hsvPtr);
|
|
180 |
convertToHSV(source); |
|
175 | 181 |
|
176 | 182 |
// detect blob |
177 | 183 |
|
... | ... | |
182 | 188 |
detectBlobs(hsvPtr, &comX, &comY, &area); |
183 | 189 |
|
184 | 190 |
// give distance as inversely proportional to area, for (x,y,z) |
191 |
if (area > 10){ |
|
192 |
|
|
193 |
int distance = 100.0 / (float) area; |
|
185 | 194 |
|
186 |
int distance = 100.0 / (float) area;
|
|
195 |
// fill point based on target and tf
|
|
187 | 196 |
|
188 |
// fill point based on target and tf
|
|
197 |
geometry_msgs::Point origPoint;
|
|
189 | 198 |
|
190 |
geometry_msgs::Point origPoint; |
|
199 |
origPoint.x = comX; |
|
200 |
origPoint.y = comY; |
|
201 |
origPoint.z = distance; |
|
191 | 202 |
|
192 |
origPoint.x = comX; |
|
193 |
origPoint.y = comY; |
|
194 |
origPoint.z = distance; |
|
195 |
|
|
196 |
// TODO transform into point |
|
203 |
// TODO transform into point |
|
197 | 204 |
|
198 |
// for now just publish that |
|
205 |
// for now just publish that
|
|
199 | 206 |
|
200 |
pub.publish(origPoint); |
|
207 |
pub.publish(origPoint); |
|
208 |
} |
|
201 | 209 |
} |
202 | 210 |
|
203 | 211 |
int main(int argc, char **argv) { |
204 | 212 |
ros::init(argc, argv, "v2v3_converter"); |
205 | 213 |
ros::NodeHandle n; |
206 |
ros::Subscriber sub = n.subscribe("openni/image", 1, target_cb);
|
|
214 |
ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb);
|
|
207 | 215 |
pub = n.advertise<geometry_msgs::Point>("target_3d", 1000); |
216 |
img_pub = n.advertise<sensor_msgs::Image>("blob_image", 1000); |
|
208 | 217 |
ros::spin(); |
209 | 218 |
return 0; |
210 | 219 |
} |
Also available in: Unified diff