Statistics
| Branch: | Revision:

root / vision / src / v2v3_converter.cpp @ 14c71a7d

History | View | Annotate | Download (5.91 KB)

1
#include <ros/ros.h>
2
#include <stdio.h>
3
#include <vision/TargetDescriptor.h>
4
#include <image_transport/image_transport.h>
5
#include <geometry_msgs/Point.h>
6
#include <tf/transform_listener.h>
7

    
8
ros::Publisher pub; // publishes Point
9
ros::Publisher img_pub; //publishes Image with only blob showing
10

    
11
int hk = 216; // target hsv values
12
int sk = 255;
13
int vk = 255;
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
 */
19
bool isRight(sensor_msgs::Image * img, int p){
20
        return (img->data[p + 1] == img->data[p]);
21
}
22

    
23
bool isLeft(sensor_msgs::Image * img, int p){
24
        return (img->data[p - 1] == img->data[p]);
25
}
26

    
27
bool isUp(sensor_msgs::Image * img, int p){
28
        return (img->data[p - img->width] == img->data[p]);
29
}
30

    
31
bool isDown(sensor_msgs::Image * img, int p){
32
        return (img->data[p + img->width] == img->data[p]);
33
}
34

    
35
int min(int a, int b){
36
        if (a <= b) return a;
37
        else return b;
38
}
39

    
40
int max(int a, int b){
41
        if (a >= b) return a;
42
        else return b;
43
}
44

    
45
/* Checks for any adjacent pixel, ensuring a continuous blob.
46
 */
47
bool hasAnAdjacent(sensor_msgs::Image * image, int j){
48
        int nl = image->height; // number of lines
49
        int nc = image->width; 
50
        int nChannels = image->step / image->width;
51
        int pos = j / nChannels; 
52
        if ((pos % nc) != 0){
53
                if (isLeft(image, pos)) return true;
54
        }
55
        if ((pos / nc) != 0){
56
                if (isUp(image, pos)) return true;
57
        }
58
        if ((pos % nc) != nc - 1){
59
                if (isRight(image, pos)) return true;
60
        }
61
        if ((pos / nc) != nc - 1){
62
                if (isDown(image, pos)) return true;
63
        }
64
        else return false;
65
}
66

    
67
/* Detects the blobs in an image within above defined constraints. Transforms image into 255
68
 * for pixels in the blob, 0 for not in the blob.
69
 */
70
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){
71
        int nl = image->height; // number of lines
72
        int nc = image->step; // number of columns
73
        int nChannels = image->step / image->width; // effective width of each pixel
74
        
75
        // process image
76
        for (int i = 0; i < nl; i++){
77
                for (int j = 0; j < nc; j+= nChannels){
78
                    // if within bounds...        
79
            if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){ 
80
                                image->data[i * image->width + j] = 255; // set each to 255
81
                                image->data[i * image->width + j+1] = 255;
82
                                image->data[i * image->width + j+2] = 255;
83
                        }
84
            // otherwise set to 0
85
                        else {
86
                                image->data[i * image->width + j] = 0;
87
                                image->data[i * image->width + j+1] = 0;
88
                                image->data[i * image->width + j+2] = 0;
89
                        }
90
                }
91
        }
92

    
93

    
94
    // The following is to find the average
95
        int comXSum = 0; 
96
        int comYSum = 0; 
97
        int blobPixelCount = 0; 
98
        for (int i = 0; i < image->width * image->height; i+=nChannels){
99
        // if the pixel is not both the right hue and has adjacents, set to 0
100
                if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){
101
                        image->data[i] = 0;
102
                        image->data[i + 1] = 0;
103
                        image->data[i + 2] = 0;
104
                }
105
        else {
106
                    blobPixelCount++;
107
                    comXSum += image->data[i] / 255;
108
                    comYSum += image->data[i] / 255;
109
        }
110
        }
111
  if (blobPixelCount != 0){
112
        // get average
113
          comXSum /= blobPixelCount;
114
          comYSum /= blobPixelCount;
115
  }
