Statistics
| Branch: | Revision:

root / vision / src / opencvdetect.cpp @ 53f28116

History | View | Annotate | Download (4.33 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
  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
  IplImage input(cv_ptr->image);
61

    
62
  IplImage *img;
63
  IplImage *hsv_img;
64
  IplImage *bw_img;
65
  IplImage *i1;
66
  CBlobResult blobs;
67
  CBlob blobArea;
68
  
69
  // Initialize images, allocate memory
70

    
71
  img = cvCloneImage(&input);
72
  hsv_img = cvCloneImage(img);
73
  bw_img = cvCreateImage(cvSize(img->width, img->height), 8, 1);
74
  i1 = cvCreateImage(cvSize(img->width, img->height), 8, 1);
75

    
76
  // Smooth input image using a Gaussian filter, assign HSV, BW image
77
  cvSmooth(&input, img, CV_GAUSSIAN, 7, 9, 0 ,0);
78
  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
  sensor_msgs::Image blob_image;
109
  blob_image.height = img->height;
110
  blob_image.width = img->width;
111
  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
  img_pub.publish(blob_image);  
115

    
116
  // Clean up
117
  cvReleaseImage(&i1);
118
  cvReleaseImage(&bw_img);
119
  cvReleaseImage(&hsv_img);
120
  cvReleaseImage(&img);
121

    
122
  //int comX;
123
  //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
}