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