root / vision / src / v2v3_converter.cpp @ b1055c82
History | View | Annotate | Download (3.69 KB)
1 | 80eadc1e | Tom Mullins | #include <ros/ros.h> |
---|---|---|---|
2 | #include <vision/TargetDescriptor.h> |
||
3 | #include <geometry_msgs/Point.h> |
||
4 | #include <tf/transform_listener.h> |
||
5 | b1055c82 | Tom Mullins | #include <opencv/cv.hpp> |
6 | //#include <cv_bridge/cv_bridge.h>
|
||
7 | 80eadc1e | Tom Mullins | |
8 | ros::Publisher pub; |
||
9 | |||
10 | 880aeb43 | Nick Stanley | int hk = 25; |
11 | int sk = 255; |
||
12 | int vk = 255; |
||
13 | |||
14 | b1055c82 | Tom Mullins | bool isRight(cv_bridge::CvImagePtr img, int p){ |
15 | 880aeb43 | Nick Stanley | return (img->data[p + 1] == img->data[p]); |
16 | } |
||
17 | |||
18 | b1055c82 | Tom Mullins | bool isLeft(cv_bridge::CvImagePtr img, int p){ |
19 | 880aeb43 | Nick Stanley | return (img->data[p - 1] == img->data[p]); |
20 | } |
||
21 | |||
22 | b1055c82 | Tom Mullins | bool isUp(cv_bridge::CvImagePtr img, int p){ |
23 | return (img->data[p - img->width() * img->channels()] == img->data[p]);
|
||
24 | 880aeb43 | Nick Stanley | } |
25 | |||
26 | b1055c82 | Tom Mullins | bool isDown(cv_bridge::CvImagePtr img, int p){ |
27 | 880aeb43 | Nick Stanley | return (img->data[p + img->width * img->nChannels] == img->data[p]);
|
28 | } |
||
29 | |||
30 | b1055c82 | Tom Mullins | bool hasAnAdjacent(cv_bridge::CvImagePtr image, int j){ |
31 | 880aeb43 | Nick Stanley | 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;
|
||
34 | if ((pos % nc) != 0){ |
||
35 | if isLeft() return true; |
||
36 | } |
||
37 | if ((pos / nc) != 0){ |
||
38 | if isUp() return true; |
||
39 | } |
||
40 | if ((pos % nc) != nc - 1){ |
||
41 | if isRight() return true; |
||
42 | } |
||
43 | if ((pos / nc) != nc - 1){ |
||
44 | if isDown() 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 | b1055c82 | Tom Mullins | void detectBlobs(cv_bridge::CvImagePtr image, int * comX, int * comY, int * area){ |
53 | 880aeb43 | Nick Stanley | int nl = image->height; // number of lines |
54 | int nc = image->width * image->nChannels; // number of columns |
||
55 | int step = image->widthStep; // effective width |
||
56 | |||
57 | // process image
|
||
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; |
||
64 | } |
||
65 | else {
|
||
66 | image->data[j] = 0;
|
||
67 | image->data[j+1] = 0; |
||
68 | image->data[j+2] = 0; |
||
69 | } |
||
70 | } |
||
71 | } |
||
72 | |||
73 | int comXSum = 0; |
||
74 | int comYSum = 0; |
||
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; |
||
84 | } |
||
85 | blobPixelCount++; |
||
86 | comXSum += data[i] % nc + 1;
|
||
87 | comYSum += data[i] / nc + 1;
|
||
88 | } |
||
89 | comXSum /= blobPixelCount; |
||
90 | comYSum /= blobPixelCount; |
||
91 | |||
92 | *area = blobPixelCount; |
||
93 | *comX = comXSum; |
||
94 | *comY = comYSum; |
||
95 | } |
||
96 | |||
97 | void target_cb(const sensor_msgs::msg::_Image::Image orig) { |
||
98 | 05e1c990 | Tom Mullins | tf::TransformListener listener; |
99 | 80eadc1e | Tom Mullins | geometry_msgs::Point point; |
100 | tf::StampedTransform transform; |
||
101 | try {
|
||
102 | listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform); |
||
103 | } |
||
104 | catch (tf::TransformException ex) {
|
||
105 | ROS_ERROR("%s", ex.what());
|
||
106 | } |
||
107 | 880aeb43 | Nick Stanley | |
108 | // convert to OpenCV
|
||
109 | |||
110 | Image * source = &orig; // pointer to the image
|
||
111 | |||
112 | b1055c82 | Tom Mullins | cv_bridge::CvImagePtr origRGBImage = toCvCopy(const sensor_msgs::Image& source,
|
113 | 880aeb43 | Nick Stanley | const std::string& encoding = "rgb8"); // convert to OpenCV, CHECK FOR SYNTAX |
114 | |||
115 | // convert to HSV
|
||
116 | |||
117 | b1055c82 | Tom Mullins | cv_bridge::CvImagePtr HSVImage; |
118 | 880aeb43 | Nick Stanley | cvCvtColor (origRGBImage, HSVImage, CV_RGB2HSV); |
119 | |||
120 | // detect blob
|
||
121 | |||
122 | int comX;
|
||
123 | int comY;
|
||
124 | int area;
|
||
125 | |||
126 | detectBlobs(HSVImage, &comX, &comY, &area); |
||
127 | |||
128 | // give distance as inversely proportional to area, for (x,y,z)
|
||
129 | |||
130 | int distance = 100.0 / (float) area; |
||
131 | |||
132 | // fill point based on target and tf
|
||
133 | |||
134 | geometry_msgs::Point origPoint = new Point_();
|
||
135 | |||
136 | origPoint.x = comX; |
||
137 | origPoint.y = comY; |
||
138 | origPoint.z = distance; |
||
139 | |||
140 | // TODO transform into point
|
||
141 | |||
142 | geometry_msgs::Point point; |
||
143 | |||
144 | 80eadc1e | Tom Mullins | pub.publish(point); |
145 | } |
||
146 | |||
147 | int main(int argc, char **argv) { |
||
148 | ros::init(argc, argv, "v2v3_converter");
|
||
149 | ros::NodeHandle n; |
||
150 | 880aeb43 | Nick Stanley | ros::Subscriber sub = n.subscribe("openni/image", 1, target_cb); |
151 | 80eadc1e | Tom Mullins | pub = n.advertise<geometry_msgs::Point>("target_3d", 1000); |
152 | ros::spin(); |
||
153 | 05e1c990 | Tom Mullins | return 0; |
154 | 80eadc1e | Tom Mullins | } |