Statistics
| Branch: | Revision:

root / vision / src / v2v3_converter.cpp @ 06738945

History | View | Annotate | Download (5.11 KB)

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

    
7
ros::Publisher pub;
8

    
9
int hk = 25; 
10
int sk = 255;
11
int vk = 255;
12

    
13
bool isRight(sensor_msgs::Image * img, int p){
14
        return (img->data[p + 1] == img->data[p]);
15
}
16

    
17
bool isLeft(sensor_msgs::Image * img, int p){
18
        return (img->data[p - 1] == img->data[p]);
19
}
20

    
21
bool isUp(sensor_msgs::Image * img, int p){
22
        return (img->data[p - img->width] == img->data[p]);
23
}
24

    
25
bool isDown(sensor_msgs::Image * img, int p){
26
        return (img->data[p + img->width] == img->data[p]);
27
}
28

    
29
bool hasAnAdjacent(sensor_msgs::Image * image, int j){
30
        int nl = image->height; // number of lines
31
        int nc = image->width; 
32
    int nChannels = image->width / image->step;
33
        int pos = j / nChannels; 
34
        if ((pos % nc) != 0){
35
                if (isLeft(image, pos)) return true;
36
        }
37
        if ((pos / nc) != 0){
38
                if (isUp(image, pos)) return true;
39
        }
40
        if ((pos % nc) != nc - 1){
41
                if (isRight(image, pos)) return true;
42
        }
43
        if ((pos / nc) != nc - 1){
44
                if (isDown(image, pos)) return true;
45
        }
46
        else return false;
47
}
48

    
49
/* Detects the blobs in an image within above defined constraints. Transforms image into 1 
50
 * for pixels in the blob, 0 for not in the blob.
51
 */
52
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){
53
        int nl = image->height; // number of lines
54
        int nc = image->step; // number of columns
55
        int nChannels = image->step / image->width; // effective width
56
        
57
        // process image
58
        for (int i = 1; i < nl; i++){
59
                for (int j = 0; j < nc; j+= nChannels){
60
                        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;
64
                        }
65
                        else {
66
                                image->data[i * image->width + j] = 0;
67
                                image->data[i * image->width + j+1] = 0;
68
                                image->data[i * image->width + j+2] = 0;
69
                        }
70
                }
71
        }
72
        
73
        int comXSum = 0;
74
        int comYSum = 0;
75
        int blobPixelCount = 0;
76
        for (int i = 0; i < image->width * image->height; i+=nChannels){
77
                if ( !(image->data[i] == 1 && hasAnAdjacent(image, i)) ){
78
                        image->data[i] = 0;
79
                        image->data[i + 1] = 0;
80
                        image->data[i + 2] = 0;
81
                }
82
                blobPixelCount++;
83
                comXSum += image->data[i] % nc + 1;
84
                comYSum += image->data[i] / nc + 1;
85
        }
86
        comXSum /= blobPixelCount;
87
        comYSum /= blobPixelCount;
88
        
89
        *area = blobPixelCount;
90
        *comX = comXSum;
91
        *comY = comYSum;
92
}
93

    
94
void convertToHSV(sensor_msgs::Image * orig, sensor_msgs::Image * result){
95
  int nChannels = orig->width / orig->step;
96

    
97
  // get the max, set it as v
98

    
99
  for (int i = 0; i < orig->height * orig->width; i += nChannels){
100
      int h;
101
      int s;
102
      int v; 
103
      int min;
104

    
105
      // get min, max
106
      if (orig->data[i] >= orig->data[i+1] && orig->data[i] >= orig->data[i+2]){
107
        v = orig->data[i];
108
        if (orig->data[i+1] >= orig->data[i+2]){
109
            min = orig->data[i+2];
110
        }
111
        else min = orig->data[i+1];
112
      }
113
      else if (orig->data[i+1] >= orig->data[i] && orig->data[i+1] >= orig->data[i+2]){
114
        v = orig->data[i+1];
115
        if (orig->data[i] >= orig->data[i+2]){
116
          min = orig->data[i+2];
117
        }
118
        else min = orig->data[i];
119
      }
120
      else {
121
        v = orig->data[i+2];
122
        if (orig->data[i] >= orig->data[i+1]){
123
            min = orig->data[i+1];
124
        }
125
        else min = orig->data[i];
126
      }
127

    
128
      // set s
129
      if (v != 0){
130
      s = (v - min) / v; 
131
      }
132
      else s = 0;
133

    
134
      // set h
135
      if (s == 0) h = 0;
136
      else if (v == orig->data[i]){
137
        h = 60 * (orig->data[i+1] - orig->data[i+2]) / s;
138
      }
139
      else if (v == orig->data[i+1]){
140
        h = 120 + 60 * (orig->data[i+2] - orig->data[i]) / s;
141
      }
142
      else {
143
        h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s;
144
      }
145
      
146
      result->data[i] = h;
147
      result->data[i+1] = s;
148
      result->data[i+2] = v;
149

    
150
  }
151

    
152
}
153

    
154
void target_cb(const sensor_msgs::Image orig) {
155
  tf::TransformListener listener;
156
  geometry_msgs::Point point;
157
  tf::StampedTransform transform;
158
  try {
159
    listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);
160
  }
161
  catch (tf::TransformException ex) {
162
    ROS_ERROR("%s", ex.what());
163
  }
164

    
165
  // convert to OpenCV
166

    
167
  sensor_msgs::Image copy = orig;
168
  sensor_msgs::Image * source = &copy; // pointer to the image
169
  sensor_msgs::Image hsvImage = sensor_msgs::Image();
170
  sensor_msgs::Image * hsvPtr = &hsvImage;
171

    
172
  // convert to HSV
173
  
174
  convertToHSV(source, hsvPtr);
175

    
176
  // detect blob
177

    
178
  int comX; 
179
  int comY;
180
  int area;
181
  
182
  detectBlobs(hsvPtr, &comX, &comY, &area);
183

    
184
  // give distance as inversely proportional to area, for (x,y,z) 
185

    
186
  int distance = 100.0 / (float) area;
187

    
188
  // fill point based on target and tf
189

    
190
  geometry_msgs::Point origPoint;
191

    
192
  origPoint.x = comX;
193
  origPoint.y = comY; 
194
  origPoint.z = distance;
195

    
196
  // TODO transform into point
197
 
198
  // for now just publish that
199

    
200
  pub.publish(origPoint);
201
}
202

    
203
int main(int argc, char **argv) {
204
  ros::init(argc, argv, "v2v3_converter");
205
  ros::NodeHandle n;
206
  ros::Subscriber sub = n.subscribe("openni/image", 1, target_cb);
207
  pub = n.advertise<geometry_msgs::Point>("target_3d", 1000);
208
  ros::spin();
209
  return 0;
210
}