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