116
    // output results
117
        *area = blobPixelCount;
118
    *comX = comXSum;
119
    *comY = comYSum;
120

    
121
    img_pub.publish(*image); // publish new image of only black and white
122
}
123

    
124
void convertToHSV(sensor_msgs::Image * orig){
125

    
126
  int nChannels = orig->step / orig->width; 
127
  printf("Converting to HSV \n");
128

    
129
  // get the max, set it as v
130

    
131
  for (int i = 0; i < orig->height * orig->width; i += nChannels){
132
      int h;
133
      int s;
134
      int v; 
135
      int low;
136

    
137
      // get min, max
138
      v = max(max(orig->data[i], orig->data[i+1]), orig->data[i+2]);
139
      low = min(min(orig->data[i], orig->data[i+1]), orig->data[i+2]);
140

    
141
      // set s
142
      if (v != 0){
143
      s = (v - low) / v; 
144
      }
145
      else s = 0;
146

    
147
      // set h
148
      if (s == 0) h = 0;
149
      else if (v == orig->data[i]){
150
        h = 60 * (orig->data[i+1] - orig->data[i+2]) / s;
151
      }
152
      else if (v == orig->data[i+1]){
153
        h = 120 + 60 * (orig->data[i+2] - orig->data[i]) / s;
154
      }
155
      else {
156
        h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s;
157
      }
158
      
159
      // set values
160
      orig->data[i] = h;
161
      orig->data[i+1] = s;
162
      orig->data[i+2] = v;
163

    
164
  }
165
  printf("Conversion complete\n");
166
}
167

    
168
void target_cb(const sensor_msgs::Image orig) {
169

    
170
  printf("beginning callback \n");
171
  tf::TransformListener listener;
172
  geometry_msgs::Point point;
173
  tf::StampedTransform transform;
174
  /*try {
175
    listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);
176
  }
177
  catch (tf::TransformException ex) {
178
    ROS_ERROR("%s", ex.what());
179
  }*/
180

    
181
  // image types
182

    
183
  sensor_msgs::Image copy = orig;
184
  sensor_msgs::Image * source = &copy; // pointer to the image
185
  sensor_msgs::Image hsvImage = orig; // May be time 
186
  sensor_msgs::Image * hsvPtr = &hsvImage;
187

    
188
  // convert to HSV
189
  
190
  convertToHSV(source);
191

    
192
  // detect blob
193

    
194
  int comX; 
195
  int comY;
196
  int area;
197
  
198
  detectBlobs(hsvPtr, &comX, &comY, &area);
199

    
200
  int distance;
201

    
202
  // give distance as inversely proportional to area, for (x,y,z) 
203
  if (area > 1){
204
    
205
    distance = 100.0 / (float) area;
206

    
207
  }
208
  else {
209
    distance = -1;
210
  } 
211

    
212
  // fill point based on target and tf
213

    
214
  geometry_msgs::Point origPoint;
215

    
216
  // TODO: Change coordinate axes
217

    
218
  origPoint.x = comX;
219
  origPoint.y = comY; 
220
  origPoint.z = distance;
221

    
222
  // TODO transform into point
223
 
224
  // for now just publish that
225
    
226
  pub.publish(origPoint);
227

    
228
  printf("Ending callback");
229

    
230
}
231

    
232
void target_cb_test(const sensor_msgs::Image image){
233

    
234
        geometry_msgs::Point point;
235
        point.x = 1;
236
        point.y = 2;
237
        point.z = 3;
238
        pub.publish(point);
239
}
240

    
241
int main(int argc, char **argv) {
242
  ros::init(argc, argv, "v2v3_converter");
243
  ros::NodeHandle n;
244
  pub = n.advertise<geometry_msgs::Point>("vision/target_3d", 1000);
245
  img_pub = n.advertise<sensor_msgs::Image>("vision/blob_image", 1000);
246
  ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb);
247
  ros::spin();
248
  return 0;
249
}