calibrate_save_camera_laser_points.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 
3 #include <opencv2/core/core.hpp>
4 #include <opencv2/highgui/highgui.hpp>
5 #include <opencv2/imgproc/imgproc.hpp>
6 #include <iostream>
7 
8 #include "mtt/TargetList.h"
9 
10 #include <image_transport/image_transport.h>
11 
12 #include <cv_bridge/cv_bridge.h>
13 
14 #include "sensor_msgs/LaserScan.h"
15 #include "sensor_msgs/PointCloud.h"
16 #include <sensor_msgs/image_encodings.h>
17 
18 #include <pcl_conversions/pcl_conversions.h>
19 
20 using namespace std;
21 using namespace cv;
22 namespace enc = sensor_msgs::image_encodings;
23 
24 class ImageConverter
25 {
26  ros::NodeHandle nh_;
27  image_transport::ImageTransport it_;
28  image_transport::Subscriber image_sub_;
29  ros::Subscriber mtt_laser_sub_;
30 
31  public:
32 
33  double distance_to_target_from_center_bumper;
34  double largura;
35  int laser_done;
36  Point3f laser_left_point;
37  Point3f laser_right_point;
38 
39  std::string imagePoints1FileName;
40 // std::string imagePoints2FileName;
41 // std::string imagePoints3FileName;
42 
43  std::string objectPoints1FileName;
44 // std::string objectPoints2FileName;
45 // std::string objectPoints3FileName;
46 
47  ImageConverter()
48  : it_(nh_)
49  {
50  largura=0;
51  laser_done = 0;
52 
53  ros::NodeHandle nh_local("~");
54 
55  image_sub_ = it_.subscribe("/xb3/right/new_info/image_rect_color", 1, &ImageConverter::imageCb, this);
56 // image_sub_ = it_.subscribe("image", 1, &ImageConverter::imageCb, this);
57 // image_sub_ = it_.subscribe("/usb_cam_reader/image_raw", 1, &ImageConverter::imageCb, this);
58  mtt_laser_sub_ = nh_.subscribe("/new_targets", 1, &ImageConverter::mttLaserGather, this);
59 
60  nh_local.param<std::string>("imagePoints1FileName",imagePoints1FileName,"");
61 // nh_local.param<std::string>("imagePoints2FileName",imagePoints2FileName,"");
62 // nh_local.param<std::string>("imagePoints3FileName",imagePoints3FileName,"");
63 
64  nh_local.param<std::string>("objectPoints1FileName",objectPoints1FileName,"");
65 // nh_local.param<std::string>("objectPoints2FileName",objectPoints2FileName,"");
66 // nh_local.param<std::string>("objectPoints3FileName",objectPoints3FileName,"");
67 
68  }
69 
70  ~ImageConverter()
71  {
72 
73  }
74 
75  void imageCb(const sensor_msgs::ImageConstPtr& cam_msg)
76  {
77  cv_bridge::CvImagePtr cv_ptr;
78  try
79  {
80  cv_ptr = cv_bridge::toCvCopy(cam_msg, enc::BGR8);
81  }
82  catch (cv_bridge::Exception& e)
83  {
84  ROS_ERROR("cv_bridge exception: %s", e.what());
85  return;
86  }
87 // imshow("input4", cv_ptr->image);
88  // Convert code to ros, to subscrive image and laser data
89 
90 // Mat input = imread("/home/asus/workingcopies/lar4/src/perception/pedestrians/multimodal_pedestrian_detect/images/frame0009.jpg", CV_LOAD_IMAGE_COLOR);
91 
92  Mat input = cv_ptr->image;
93  Mat redOnly;
94 
95  if (laser_done == 2)
96  {
97 // resize(input, input, Size(800,600));
98 
99  // for(int j=1; j<210; j++)
100  // for(int i=1; i<800 ; i++)
101  // input.at<double>(j,i) = 0;
102 
103  //Grayscale matrix
104  Mat grayscaleMat(input.size(), CV_8UC1);
105 
106 // imshow("drawing1", grayscaleMat);
107 
108  cvtColor(input, grayscaleMat, CV_BGR2GRAY);
109  threshold(grayscaleMat, grayscaleMat, 170, 255, THRESH_BINARY);
110 
111 // imshow("drawing1", grayscaleMat);
112  // inRange(input, Scalar(75, 90, 0), Scalar(120,170,15), redOnly); // placa verde plastica
113 // inRange(input, Scalar(180, 180, 180), Scalar(255,255,255), redOnly); // placa branca
114 
115  //Find the contours. Use the contourOutput Mat so the original image doesn't get overwritten
116  vector<vector<Point> > contours;
117 // Mat contourOutput = redOnly.clone();
118  Mat contourOutput = grayscaleMat.clone();
119  findContours( contourOutput, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE );
120 
121  vector<vector<Point> > contours_poly( contours.size() );
122 
123  Mat drawing = Mat::zeros( grayscaleMat.size(), CV_8UC3 );
124 
125  int max_size_triangle=0;
126  for(size_t i = 0; i < contours.size(); i++)
127  {
128  approxPolyDP(Mat(contours[i]), contours_poly[i], 5, true);
129  // cout << "number of triangles: " << contours_poly.size() << endl;
130 
131  // cout << "size_t: " << i << endl;
132 // cout << "size_poly: " << contours_poly[i].size() << endl;
133  drawContours(drawing, contours_poly, i, Scalar(0, 0, 255),CV_FILLED,8); // fill RED
134  if (contours_poly[i].size() == 4) //from all object, only triangles are choosen
135  {
136 // cout << "arclength: " << arcLength(Mat(contours[i]),true) << endl;
137 // cout << "size_poly: " << contours_poly[i].size() << endl;
138 // cout << "size: " << contours[i].size() << endl;
139  // drawContours(drawing, contours_poly, i, Scalar(0, 0, 255),CV_FILLED,8); // fill RED
140  if(i > 0)
141  if( arcLength(Mat(contours[i]),true) > arcLength(Mat(contours[max_size_triangle]),true)) //choose bigger triangle
142  {
143  max_size_triangle = i;
144  }
145  }
146  }
147 
148  double min_width;
149  double max_width;
150  double left_height;
151  double right_height;
152 
153  Point left_upper_point;
154  Point right_upper_point;
155  Point left_lower_point;
156  Point right_lower_point;
157 
158  if (contours_poly[max_size_triangle][1].y < (contours_poly[max_size_triangle][0].y + 10) )
159  {
160  left_upper_point.x = contours_poly[max_size_triangle][1].x;
161  left_upper_point.y = contours_poly[max_size_triangle][1].y;
162 
163  right_upper_point.x = contours_poly[max_size_triangle][0].x;
164  right_upper_point.y = contours_poly[max_size_triangle][0].y;
165 
166  left_lower_point.x = contours_poly[max_size_triangle][2].x;
167  left_lower_point.y = contours_poly[max_size_triangle][2].y;
168 
169  right_lower_point.x = contours_poly[max_size_triangle][3].x;
170  right_lower_point.y = contours_poly[max_size_triangle][3].y;
171 
172  }else{
173  left_upper_point.x = contours_poly[max_size_triangle][0].x;
174  left_upper_point.y = contours_poly[max_size_triangle][0].y;
175 
176  right_upper_point.x = contours_poly[max_size_triangle][3].x;
177  right_upper_point.y = contours_poly[max_size_triangle][3].y;
178 
179  left_lower_point.x = contours_poly[max_size_triangle][1].x;
180  left_lower_point.y = contours_poly[max_size_triangle][1].y;
181 
182  right_lower_point.x = contours_poly[max_size_triangle][2].x;
183  right_lower_point.y = contours_poly[max_size_triangle][2].y;
184  }
185 
186  min_width = right_upper_point.x - left_upper_point.x;
187  max_width = right_lower_point.x - left_lower_point.x;
188  left_height = left_lower_point.y - left_upper_point.y;
189  right_height = right_lower_point.y - right_upper_point.y;
190 
191  cout << "triangle pixel size: " << max_size_triangle << " " << contours_poly[max_size_triangle] << " " << arcLength(Mat(contours[max_size_triangle]),true) << endl
192  << "max width: " << max_width << endl
193  << "min width: " << min_width << endl
194  << "left_height: " << left_height << endl
195  << "right_height: " << right_height << endl;
196 
197  drawContours(drawing, contours_poly, max_size_triangle, Scalar(255, 255, 255),CV_FILLED,8); // fill WHITE
198 
199 // cout << "here" << endl;
200 
201  double object_width_percent_laser_given = largura; // inside value is the percentage, return by the laser
202 
203  int laser_left_line_int = object_width_percent_laser_given * left_height;
204  double laser_left_line = object_width_percent_laser_given * left_height;
205 
206  int laser_right_line_int = object_width_percent_laser_given * right_height;
207  double laser_right_line = object_width_percent_laser_given * right_height;
208 
209  // cout << "laser_line, double: " << laser_line << " int: " << laser_line_int << endl;
210 
211  if(laser_left_line - laser_left_line_int >=0.5 )
212  laser_left_line_int++;
213 
214  if(laser_right_line - laser_right_line_int >=0.5 )
215  laser_right_line_int++;
216 
217  int laser_left_line_position = left_upper_point.y + laser_left_line_int;
218  int laser_right_line_position = right_upper_point.y + laser_right_line_int;
219 
220  //Grayscale matrix
221  Mat grayscaleMat_2(drawing.size(), CV_8UC1);
222 
223  //Convert BGR to Gray
224  cvtColor(drawing, grayscaleMat_2, CV_BGR2GRAY);
225 
226  threshold(grayscaleMat_2, grayscaleMat_2, 245, 255, THRESH_BINARY);
227 // cout << "here2" << endl;
228  int k=0;
229  Point left_point;
230  Point right_point;
231 
232  for(int col=(left_lower_point.x - 10); col<(right_lower_point.x + 10); col++)
233  if(grayscaleMat_2.at<uchar>(laser_left_line_position,col) != 0)
234  if (k==0)
235  {
236  left_point.y=laser_left_line_position;
237  left_point.x=col - 2;
238  break;
239  }
240 
241  for(int col=(right_lower_point.x + 10); col>(left_lower_point.x - 10); col--)
242  if(grayscaleMat_2.at<uchar>(laser_left_line_position,col) != 0)
243  if (k==0)
244  {
245  right_point.y=laser_left_line_position;
246  right_point.x=col + 2;
247  break;
248  }
249 
250 
251 // cout << "laser_line, double: " << laser_line << " int: " << laser_line_int << endl;
252  cout << "points, left_point: " << left_point << " right_point: " << right_point << endl;
253 
254  line(input, left_point, right_point, Scalar(0, 0, 255), 2, 8,0);
255 
256  // // gravar pontos da imagem automaticamente
257 
258  // Grava os pontos da imagem onde o feixe laser está a tocar
259  fstream fs;
260  fs.open (imagePoints1FileName, fstream::in | fstream::out | fstream::app);
261 
262 
263  fs << left_point.x << " " << left_point.y << "\n";
264  fs << right_point.x << " " << right_point.y << "\n";
265 
266  fs.close();
267 
268  cout << "please change the object position, and then run the program again" << endl;
269 
270  imshow("input_incial", input);
271 
272  imshow("drawing", drawing);
273  imshow("drawing1", grayscaleMat_2);
274  waitKey(0);
275 
276  image_sub_.shutdown();
277  }
278 
279  }
280 
281  void mttLaserGather(const mtt::TargetList& laser_msg)
282  {
283  double distance_to_target_from_center_bumper_;
284  double theta_;
285  double largura_;
286 
287  cout << "laser_size: " << laser_msg.Targets.size() << endl;
288 
289  for(int i=0; i<laser_msg.Targets.size();i++)
290  {
291  distance_to_target_from_center_bumper_ = sqrt(laser_msg.Targets[i].pose.position.x * laser_msg.Targets[i].pose.position.x +
292  laser_msg.Targets[i].pose.position.y * laser_msg.Targets[i].pose.position.y);
293 
294  theta_ = acos(laser_msg.Targets[i].pose.position.x / distance_to_target_from_center_bumper);
295 
296  largura_ = sqrt(((laser_msg.Targets[i].finalpose.x - laser_msg.Targets[i].initialpose.x) * (laser_msg.Targets[i].finalpose.x - laser_msg.Targets[i].initialpose.x)) + ((laser_msg.Targets[i].finalpose.y - laser_msg.Targets[i].initialpose.y) * (laser_msg.Targets[i].finalpose.y - laser_msg.Targets[i].initialpose.y)));
297 
298  if(laser_msg.Targets[i].pose.position.x > 0 && laser_msg.Targets[i].pose.position.y > -10 && laser_msg.Targets[i].pose.position.y < 10 /*&& largura_ > 0.10 && largura_ < 1.50*/) {
299 // if(laser_msg.Targets[i].id == 1)
300 // {
301  distance_to_target_from_center_bumper = distance_to_target_from_center_bumper_;
302 
303  cout << "largura"<< largura << endl;
304 
305  if (largura_ > largura)
306  {
307  largura = largura_;
308 
309  laser_left_point = Point3f(laser_msg.Targets[i].initialpose.x, laser_msg.Targets[i].initialpose.y, laser_msg.Targets[i].initialpose.z);
310 
311  laser_right_point = Point3f(laser_msg.Targets[i].finalpose.x, laser_msg.Targets[i].finalpose.y, laser_msg.Targets[i].finalpose.z);
312  }
313  }
314  }
315 
316  laser_done = laser_done + 1;
317 
318  cout << "laser_done, width: " << largura << endl;
319 
320  if (laser_done==2)
321  {
322  // Gravar pontos dos objectos automaticamente
323  fstream fs;
324  fs.open (objectPoints1FileName, fstream::in | fstream::out | fstream::app);
325 
326  fs << laser_left_point.x << " " << laser_left_point.y << " " << 0 << "\n";
327  fs << laser_right_point.x << " " << laser_right_point.y << " " << 0 << "\n";
328 
329  fs.close();
330 
331  mtt_laser_sub_.shutdown(); // the subscriber only runs one time
332  }
333  }
334 };
335 
336 
337 int main(int argc, char** argv)
338 {
339  ros::init(argc, argv, "calibrate_save_camera_laser_points_node");
340 
341  ImageConverter ic;
342 
343  ros::spin();
344  return 0;
345 }
imagePoints1FileName
std::string imagePoints1FileName
Definition: calibrate_camera_extrinsic_parameters.cpp:47
main
int main(int argc, char **argv)
Definition: calibrate_save_camera_laser_points.cpp:337
ImageConverter::mttLaserGather
void mttLaserGather(const mtt::TargetList &laser_msg)
Definition: calibrate_save_camera_laser_points.cpp:281
ImageConverter::imageCb
void imageCb(const sensor_msgs::ImageConstPtr &cam_msg)
Definition: calibrate_save_camera_laser_points.cpp:75
objectPoints1FileName
std::string objectPoints1FileName
Definition: calibrate_camera_extrinsic_parameters.cpp:48
ImageConverter::it_
image_transport::ImageTransport it_
Definition: calibrate_save_camera_laser_points.cpp:27
ImageConverter::image_sub_
image_transport::Subscriber image_sub_
Definition: calibrate_save_camera_laser_points.cpp:28

multimodal_pedestrian_detect
Author(s): Rui Azevedo
autogenerated on Mon Mar 2 2015 01:32:27