Revision 880aeb43
ID | 880aeb436048c6896f4facb2e1657b600f691367 |
Edited the v2v3 converter file
vision/src/v2v3_converter.cpp | ||
---|---|---|
2 | 2 |
#include <vision/TargetDescriptor.h> |
3 | 3 |
#include <geometry_msgs/Point.h> |
4 | 4 |
#include <tf/transform_listener.h> |
5 |
// include opencv |
|
5 | 6 |
|
6 | 7 |
ros::Publisher pub; |
7 | 8 |
|
8 |
void target_cb(const vision::TargetDescriptor target) { |
|
9 |
int hk = 25; |
|
10 |
int sk = 255; |
|
11 |
int vk = 255; |
|
12 |
|
|
13 |
bool isRight(CvImagePtr img, int p){ |
|
14 |
return (img->data[p + 1] == img->data[p]); |
|
15 |
} |
|
16 |
|
|
17 |
bool isLeft(CvImagePtr img, int p){ |
|
18 |
return (img->data[p - 1] == img->data[p]); |
|
19 |
} |
|
20 |
|
|
21 |
bool isUp(CvImagePtr img, int p){ |
|
22 |
return (img->data[p - img->width * img->nChannels] == img->data[p]); |
|
23 |
} |
|
24 |
|
|
25 |
bool isDown(CvImagePtr img, int p){ |
|
26 |
return (img->data[p + img->width * img->nChannels] == img->data[p]); |
|
27 |
} |
|
28 |
|
|
29 |
bool hasAnAdjacent(CvImagePtr image, int j){ |
|
30 |
int nl = image->height; // number of lines |
|
31 |
int nc = image->width; // number of columns CHECK TO MAKE SURE THIS IS RIGHT AND NOT WITH CHANNELS |
|
32 |
int pos = j / image->nChannels; |
|
33 |
if ((pos % nc) != 0){ |
|
34 |
if isLeft() return true; |
|
35 |
} |
|
36 |
if ((pos / nc) != 0){ |
|
37 |
if isUp() return true; |
|
38 |
} |
|
39 |
if ((pos % nc) != nc - 1){ |
|
40 |
if isRight() return true; |
|
41 |
} |
|
42 |
if ((pos / nc) != nc - 1){ |
|
43 |
if isDown() return true; |
|
44 |
} |
|
45 |
else return false; |
|
46 |
} |
|
47 |
|
|
48 |
/* Detects the blobs in an image within above defined constraints. Transforms image into 1 |
|
49 |
* for pixels in the blob, 0 for not in the blob. |
|
50 |
*/ |
|
51 |
void detectBlobs(CvImagePtr image, int * comX, int * comY, int * area){ |
|
52 |
int nl = image->height; // number of lines |
|
53 |
int nc = image->width * image->nChannels; // number of columns |
|
54 |
int step = image->widthStep; // effective width |
|
55 |
|
|
56 |
// process image |
|
57 |
for (int i = 1; i < nl; i++){ |
|
58 |
for (int j = 0; j < nc; j+= image->nChannels){ |
|
59 |
if (image->data[j] > 30 || image->data[j] < 20){ |
|
60 |
image->data[j] = 1; |
|
61 |
image->data[j+1] = 1; |
|
62 |
image->data[j+2] = 1; |
|
63 |
} |
|
64 |
else { |
|
65 |
image->data[j] = 0; |
|
66 |
image->data[j+1] = 0; |
|
67 |
image->data[j+2] = 0; |
|
68 |
} |
|
69 |
} |
|
70 |
} |
|
71 |
|
|
72 |
int comXSum = 0; |
|
73 |
int comYSum = 0; |
|
74 |
int blobPixelCount = 0; |
|
75 |
int maxHeight = 0; |
|
76 |
int minHeight = 0; |
|
77 |
for (int i = 0; i < image->width * image->height * image->nChannels; |
|
78 |
i+=image->nChannels){ |
|
79 |
if ( !(data[i] == 1 && hasAnAdjacent(image, i)) ){ |
|
80 |
data[i] = 0; |
|
81 |
data[i + 1] = 0; |
|
82 |
data[i + 2] = 0; |
|
83 |
} |
|
84 |
blobPixelCount++; |
|
85 |
comXSum += data[i] % nc + 1; |
|
86 |
comYSum += data[i] / nc + 1; |
|
87 |
} |
|
88 |
comXSum /= blobPixelCount; |
|
89 |
comYSum /= blobPixelCount; |
|
90 |
|
|
91 |
*area = blobPixelCount; |
|
92 |
*comX = comXSum; |
|
93 |
*comY = comYSum; |
|
94 |
} |
|
95 |
|
|
96 |
void target_cb(const sensor_msgs::msg::_Image::Image orig) { |
|
9 | 97 |
tf::TransformListener listener; |
10 | 98 |
geometry_msgs::Point point; |
11 | 99 |
tf::StampedTransform transform; |
... | ... | |
15 | 103 |
catch (tf::TransformException ex) { |
16 | 104 |
ROS_ERROR("%s", ex.what()); |
17 | 105 |
} |
18 |
// TODO fill point based on target and tf |
|
106 |
|
|
107 |
// convert to OpenCV |
|
108 |
|
|
109 |
Image * source = &orig; // pointer to the image |
|
110 |
|
|
111 |
CvImagePtr origRGBImage = toCvCopy(const sensor_msgs::Image& source, |
|
112 |
const std::string& encoding = "rgb8"); // convert to OpenCV, CHECK FOR SYNTAX |
|
113 |
|
|
114 |
// convert to HSV |
|
115 |
|
|
116 |
CvImagePtr HSVImage; |
|
117 |
cvCvtColor (origRGBImage, HSVImage, CV_RGB2HSV); |
|
118 |
|
|
119 |
// detect blob |
|
120 |
|
|
121 |
int comX; |
|
122 |
int comY; |
|
123 |
int area; |
|
124 |
|
|
125 |
detectBlobs(HSVImage, &comX, &comY, &area); |
|
126 |
|
|
127 |
// give distance as inversely proportional to area, for (x,y,z) |
|
128 |
|
|
129 |
int distance = 100.0 / (float) area; |
|
130 |
|
|
131 |
// fill point based on target and tf |
|
132 |
|
|
133 |
geometry_msgs::Point origPoint = new Point_(); |
|
134 |
|
|
135 |
origPoint.x = comX; |
|
136 |
origPoint.y = comY; |
|
137 |
origPoint.z = distance; |
|
138 |
|
|
139 |
// TODO transform into point |
|
140 |
|
|
141 |
geometry_msgs::Point point; |
|
142 |
|
|
19 | 143 |
pub.publish(point); |
20 | 144 |
} |
21 | 145 |
|
22 | 146 |
int main(int argc, char **argv) { |
23 | 147 |
ros::init(argc, argv, "v2v3_converter"); |
24 | 148 |
ros::NodeHandle n; |
25 |
ros::Subscriber sub = n.subscribe("target_2d", 1, target_cb);
|
|
149 |
ros::Subscriber sub = n.subscribe("openni/image", 1, target_cb);
|
|
26 | 150 |
pub = n.advertise<geometry_msgs::Point>("target_3d", 1000); |
27 | 151 |
ros::spin(); |
28 | 152 |
return 0; |
Also available in: Unified diff