root / vision / src / opencvdetect.cpp @ b11b2b30
History | View | Annotate | Download (4.69 KB)
1 |
#include <opencv/cv.h> |
---|---|
2 |
#include <opencv/highgui.h> |
3 |
#include <opencv/cxcore.h> |
4 |
#include <ros/ros.h> |
5 |
#include <stdio.h> |
6 |
#include <cv_bridge/cv_bridge.h> |
7 |
#include <sensor_msgs/image_encodings.h> |
8 |
#include <vision/TargetDescriptor.h> |
9 |
#include <image_transport/image_transport.h> |
10 |
#include <geometry_msgs/Point.h> |
11 |
#include <tf/transform_listener.h> |
12 |
#include <algorithm> |
13 |
#include <blob.h> |
14 |
#include <BlobResult.h> |
15 |
|
16 |
using namespace std; |
17 |
using namespace cv; |
18 |
|
19 |
ros::Publisher pub; |
20 |
ros::Publisher img_pub; |
21 |
|
22 |
int hk = 216; |
23 |
int sk = 255; |
24 |
int vk = 255; |
25 |
//int threshold = 25;
|
26 |
|
27 |
int filter(int r, int g, int b, int threshold) { |
28 |
int orange[3] = {hk,sk,vk}; // Orange in HSV |
29 |
int diff = (orange[0]-r)*(orange[0]-r) + |
30 |
(orange[1]-g)*(orange[1]-g); |
31 |
|
32 |
if(diff < threshold) return abs(diff-threshold); |
33 |
return 0; |
34 |
} |
35 |
|
36 |
void target_cb(const sensor_msgs::Image orig) { |
37 |
|
38 |
printf("beginning callback \n");
|
39 |
tf::TransformListener listener; |
40 |
geometry_msgs::Point point; |
41 |
tf::StampedTransform transform; |
42 |
/*try {
|
43 |
listener.lookupTransform("/camera", "/kinect", ros::Time(0), transform);
|
44 |
}
|
45 |
catch (tf::TransformException ex) {
|
46 |
ROS_ERROR("%s", ex.what());
|
47 |
}*/
|
48 |
|
49 |
// convert to OpenCV
|
50 |
sensor_msgs::Image hsvcopy = orig; |
51 |
cv_bridge::CvImagePtr cv_ptr; |
52 |
try{
|
53 |
cv_ptr = cv_bridge::toCvCopy(orig, sensor_msgs::image_encodings::BGR8); |
54 |
} |
55 |
catch (cv_bridge::Exception& e) {
|
56 |
ROS_ERROR("cv_bridge exception: %s", e.what());
|
57 |
return;
|
58 |
} |
59 |
|
60 |
// detect blob
|
61 |
IplImage* input; |
62 |
input = new IplImage;
|
63 |
*input = cv_ptr->image; |
64 |
|
65 |
IplImage* img; |
66 |
IplImage *hsv_img; |
67 |
IplImage *bw_img; |
68 |
IplImage* i1; |
69 |
CBlobResult blobs; |
70 |
CBlob blobArea; |
71 |
|
72 |
// Initialize images, allocate memory
|
73 |
|
74 |
img = cvCloneImage(input); |
75 |
hsv_img = cvCloneImage(img); |
76 |
bw_img = cvCreateImage(cvSize(img->width, img->height), 8, 1);; |
77 |
i1 = cvCreateImage(cvSize(img->width, img->height), 8, 1); |
78 |
|
79 |
printf("Initialized!\n");
|
80 |
printf("%x\n", input);
|
81 |
printf("%x\n", img);
|
82 |
printf("%x\n", hsv_img);
|
83 |
printf("%x\n", bw_img);
|
84 |
printf("%x\n", i1);
|
85 |
// Smooth input image using a Gaussian filter, assign HSV, BW image
|
86 |
cvSmooth(input, img, CV_GAUSSIAN, 7, 9, 0 ,0); |
87 |
printf("1");
|
88 |
cvCvtColor(img, bw_img, CV_RGB2GRAY); |
89 |
printf("2");
|
90 |
cvCvtColor(img, hsv_img, CV_RGB2HSV); |
91 |
printf("3");
|
92 |
|
93 |
// Simple filter that creates a mask whose color is near orange in i1
|
94 |
for(int i = 0; i < hsv_img->height; i++) { |
95 |
for(int j = 0; j < hsv_img->width; j++) { |
96 |
int r = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 2]; |
97 |
int g = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 1]; |
98 |
int b = ((uchar *)(hsv_img->imageData + i*hsv_img->widthStep))[j*hsv_img->nChannels + 0]; |
99 |
int f = filter(r, g, b, 5000); |
100 |
if(f) {
|
101 |
((uchar *)(i1->imageData + i*i1->widthStep))[j] = 250;
|
102 |
} |
103 |
} |
104 |
} |
105 |
|
106 |
printf("4");
|
107 |
// Detect Blobs using the mask and a threshold for area. Sort blobs according to area
|
108 |
blobs = CBlobResult(bw_img, i1, 100/*, true*/); |
109 |
blobs.Filter(blobs, B_INCLUDE, CBlobGetArea(), B_GREATER, 500);
|
110 |
blobs.GetNthBlob(CBlobGetArea(), blobs.GetNumBlobs() - 1, blobArea); // example |
111 |
|
112 |
for (int i = 1; i < blobs.GetNumBlobs(); i++ ) |
113 |
{ |
114 |
blobArea = blobs.GetBlob(i); |
115 |
if(blobArea.Area() > 150) |
116 |
blobArea.FillBlob(img, cvScalar(255, 0, 0)); |
117 |
} |
118 |
|
119 |
// Publish blob image
|
120 |
|
121 |
cv_bridge::CvImage blob_image; |
122 |
//blob_image.header = img->header;
|
123 |
blob_image.encoding = sensor_msgs::image_encodings::BGR8; |
124 |
blob_image.image = *(img->imageData); |
125 |
|
126 |
/*sensor_msgs::Image blob_image;
|
127 |
blob_image.height = img->height;
|
128 |
blob_image.width = img->width;
|
129 |
blob_image.data = *(img->imageData); */
|
130 |
img_pub.publish(blob_image); |
131 |
|
132 |
// Clean up
|
133 |
cvReleaseImage(&i1); |
134 |
cvReleaseImage(&bw_img); |
135 |
cvReleaseImage(&hsv_img); |
136 |
cvReleaseImage(&img); |
137 |
cvReleaseImage(&input); |
138 |
|
139 |
//int comX;
|
140 |
//int comY;
|
141 |
//int area;
|
142 |
|
143 |
//int distance;
|
144 |
|
145 |
// fill point based on target and tf
|
146 |
|
147 |
// geometry_msgs::Point origPoint;
|
148 |
|
149 |
// TODO: Change coordinate axes
|
150 |
|
151 |
// origPoint.x = distance;
|
152 |
// origPoint.y = width / 2 - comX;
|
153 |
// origPoint.z = comY - height / 2;
|
154 |
|
155 |
// TODO transform into point
|
156 |
|
157 |
// for now just publish that
|
158 |
|
159 |
// pub.publish(origPoint);
|
160 |
|
161 |
printf("Ending callback\n");
|
162 |
|
163 |
} |
164 |
|
165 |
void target_cb_test(const sensor_msgs::Image image){ |
166 |
|
167 |
geometry_msgs::Point point; |
168 |
point.x = 1;
|
169 |
point.y = 2;
|
170 |
point.z = 3;
|
171 |
pub.publish(point); |
172 |
} |
173 |
|
174 |
int main(int argc, char **argv) { |
175 |
ros::init(argc, argv, "v2v3_converter");
|
176 |
ros::NodeHandle n; |
177 |
pub = n.advertise<geometry_msgs::Point>("vision/target_3d", 1000); |
178 |
img_pub = n.advertise<sensor_msgs::Image>("vision/blob_image", 1000); |
179 |
ros::Subscriber sub = n.subscribe("/camera/rgb/image_color", 1, target_cb); |
180 |
ros::spin(); |
181 |
return 0; |
182 |
} |