Revision 06738945 vision/src/v2v3_converter.cpp

View differences:

vision/src/v2v3_converter.cpp
1 1
#include <ros/ros.h>
2 2
#include <vision/TargetDescriptor.h>
3
#include <image_transport/image_transport.h>
3 4
#include <geometry_msgs/Point.h>
4 5
#include <tf/transform_listener.h>
5
#include <opencv/cv.hpp>
6
//#include <cv_bridge/cv_bridge.h>
7 6

  
8 7
ros::Publisher pub;
9 8

  
......
11 10
int sk = 255;
12 11
int vk = 255;
13 12

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

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

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

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

  
30
bool hasAnAdjacent(cv_bridge::CvImagePtr image, int j){
29
bool hasAnAdjacent(sensor_msgs::Image * image, int j){
31 30
	int nl = image->height; // number of lines
32
	int nc = image->width; // number of columns CHECK TO MAKE SURE THIS IS RIGHT AND NOT WITH CHANNELS
33
	int pos = j / image->nChannels; 
31
	int nc = image->width; 
32
    int nChannels = image->width / image->step;
33
	int pos = j / nChannels; 
34 34
	if ((pos % nc) != 0){
35
		if isLeft() return true;
35
		if (isLeft(image, pos)) return true;
36 36
	}
37 37
	if ((pos / nc) != 0){
38
		if isUp() return true;
38
		if (isUp(image, pos)) return true;
39 39
	}
40 40
	if ((pos % nc) != nc - 1){
41
		if isRight() return true;
41
		if (isRight(image, pos)) return true;
42 42
	}
43 43
	if ((pos / nc) != nc - 1){
44
		if isDown() return true;
44
		if (isDown(image, pos)) return true;
45 45
	}
46 46
	else return false;
47 47
}
......
49 49
/* Detects the blobs in an image within above defined constraints. Transforms image into 1 
50 50
 * for pixels in the blob, 0 for not in the blob.
51 51
 */
52
void detectBlobs(cv_bridge::CvImagePtr image, int * comX, int * comY, int * area){
52
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){
53 53
	int nl = image->height; // number of lines
54
	int nc = image->width * image->nChannels; // number of columns
55
	int step = image->widthStep; // effective width
54
	int nc = image->step; // number of columns
55
	int nChannels = image->step / image->width; // effective width
56 56
	
57 57
	// process image
58 58
	for (int i = 1; i < nl; i++){
59
		for (int j = 0; j < nc; j+= image->nChannels){
60
			if (image->data[j] > 30 || image->data[j] < 20){
61
				image->data[j] = 1;
62
				image->data[j+1] = 1;
63
				image->data[j+2] = 1;
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 64
			}
65 65
			else {
66
				image->data[j] = 0;
67
				image->data[j+1] = 0;
68
				image->data[j+2] = 0;
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 69
			}
70 70
		}
71 71
	}
......
73 73
	int comXSum = 0;
74 74
	int comYSum = 0;
75 75
	int blobPixelCount = 0;
76
	int maxHeight = 0;
77
	int minHeight = 0;
78
	for (int i = 0; i < image->width * image->height * image->nChannels; 
79
		i+=image->nChannels){
80
		if ( !(data[i] == 1 && hasAnAdjacent(image, i)) ){
81
			data[i] = 0;
82
			data[i + 1] = 0;
83
			data[i + 2] = 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;
84 81
		}
85 82
		blobPixelCount++;
86
		comXSum += data[i] % nc + 1;
87
		comYSum += data[i] / nc + 1;
83
		comXSum += image->data[i] % nc + 1;
84
		comYSum += image->data[i] / nc + 1;
88 85
	}
89 86
	comXSum /= blobPixelCount;
90 87
	comYSum /= blobPixelCount;
......
94 91
	*comY = comYSum;
95 92
}
96 93

  
97
void target_cb(const sensor_msgs::msg::_Image::Image orig) {
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) {
98 155
  tf::TransformListener listener;
99 156
  geometry_msgs::Point point;
100 157
  tf::StampedTransform transform;
......
107 164

  
108 165
  // convert to OpenCV
109 166

  
110
  Image * source = &orig; // pointer to the image
111

  
112
  cv_bridge::CvImagePtr origRGBImage = toCvCopy(const sensor_msgs::Image& source, 
113
	const std::string& encoding = "rgb8"); // convert to OpenCV, CHECK FOR SYNTAX
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;
114 171

  
115 172
  // convert to HSV
116

  
117
  cv_bridge::CvImagePtr HSVImage;
118
  cvCvtColor (origRGBImage, HSVImage, CV_RGB2HSV);
173
  
174
  convertToHSV(source, hsvPtr);
119 175

  
120 176
  // detect blob
121 177

  
......
123 179
  int comY;
124 180
  int area;
125 181
  
126
  detectBlobs(HSVImage, &comX, &comY, &area);
182
  detectBlobs(hsvPtr, &comX, &comY, &area);
127 183

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

  
......
131 187

  
132 188
  // fill point based on target and tf
133 189

  
134
  geometry_msgs::Point origPoint = new Point_(); 
190
  geometry_msgs::Point origPoint;
135 191

  
136 192
  origPoint.x = comX;
137 193
  origPoint.y = comY; 
138 194
  origPoint.z = distance;
139 195

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

  
142
  geometry_msgs::Point point;
143

  
144
  pub.publish(point);
200
  pub.publish(origPoint);
145 201
}
146 202

  
147 203
int main(int argc, char **argv) {

Also available in: Unified diff