blob_tracker.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <image_transport/image_transport.h>
3 #include <boost/thread/mutex.hpp>
4 #include <std_msgs/String.h>
5 #include <std_msgs/Int8.h>
6 
7 #include <cv_bridge/cv_bridge.h>
8 
9 #include <visp_bridge/3dpose.h>
10 
11 #include <visp3/gui/vpDisplayX.h>
12 #include <visp3/gui/vpDisplayGDI.h>
13 #include <visp3/gui/vpDisplayOpenCV.h>
14 #include <visp3/core/vpImageConvert.h>
15 #include <visp3/core/vpPixelMeterConversion.h>
16 #include <visp3/blob/vpDot2.h>
17 #include <visp3/vision/vpPose.h>
18 
19 
20 #include <visp_ros/BlobTracker.h>
21 
22 namespace {
23 class BlobTracker {
24  enum STATE_T {
25  START,
26  INIT,
27  TRACK,
28  END,
29  QUIT
30  };
31 
32  ros::NodeHandle m_nh;
33  image_transport::ImageTransport m_it;
34  image_transport::Subscriber m_image_sub;
35  ros::Publisher m_tracker_publisher;
36  ros::Publisher m_status_publisher;
37  unsigned int m_queue_size;
38  STATE_T m_state;
39  boost::mutex m_lock;
40 #ifdef VISP_HAVE_X11
41  vpDisplayX *m_display;
42 #elif defined(VISP_HAVE_GDI)
43  vpDisplayGDI *m_display;
44 #elif defined(VISP_HAVE_OPENCV)
45  vpDisplayOpenCV *m_display;
46 #endif
47  vpImage<unsigned char> m_I_grayscale;
48  std::vector<vpDot2> m_blob;
49  unsigned int m_nblobs;
50  double m_square_size;
51  vpCameraParameters m_cam;
52  std::vector<vpPoint> m_points_3d;
53  std::vector<vpImagePoint> m_points_2d;
54  vpHomogeneousMatrix m_cMo;
55  unsigned int m_thickness;
56  double m_cam_px;
57  double m_cam_py;
58  double m_cam_u0;
59  double m_cam_v0;
60  double m_cam_kud;
61  double m_cam_kdu;
62 
63 public:
64  BlobTracker() : m_it(m_nh), m_image_sub(), m_tracker_publisher(), m_status_publisher(), m_queue_size(1), m_state(START),
65  m_lock(), m_display(NULL), m_I_grayscale(), m_blob(), m_nblobs(4), m_square_size(0.12), m_cam(), m_points_3d(), m_points_2d(), m_cMo(), m_thickness(2),
66  m_cam_px(-1), m_cam_py(-1), m_cam_u0(-1), m_cam_v0(-1), m_cam_kud(-1), m_cam_kdu(-1)
67  {
68  m_image_sub = m_it.subscribe("/camera/image_raw", m_queue_size, &BlobTracker::callback, this);
69  m_tracker_publisher = m_nh.advertise<visp_ros::BlobTracker>("blob_tracker/data", m_queue_size);
70  m_status_publisher = m_nh.advertise<std_msgs::Int8>("blob_tracker/status", m_queue_size);
71 
72  m_blob.resize(m_nblobs);
73  m_points_3d.resize(m_nblobs);
74  m_points_2d.resize(m_nblobs);
75 
76  // Load object size
77  m_nh.getParam("square_size", m_square_size);
78 
79  // Load camera parameters
80  m_nh.getParam("cam_px", m_cam_px);
81  m_nh.getParam("cam_py", m_cam_py);
82  m_nh.getParam("cam_u0", m_cam_u0);
83  m_nh.getParam("cam_v0", m_cam_v0);
84  m_nh.getParam("cam_kud", m_cam_kud);
85  m_nh.getParam("cam_kdu", m_cam_kdu);
86 
87  if (m_cam_px < 0 || m_cam_py < 0 || m_cam_u0 < 0 || m_cam_v0 < 0) {
88  ROS_ERROR("Camera parameters are not set");
89  }
90 
91  if (m_cam_kud == -1 || m_cam_kdu == -1) {
92  m_cam.initPersProjWithoutDistortion(m_cam_px, m_cam_py, m_cam_u0, m_cam_v0);
93  }
94  else {
95  m_cam.initPersProjWithDistortion(m_cam_px, m_cam_py, m_cam_u0, m_cam_v0, m_cam_kud, m_cam_kdu);
96  }
97 
98  // Construct 3D model
99  //
100  // pt0 pt1
101  // x x
102  //
103  // --------------> x
104  // |
105  // pt3 | pt2
106  // x | x
107  // |
108  // \/ y
109 
110  m_points_3d[0] = vpPoint(-m_square_size / 2., -m_square_size / 2., 0); // top/left
111  m_points_3d[1] = vpPoint( m_square_size / 2., -m_square_size / 2., 0); // top/right
112  m_points_3d[2] = vpPoint( m_square_size / 2., m_square_size / 2., 0); // bottom/right
113  m_points_3d[3] = vpPoint(-m_square_size / 2., m_square_size / 2., 0); // bottom/left
114  }
115 
116  void spin() {
117  ros::Rate loop_rate(60);
118  bool quit = false;
119  while (m_nh.ok() && ! quit) {
120  {
121  boost::mutex::scoped_lock(m_lock);
122  switch (m_state) {
123  case START:
124  break;
125 
126  case INIT:
127  break;
128 
129  case TRACK:
130  break;
131 
132  case END:
133  break;
134 
135  case QUIT:
136  quit = true;
137  break;
138 
139  default:
140  break;
141  }
142  }
143 
144  ros::spinOnce();
145  loop_rate.sleep();
146  }
147  if (m_display) {
148  delete m_display;
149  m_display = NULL;
150  }
151  std::cout << "Image processing stopped..." << std::endl;
152  }
153 
154  void init_display () {
155 #ifdef VISP_HAVE_X11
156  m_display = new vpDisplayX;
157 #elif VISP_HAVE_GDI
158  m_display = new vpDisplayGDI;
159 #elif VISP_HAVE_OPENCV
160  m_display = new vpDisplayOpenCV;
161 #endif
162  if (m_display) {
163  std::cout << "Image size: " << m_I_grayscale.getWidth() << " x " << m_I_grayscale.getHeight() << std::endl;
164  std::cout << "Camera parameters:\n" << m_cam << std::endl;
165  m_display->init(m_I_grayscale, 80, m_I_grayscale.getHeight()+20, "Image processing");
166  }
167  }
168 
169  void display(const sensor_msgs::ImageConstPtr& msg) {
170  try {
171  cv::Mat cv_frame = cv_bridge::toCvShare(msg, "bgr8")->image;
172  vpImageConvert::convert(cv_frame, m_I_grayscale);
173 
174  if (m_display == NULL) {
175  init_display();
176  }
177 
178  vpDisplay::display(m_I_grayscale);
179  vpDisplay::displayText(m_I_grayscale, 20, 20, "Click to start initialisation", vpColor::red);
180  vpDisplay::flush(m_I_grayscale);
181 
182  if (vpDisplay::getClick(m_I_grayscale, false)) {
183  m_state = INIT;
184  }
185  } catch (cv_bridge::Exception& e) {
186  ROS_ERROR("cv_bridge exception: %s", e.what());
187  return;
188  }
189  }
190 
191  void callback(const sensor_msgs::ImageConstPtr& msg) {
192  boost::mutex::scoped_lock(m_lock);
193 
194  switch (m_state) {
195  case START:
196  {
197  std_msgs::Int8 status_msg;
198  status_msg.data = 0;
199  m_status_publisher.publish(status_msg);
200 
201  display(msg);
202  break;
203  }
204 
205  case INIT:
206  {
207  std_msgs::Int8 status_msg;
208  status_msg.data = 0;
209  m_status_publisher.publish(status_msg);
210 
211  cv::Mat cv_frame = cv_bridge::toCvShare(msg, "bgr8")->image;
212  vpImageConvert::convert(cv_frame, m_I_grayscale);
213 
214  vpDisplay::display(m_I_grayscale);
215  vpDisplay::displayText(m_I_grayscale, 20, 20, "Click in the 4 blobs clockwise to initialise the tracker", vpColor::red);
216  vpDisplay::flush(m_I_grayscale);
217  try {
218  for (unsigned int i = 0; i < m_blob.size(); i++) {
219  m_blob[i].setGraphics(true);
220  m_blob[i].setGraphicsThickness(m_thickness);
221  m_blob[i].initTracking(m_I_grayscale);
222  m_blob[i].track(m_I_grayscale);
223  m_points_2d[i] = m_blob[i].getCog();
224 
225  std::stringstream ss;
226  ss << i;
227  vpDisplay::displayText(m_I_grayscale, m_blob[i].getCog() + vpImagePoint(-20, 20), ss.str(), vpColor::red);
228  vpDisplay::flush(m_I_grayscale);
229  }
230  compute_pose(m_points_3d, m_points_2d, m_cam, true, m_cMo);
231 
232  m_state = TRACK;
233  vpDisplay::flush(m_I_grayscale);
234  } catch (...) {
235  std::cout << "Tracking failed during initialisation" << std::endl;
236  m_state = START;
237  }
238 
239  break;
240  }
241 
242  case TRACK:
243  {
244  cv::Mat cv_frame = cv_bridge::toCvShare(msg, "bgr8")->image;
245  vpImageConvert::convert(cv_frame, m_I_grayscale);
246 
247  vpDisplay::display(m_I_grayscale);
248  vpDisplay::displayText(m_I_grayscale, 20, 20, "Tracking in progress", vpColor::red);
249  vpDisplay::displayText(m_I_grayscale, 40, 20, "Right click to quit", vpColor::red);
250 
251  visp_ros::BlobTracker blob_tracker_msg;
252  try {
253  for (unsigned int i = 0; i < m_blob.size(); i++) {
254  m_blob[i].track(m_I_grayscale);
255  m_points_2d[i] = m_blob[i].getCog();
256  std::stringstream ss;
257  ss << i;
258  vpDisplay::displayText(m_I_grayscale, m_blob[i].getCog() + vpImagePoint(-20, 20), ss.str(), vpColor::red);
259  }
260  compute_pose(m_points_3d, m_points_2d, m_cam, false, m_cMo);
261  vpDisplay::displayFrame(m_I_grayscale, m_cMo, m_cam, m_square_size / 2., vpColor::none, m_thickness);
262 
263  // Publish tracker results
264  ros::Time now = ros::Time::now();
265  geometry_msgs::PoseStamped msg_pose;
266  msg_pose.header.stamp = now;
267  msg_pose.header.frame_id = msg->header.frame_id;
268  msg_pose.pose = visp_bridge::toGeometryMsgsPose(m_cMo); //convert
269  blob_tracker_msg.pose_stamped = msg_pose;
271  cv_ptr->toImageMsg(blob_tracker_msg.image);
272 
273  for (unsigned int i = 0; i < m_blob.size(); i++) {
274  visp_ros::ImagePoint ip;
275  ip.i = m_blob[i].getCog().get_i();
276  ip.j = m_blob[i].getCog().get_j();
277  blob_tracker_msg.blob_cogs.push_back(ip);
278 
279  visp_ros::ProjectedPoint pp;
280  vpPixelMeterConversion::convertPoint(m_cam, m_blob[i].getCog(), pp.x, pp.y);
281  blob_tracker_msg.blob_proj.push_back(pp);
282  }
283  vpDisplay::flush(m_I_grayscale);
284 
285  std_msgs::Int8 status_msg;
286  status_msg.data = 1;
287 
288  vpMouseButton::vpMouseButtonType button;
289  if (vpDisplay::getClick(m_I_grayscale, button, false)) {
290  if (button == vpMouseButton::button3) {
291  m_state = END;
292  status_msg.data = 2; // To stop visual servo controller
293  }
294  }
295 
296  m_tracker_publisher.publish(blob_tracker_msg);
297  m_status_publisher.publish(status_msg);
298  } catch (...) {
299  std::cout << "Tracking failed" << std::endl;
300  m_state = START;
301 
302  std_msgs::Int8 status_msg;
303  status_msg.data = 0;
304  m_status_publisher.publish(status_msg);
305  }
306 
307  break;
308  }
309 
310  case END:
311  {
312  std_msgs::Int8 status_msg;
313  status_msg.data = 0;
314  m_status_publisher.publish(status_msg);
315 
316  if (m_display) {
317  delete m_display;
318  m_display = NULL;
319  }
320  m_state = QUIT;
321 
322  break;
323  }
324 
325  default:
326  break;
327  }
328  }
329 
330  double compute_pose(std::vector<vpPoint> &points_3d, const std::vector<vpImagePoint> &points_2d, const vpCameraParameters &cam,
331  bool init, vpHomogeneousMatrix &cMo)
332  {
333  vpPose pose;
334  double x = 0, y = 0;
335  for (unsigned int i = 0; i < points_3d.size(); i++) {
336  vpPixelMeterConversion::convertPoint(cam, points_2d[i], x, y);
337  points_3d[i].set_x(x);
338  points_3d[i].set_y(y);
339  pose.addPoint(points_3d[i]);
340  }
341 
342  if (init == true) {
343  vpHomogeneousMatrix cMo_dem;
344  vpHomogeneousMatrix cMo_lag;
345  double residual_dem = std::numeric_limits<double>::max();
346  double residual_lag = std::numeric_limits<double>::max();
347 
348  try {
349  pose.computePose(vpPose::DEMENTHON, cMo_dem);
350  residual_dem = pose.computeResidual(cMo_dem);
351  } catch (...) {
352  residual_dem = std::numeric_limits<double>::max();
353  }
354  try {
355  pose.computePose(vpPose::LAGRANGE, cMo_lag);
356  residual_lag = pose.computeResidual(cMo_lag);
357  } catch (...) {
358  residual_lag = std::numeric_limits<double>::max();
359  }
360  if (residual_dem < residual_lag)
361  cMo = cMo_dem;
362  else
363  cMo = cMo_lag;
364  }
365  pose.computePose(vpPose::VIRTUAL_VS, cMo);
366  double residual = pose.computeResidual(cMo);
367 
368  return residual;
369  }
370 };
371 }
372 
373 int main(int argc, char** argv) {
374  ros::init(argc, argv, "visp_ros_blob_tracker_node");
375  BlobTracker blobTracker;
376  blobTracker.spin();
377  return 0;
378 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
geometry_msgs::Pose toGeometryMsgsPose(const vpHomogeneousMatrix &mat)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool getParam(const std::string &key, std::string &s) const
static Time now()
bool ok() const
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais
autogenerated on Fri Jul 3 2020 03:41:43