Fawkes API  Fawkes Development Version
bumblebee2_thread.cpp
1 
2 /***************************************************************************
3  * bumblebee2_thread.cpp - Acquire data from Bumblebee2 stereo camera
4  *
5  * Created: Wed Jul 17 13:17:27 2013
6  * Copyright 2013 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "bumblebee2_thread.h"
23 
24 #include <pcl_utils/utils.h>
25 #include <pcl_utils/comparisons.h>
26 #include <utils/time/wait.h>
27 #include <utils/math/angle.h>
28 #include <fvcams/bumblebee2.h>
29 #include <fvutils/ipc/shm_image.h>
30 #include <fvutils/color/conversions.h>
31 #ifdef USE_TIMETRACKER
32 # include <utils/time/tracker.h>
33 #endif
34 #include <utils/time/tracker_macros.h>
35 
36 #include <interfaces/SwitchInterface.h>
37 #include <interfaces/OpenCVStereoParamsInterface.h>
38 
39 #include <triclops.h>
40 #include <opencv2/core/core.hpp>
41 #include <opencv2/calib3d/calib3d.hpp>
42 
43 using namespace std;
44 using namespace firevision;
45 using namespace fawkes;
46 
47 #define CFG_PREFIX "/bumblebee2/"
48 #define CFG_OPENCV_PREFIX CFG_PREFIX"opencv-stereo/"
49 
50 /** @class Bumblebee2Thread "bumblebee2_thread.h"
51  * Thread to acquire data from Bumblebee2 stereo camera.
52  * @author Tim Niemueller
53  */
54 
55 using namespace fawkes;
56 
57 /// @cond INTERNALS
58 /** Data internal to Triclops stereo processor
59  * This class exists to be able to hide the triclops stuff from the camera
60  * user and not to expose the Triclops SDK headers.
61  */
62 class TriclopsData
63 {
64  public:
65  TriclopsContext context;
66  TriclopsInput input;
67  TriclopsImage rectified_image;
68  TriclopsImage16 disparity_image_hires;
69  TriclopsImage disparity_image_lores;
70  TriclopsImage3d * image_3d;
71 };
72 /// @endcond
73 
74 
75 /** Constructor. */
77  : Thread("Bumblebee2Thread", Thread::OPMODE_WAITFORWAKEUP),
78  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
79  TransformAspect(TransformAspect::ONLY_PUBLISHER, "Bumblebee2")
80 {
81 }
82 
83 
84 /** Destructor. */
86 {
87 }
88 
89 
90 void
92 {
93  // prepare for finalize in the case init fails somewhere
94  bb2_ = NULL;
95  cv_disparity_ = NULL;
96  tf_left_ = tf_right_ = NULL;
97  switch_if_ = NULL;
98  params_if_ = NULL;
99  shm_img_rgb_right_ = shm_img_rgb_left_ = shm_img_yuv_right_ = shm_img_yuv_left_ = NULL;
100  shm_img_rectified_right_ = shm_img_rectified_left_ = NULL;
101  shm_img_prefiltered_right_ = shm_img_prefiltered_left_ = NULL;
102  shm_img_rgb_rect_left_ = shm_img_rgb_rect_right_ = shm_img_disparity_ = NULL;
103  triclops_ = NULL;
104 #ifdef USE_TIMETRACKER
105  tt_ = NULL;
106 #endif
107 
108  // get config data
109  cfg_base_frame_ = config->get_string(CFG_PREFIX"base-frame");
110  cfg_frames_prefix_ = config->get_string(CFG_PREFIX"frames-prefix");
111  cfg_frames_interval_ = config->get_float(CFG_PREFIX"frames-interval");
112 
113  std::string stereo_matcher = config->get_string(CFG_PREFIX"stereo-matcher");
114  if (stereo_matcher == "opencv") {
115  cfg_stereo_matcher_ = STEREO_MATCHER_OPENCV;
116  } else if (stereo_matcher == "triclops") {
117  cfg_stereo_matcher_ = STEREO_MATCHER_TRICLOPS;
118  } else {
119  throw Exception("Unknown stereo matcher %s", stereo_matcher.c_str());
120  }
121 
122  // Open camera
123  bb2_ = NULL;
124  try {
125  bb2_ = new Bumblebee2Camera();
126  bb2_->open();
127  bb2_->start();
128  bb2_->set_image_number(Bumblebee2Camera::RGB_IMAGE);
129  } catch (Exception &e) {
130  finalize();
131  throw;
132  }
133 
134  // Open blackboard interfaces
135  try {
136  switch_if_ = blackboard->open_for_writing<SwitchInterface>("bumblebee2");
137  switch_if_->set_enabled(true);
138  switch_if_->write();
139  } catch (Exception &e) {
140  finalize();
141  throw;
142  }
143 
144  // allocate buffers
145  width_ = bb2_->pixel_width();
146  height_ = bb2_->pixel_height();
147 
148  buffer_rgb_ = bb2_->buffer();
149  buffer_rgb_right_ = buffer_rgb_;
150  buffer_rgb_left_ = buffer_rgb_right_ + colorspace_buffer_size(RGB, width_, height_);
151  buffer_green_ = (unsigned char *)malloc(width_ * height_ * 2);
152  buffer_yuv_right_ = malloc_buffer(YUV422_PLANAR, width_, height_);
153  buffer_yuv_left_ = malloc_buffer(YUV422_PLANAR, width_, height_);
154  buffer_rgb_planar_right_ = malloc_buffer(RGB_PLANAR, width_, height_);
155  buffer_rgb_planar_left_ = malloc_buffer(RGB_PLANAR, width_, height_);
156 
157  triclops_ = new TriclopsData();
158  // Always the same
159  triclops_->input.inputType = TriInp_RGB;
160  triclops_->input.nrows = height_;
161  triclops_->input.ncols = width_;
162  triclops_->input.rowinc = triclops_->input.ncols;
163  /*
164  triclops_->input.u.rgb.red = buffer_yuv_right;
165  triclops_->input.u.rgb.green = buffer_yuv_left;
166  triclops_->input.u.rgb.blue = buffer_yuv_left;
167  */
168  triclops_->input.u.rgb.red = buffer_green_;
169  triclops_->input.u.rgb.green = buffer_green_ + width_ * height_;
170  triclops_->input.u.rgb.blue = triclops_->input.u.rgb.green;
171 
172  try {
173  get_triclops_context_from_camera();
174  } catch (Exception &e) {
175  finalize();
176  throw;
177  }
178 
179  // We always need this for rectification
180  triclopsSetNumberOfROIs(triclops_->context, 0);
181  triclopsSetResolutionAndPrepare(triclops_->context,
182  height_, width_, height_, width_);
183 
184 
185  TriclopsError err;
186 
187  err = triclopsGetImageCenter(triclops_->context, &center_row_, &center_col_);
188  if (err != TriclopsErrorOk) {
189  finalize();
190  throw Exception("Failed to get image center: %s", triclopsErrorToString(err));
191  }
192 
193  err = triclopsGetFocalLength(triclops_->context, &focal_length_);
194  if (err != TriclopsErrorOk) {
195  finalize();
196  throw Exception("Failed to get focal length: %s", triclopsErrorToString(err));
197  }
198 
199  err = triclopsGetBaseline(triclops_->context, &baseline_);
200  if (err != TriclopsErrorOk) {
201  finalize();
202  throw Exception("Failed to get baseline: %s", triclopsErrorToString(err));
203  }
204 
205  std::string stereo_frame = cfg_frames_prefix_;
206 
207  if (cfg_stereo_matcher_ == STEREO_MATCHER_TRICLOPS) {
208  triclopsCreateImage3d(triclops_->context, &(triclops_->image_3d));
209 
210  // Set defaults
211  triclopsSetSubpixelInterpolation(triclops_->context, 1);
212 
213  triclopsSetEdgeCorrelation(triclops_->context, 1);
214  triclopsSetLowpass(triclops_->context, 1);
215  triclopsSetDisparity(triclops_->context, 5, 100);
216  triclopsSetEdgeMask(triclops_->context, 11);
217  triclopsSetStereoMask(triclops_->context, 23);
218  triclopsSetSurfaceValidation(triclops_->context, 1);
219  triclopsSetTextureValidation(triclops_->context, 0);
220 
221  disparity_scale_factor_ = 1.0;
222 
223  stereo_frame += "right";
224 
225  } else if (cfg_stereo_matcher_ == STEREO_MATCHER_OPENCV) {
226  // *** Read config values
227  // pre-filtering (normalization of input images)
228  try {
229  std::string algorithm = config->get_string(CFG_OPENCV_PREFIX"algorithm");
230  if (algorithm == "bm") {
231  cfg_opencv_stereo_algorithm_ = OPENCV_STEREO_BM;
232  } else if (algorithm == "sgbm") {
233  cfg_opencv_stereo_algorithm_ = OPENCV_STEREO_SGBM;
234  } else {
235  finalize();
236  throw Exception("Unknown stereo algorithm '%s', use bm or sgbm", algorithm.c_str());
237  }
238 
239  std::string pre_filter_type = config->get_string(CFG_OPENCV_PREFIX"pre-filter-type");
240  cfg_bm_pre_filter_size_ = config->get_uint(CFG_OPENCV_PREFIX"pre-filter-size");
241  cfg_bm_pre_filter_cap_ = config->get_uint(CFG_OPENCV_PREFIX"pre-filter-cap");
242 
243  // correspondence using Sum of Absolute Difference (SAD)
244  cfg_bm_sad_window_size_ = config->get_uint(CFG_OPENCV_PREFIX"sad-window-size");
245  cfg_bm_min_disparity_ = config->get_int(CFG_OPENCV_PREFIX"min-disparity");
246  cfg_bm_num_disparities_ = config->get_uint(CFG_OPENCV_PREFIX"num-disparities");
247 
248  // post-filtering
249  cfg_bm_texture_threshold_ = config->get_uint(CFG_OPENCV_PREFIX"texture-threshold");
250  cfg_bm_uniqueness_ratio_ = config->get_uint(CFG_OPENCV_PREFIX"uniqueness-ratio");
251  cfg_bm_speckle_window_size_ = config->get_uint(CFG_OPENCV_PREFIX"speckle-window-size");
252  cfg_bm_speckle_range_ = config->get_uint(CFG_OPENCV_PREFIX"speckle-range");
253 
254  cfg_bm_try_smaller_windows_ = config->get_bool(CFG_OPENCV_PREFIX"try-smaller-windows");
255 
256  // SGBM specific values
257  std::string sgbm_p1 = config->get_string(CFG_OPENCV_PREFIX"sgbm-p1");
258  cfg_sgbm_p1_auto_ = (sgbm_p1 == "auto");
259  if (! cfg_sgbm_p1_auto_) {
260  cfg_sgbm_p1_ = config->get_int(CFG_OPENCV_PREFIX"sgbm-p1");
261  }
262  std::string sgbm_p2 = config->get_string(CFG_OPENCV_PREFIX"sgbm-p2");
263  cfg_sgbm_p2_auto_ = (sgbm_p2 == "auto");
264  if (! cfg_sgbm_p2_auto_) {
265  cfg_sgbm_p2_ = config->get_int(CFG_OPENCV_PREFIX"sgbm-p1");
266  }
267  cfg_sgbm_disp_12_max_diff_ = config->get_int(CFG_OPENCV_PREFIX"sgbm-disp12-max-diff");
268 
269  // *** check config values
270  if (pre_filter_type == "normalized_response") {
271  cfg_bm_pre_filter_type_ = CV_STEREO_BM_NORMALIZED_RESPONSE;
272  } else if (pre_filter_type == "xsobel") {
273  cfg_bm_pre_filter_type_ = CV_STEREO_BM_XSOBEL;
274  } else {
275  throw Exception("Invalid OpenCV stereo matcher pre filter type");
276  }
277 
278  if (cfg_bm_pre_filter_size_ < 5 || cfg_bm_pre_filter_size_ > 255 ||
279  cfg_bm_pre_filter_size_ % 2 == 0)
280  {
281  throw Exception("Pre filter size must be odd and be within 5..255");
282  }
283 
284  if( cfg_bm_pre_filter_cap_ < 1 || cfg_bm_pre_filter_cap_ > 63 ) {
285  throw Exception("Pre filter cap must be within 1..63");
286  }
287 
288  if( cfg_bm_sad_window_size_ < 5 || cfg_bm_sad_window_size_ > 255 ||
289  cfg_bm_sad_window_size_ % 2 == 0 ||
290  cfg_bm_sad_window_size_ >= std::min(width_, height_))
291  {
292  throw Exception("SAD window size must be odd, be within 5..255 and "
293  "be no larger than image width or height");
294  }
295 
296  if( cfg_bm_num_disparities_ <= 0 || cfg_bm_num_disparities_ % 16 != 0 ) {
297  throw Exception("Number of disparities must be positive and divisble by 16");
298  }
299  } catch (Exception &e) {
300  finalize();
301  throw;
302  }
303 
304  int max_disparity =
305  std::max((unsigned int)16, cfg_bm_min_disparity_ + cfg_bm_num_disparities_);
306  logger->log_debug(name(), "Min Z: %fm Max Z: %f",
307  focal_length_ * baseline_ / max_disparity,
308  (cfg_bm_min_disparity_ == 0)
309  ? std::numeric_limits<float>::infinity()
310  : focal_length_ * baseline_ / cfg_bm_min_disparity_);
311 
312  try {
313  params_if_ = blackboard->open_for_writing<OpenCVStereoParamsInterface>("bumblebee2");
314  switch (cfg_bm_pre_filter_type_) {
315  case CV_STEREO_BM_NORMALIZED_RESPONSE:
316  params_if_->set_pre_filter_type(OpenCVStereoParamsInterface::PFT_NORMALIZED_RESPONSE);
317  break;
318  case CV_STEREO_BM_XSOBEL:
319  params_if_->set_pre_filter_type(OpenCVStereoParamsInterface::PFT_XSOBEL);
320  break;
321  default:
322  throw Exception("No valid pre filter type set");
323  }
324  params_if_->set_pre_filter_size(cfg_bm_pre_filter_size_);
325  params_if_->set_pre_filter_cap(cfg_bm_pre_filter_cap_);
326  params_if_->set_sad_window_size(cfg_bm_sad_window_size_);
327  params_if_->set_min_disparity(cfg_bm_min_disparity_);
328  params_if_->set_num_disparities(cfg_bm_num_disparities_);
329  params_if_->set_texture_threshold(cfg_bm_texture_threshold_);
330  params_if_->set_uniqueness_ratio(cfg_bm_uniqueness_ratio_);
331  params_if_->set_speckle_window_size(cfg_bm_speckle_window_size_);
332  params_if_->set_speckle_range(cfg_bm_speckle_range_);
333  params_if_->set_try_smaller_windows(cfg_bm_try_smaller_windows_);
334  params_if_->write();
335  } catch (Exception &e) {
336  finalize();
337  throw;
338  }
339 
340  // We don't need this when using OpenCV
341  triclopsSetEdgeCorrelation(triclops_->context, 0);
342  triclopsSetLowpass(triclops_->context, 0);
343 
344  cv_disparity_ = new cv::Mat(height_, width_, CV_16SC1);
345  // OpenCV disparity data is scaled by factor 16, always
346  disparity_scale_factor_ = 1.f / 16.f;
347 
348  stereo_frame += "left";
349  }
350 
351  try {
352  pcl_xyz_ = new pcl::PointCloud<pcl::PointXYZ>();
353  pcl_xyz_->is_dense = false;
354  pcl_xyz_->width = width_;
355  pcl_xyz_->height = height_;
356  pcl_xyz_->points.resize(width_ * height_);
357  pcl_xyz_->header.frame_id = stereo_frame;
358 
359  pcl_xyzrgb_ = new pcl::PointCloud<pcl::PointXYZRGB>();
360  pcl_xyzrgb_->is_dense = false;
361  pcl_xyzrgb_->width = width_;
362  pcl_xyzrgb_->height = height_;
363  pcl_xyzrgb_->points.resize(width_ * height_);
364  pcl_xyzrgb_->header.frame_id = stereo_frame;
365 
366  pcl_manager->add_pointcloud("bumblebee2-xyz", pcl_xyz_);
367  pcl_manager->add_pointcloud("bumblebee2-xyzrgb", pcl_xyzrgb_);
368 
369  shm_img_rgb_right_ =
370  new SharedMemoryImageBuffer("bumblebee2-rgb-right", RGB, width_, height_);
371  shm_img_rgb_left_ =
372  new SharedMemoryImageBuffer("bumblebee2-rgb-left", RGB, width_, height_);
373  shm_img_yuv_right_ =
374  new SharedMemoryImageBuffer("bumblebee2-yuv-right", YUV422_PLANAR, width_, height_);
375  shm_img_yuv_left_ =
376  new SharedMemoryImageBuffer("bumblebee2-yuv-left", YUV422_PLANAR, width_, height_);
377  shm_img_rgb_rect_right_ =
378  new SharedMemoryImageBuffer("bumblebee2-rgb-rectified-right", RGB_PLANAR, width_, height_);
379  shm_img_rgb_rect_left_ =
380  new SharedMemoryImageBuffer("bumblebee2-rgb-rectified-left", RGB_PLANAR, width_, height_);
381  shm_img_rectified_right_ =
382  new SharedMemoryImageBuffer("bumblebee2-rectified-right", MONO8, width_, height_);
383  shm_img_rectified_left_ =
384  new SharedMemoryImageBuffer("bumblebee2-rectified-left", MONO8, width_, height_);
385  shm_img_prefiltered_right_ =
386  new SharedMemoryImageBuffer("bumblebee2-prefiltered-right", MONO8, width_, height_);
387  shm_img_prefiltered_left_ =
388  new SharedMemoryImageBuffer("bumblebee2-prefiltered-left", MONO8, width_, height_);
389  shm_img_disparity_ =
390  new SharedMemoryImageBuffer("bumblebee2-disparity", MONO8, width_, height_);
391 
392  tf_last_publish_ = new fawkes::Time(clock);
393  fawkes::Time now(clock);
394  tf::Quaternion q(-M_PI/2.f, 0, -M_PI/2.f);
395  tf::Transform t_left(q, tf::Vector3(0.0, 0.06, 0.018));
396  tf::Transform t_right(q, tf::Vector3(0.0, -0.06, 0.018));
397 
398  tf_left_ =
399  new tf::StampedTransform(t_left, now, cfg_base_frame_, cfg_frames_prefix_ + "left");
400  tf_right_ =
401  new tf::StampedTransform(t_right, now, cfg_base_frame_, cfg_frames_prefix_ + "right");
402  } catch (Exception &e) {
403  finalize();
404  throw;
405  }
406 
407 #ifdef USE_TIMETRACKER
408  tt_ = new TimeTracker();
409  tt_loopcount_ = 0;
410  ttc_full_loop_ = tt_->add_class("Full Loop");
411  ttc_transforms_ = tt_->add_class("Transforms");
412  ttc_msgproc_ = tt_->add_class("Message Processing");
413  ttc_capture_ = tt_->add_class("Capture");
414  ttc_preprocess_ = tt_->add_class("Pre-processing");
415  ttc_rectify_ = tt_->add_class("Rectification");
416  ttc_stereo_match_ = tt_->add_class("Stereo Match");
417  ttc_pcl_xyzrgb_ = tt_->add_class("PCL XYZRGB");
418  ttc_pcl_xyz_ = tt_->add_class("PCL XYZ");
419 #endif
420 }
421 
422 
423 /** Get Triclops context.
424  * This retrieves calibration information from the camera and stores it into a
425  * temporary file. With this file the Triclops context is initialized. Afterwards
426  * the temporary file is removed.
427  */
428 void
429 Bumblebee2Thread::get_triclops_context_from_camera()
430 {
431  char *tmpname = (char *)malloc(strlen("triclops_cal_XXXXXX") + 1);
432  strcpy(tmpname, "triclops_cal_XXXXXX");
433  char *tmpfile = mktemp(tmpname);
435 
436  TriclopsError err = triclopsGetDefaultContextFromFile(&(triclops_->context), tmpfile);
437  if (err != TriclopsErrorOk) {
438  free(tmpfile);
439  throw Exception("Fetching Triclops context from camera failed");
440  }
441  unlink(tmpfile);
442  free(tmpfile);
443 }
444 
445 /** Deinterlace green buffer.
446  * Method used in stereo processing. Following the PTGrey example, seems useless
447  * if we have YUV planar and thus grey images anyway.
448  * @param src source buffer
449  * @param dest destination buffer
450  * @param width width of the image
451  * @param height height of the image
452  */
453 void
454 Bumblebee2Thread::deinterlace_green(unsigned char* src, unsigned char* dest,
455  unsigned int width, unsigned int height)
456 {
457  register int i = (width*height)-2;
458  register int g = ((width*height)/3)-1;
459 
460  while (i >= 0) {
461  dest[g--] = src[i-=3];
462  }
463 }
464 
465 void
467 {
468  delete tf_left_;
469  delete tf_right_;
470 
471  pcl_manager->remove_pointcloud("bumblebee2-xyz");
472  pcl_manager->remove_pointcloud("bumblebee2-xyzrgb");
473  pcl_xyz_.reset();
474  pcl_xyzrgb_.reset();
475 
476  blackboard->close(switch_if_);
477  blackboard->close(params_if_);
478 
479  delete shm_img_rgb_right_;
480  delete shm_img_rgb_left_;
481  delete shm_img_yuv_right_;
482  delete shm_img_yuv_left_;
483  delete shm_img_rectified_right_;
484  delete shm_img_rectified_left_;
485  delete shm_img_prefiltered_right_;
486  delete shm_img_prefiltered_left_;
487  delete shm_img_rgb_rect_left_;
488  delete shm_img_rgb_rect_right_;
489  delete shm_img_disparity_;
490 
491  delete triclops_;
492  delete cv_disparity_;
493 
494  if (buffer_green_) free(buffer_green_);
495  if (buffer_yuv_right_) free(buffer_yuv_right_);
496  if (buffer_yuv_left_) free(buffer_yuv_left_);
497  if (buffer_rgb_planar_right_) free(buffer_rgb_planar_right_);
498  if (buffer_rgb_planar_left_) free(buffer_rgb_planar_left_);
499  if (bb2_) {
500  try {
501  bb2_->stop();
502  bb2_->close();
503  } catch (Exception &e) {
504  logger->log_warn(name(), "Stopping camera failed, exception follows");
505  logger->log_warn(name(), e);
506  }
507  delete bb2_;
508  }
509 
510 #ifdef USE_TIMETRACKER
511  delete tt_;
512 #endif
513 }
514 
515 
516 void
518 {
519  TIMETRACK_START(ttc_full_loop_);
520 
521  fawkes::Time now(clock);
522  if ((now - tf_last_publish_) > cfg_frames_interval_) {
523  TIMETRACK_START(ttc_transforms_);
524  tf_last_publish_->stamp();
525 
526  // date time stamps slightly into the future so they are valid
527  // for longer and need less frequent updates.
528  fawkes::Time timestamp = now + (cfg_frames_interval_ * 1.1);
529 
530  tf_left_->stamp = timestamp;
531  tf_right_->stamp = timestamp;
532 
533  tf_publisher->send_transform(*tf_left_);
534  tf_publisher->send_transform(*tf_right_);
535  TIMETRACK_END(ttc_transforms_);
536  }
537 
538  bool uses_triclops = (cfg_stereo_matcher_ == STEREO_MATCHER_TRICLOPS);
539  bool uses_opencv = (cfg_stereo_matcher_ == STEREO_MATCHER_OPENCV);
540 
541 
542  TIMETRACK_START(ttc_msgproc_);
543 
544  if (uses_opencv) {
545  while (! params_if_->msgq_empty()) {
547  params_if_->msgq_first_safe(msg))
548  {
549  switch (msg->pre_filter_type()) {
550  case OpenCVStereoParamsInterface::PFT_NORMALIZED_RESPONSE:
551  cfg_bm_pre_filter_type_ = CV_STEREO_BM_NORMALIZED_RESPONSE;
552  break;
553  case OpenCVStereoParamsInterface::PFT_XSOBEL:
554  cfg_bm_pre_filter_type_ = CV_STEREO_BM_XSOBEL;
555  break;
556  }
557  params_if_->set_pre_filter_type(msg->pre_filter_type());
558  params_if_->write();
560  params_if_->msgq_first_safe(msg))
561  {
562  cfg_bm_pre_filter_size_ = msg->pre_filter_size();
563  params_if_->set_pre_filter_size(cfg_bm_pre_filter_size_);
564  params_if_->write();
566  params_if_->msgq_first_safe(msg))
567  {
568  cfg_bm_pre_filter_cap_ = msg->pre_filter_cap();
569  params_if_->set_pre_filter_cap(cfg_bm_pre_filter_cap_);
570  params_if_->write();
572  params_if_->msgq_first_safe(msg))
573  {
574  cfg_bm_sad_window_size_ = msg->sad_window_size();
575  params_if_->set_sad_window_size(cfg_bm_sad_window_size_);
576  params_if_->write();
578  params_if_->msgq_first_safe(msg))
579  {
580  cfg_bm_min_disparity_ = msg->min_disparity();
581  params_if_->set_min_disparity(cfg_bm_min_disparity_);
582  params_if_->write();
584  params_if_->msgq_first_safe(msg))
585  {
586  cfg_bm_num_disparities_ = msg->num_disparities();
587  params_if_->set_num_disparities(cfg_bm_num_disparities_);
588  params_if_->write();
590  params_if_->msgq_first_safe(msg))
591  {
592  cfg_bm_texture_threshold_ = msg->texture_threshold();
593  params_if_->set_texture_threshold(cfg_bm_texture_threshold_);
594  params_if_->write();
596  params_if_->msgq_first_safe(msg))
597  {
598  cfg_bm_uniqueness_ratio_ = msg->uniqueness_ratio();
599  params_if_->set_uniqueness_ratio(cfg_bm_uniqueness_ratio_);
600  params_if_->write();
602  params_if_->msgq_first_safe(msg))
603  {
604  cfg_bm_speckle_window_size_ = msg->speckle_window_size();
605  params_if_->set_speckle_window_size(cfg_bm_speckle_window_size_);
606  params_if_->write();
608  params_if_->msgq_first_safe(msg))
609  {
610  cfg_bm_speckle_range_ = msg->speckle_range();
611  params_if_->set_speckle_range(cfg_bm_speckle_range_);
612  params_if_->write();
614  params_if_->msgq_first_safe(msg))
615  {
616  cfg_bm_try_smaller_windows_ = msg->is_try_smaller_windows();
617  params_if_->set_try_smaller_windows(cfg_bm_try_smaller_windows_);
618  params_if_->write();
619  }
620 
621  params_if_->msgq_pop();
622  }
623  }
624 
625  while (! switch_if_->msgq_empty()) {
627  switch_if_->msgq_first_safe(msg))
628  {
629  switch_if_->set_enabled(true);
630  switch_if_->write();
631  } else if (SwitchInterface::DisableSwitchMessage *msg =
632  switch_if_->msgq_first_safe(msg))
633  {
634  switch_if_->set_enabled(false);
635  switch_if_->write();
636  }
637 
638  switch_if_->msgq_pop();
639  }
640  TIMETRACK_END(ttc_msgproc_);
641 
642  if (! switch_if_->is_enabled()) {
643  TIMETRACK_ABORT(ttc_full_loop_);
644  TimeWait::wait(250000);
645  return;
646  }
647 
648  TIMETRACK_START(ttc_capture_);
649 
650  // Acquire and process data
651  bb2_->capture();
652  fawkes::Time capture_ts(clock);
653 
654  TIMETRACK_INTER(ttc_capture_, ttc_preprocess_)
655 
656  bb2_->deinterlace_stereo();
657  bb2_->decode_bayer();
658 
659  // Publish RGB and YUV images if requested
660  if (shm_img_rgb_right_->num_attached() > 1) {
661  shm_img_rgb_right_->lock_for_write();
662  memcpy(shm_img_rgb_right_->buffer(), buffer_rgb_right_,
663  colorspace_buffer_size(RGB, width_, height_));
664  shm_img_rgb_right_->set_capture_time(&capture_ts);
665  shm_img_rgb_right_->unlock();
666  }
667 
668  if (shm_img_rgb_left_->num_attached() > 1) {
669  shm_img_rgb_left_->lock_for_write();
670  memcpy(shm_img_rgb_left_->buffer(), buffer_rgb_left_,
671  colorspace_buffer_size(RGB, width_, height_));
672  shm_img_rgb_left_->set_capture_time(&capture_ts);
673  shm_img_rgb_left_->unlock();
674  }
675 
676  if (shm_img_yuv_right_->num_attached() > 1) {
677  shm_img_yuv_right_->lock_for_write();
678  convert(RGB, YUV422_PLANAR, buffer_rgb_right_,
679  shm_img_yuv_right_->buffer(), width_, height_);
680  shm_img_yuv_right_->set_capture_time(&capture_ts);
681  shm_img_yuv_right_->unlock();
682  }
683 
684  if (shm_img_yuv_left_->num_attached() > 1) {
685  shm_img_yuv_left_->lock_for_write();
686  convert(RGB, YUV422_PLANAR, buffer_rgb_left_,
687  shm_img_yuv_left_->buffer(), width_, height_);
688  shm_img_yuv_left_->set_capture_time(&capture_ts);
689  shm_img_yuv_left_->unlock();
690  }
691 
692  TriclopsError err;
693 
694  // Extract green buffer and rectify image
695  deinterlace_green(buffer_rgb_, buffer_green_, width_, 6 * height_);
696 
697  TIMETRACK_INTER(ttc_preprocess_, ttc_rectify_);
698 
699  err = triclopsRectify(triclops_->context, &(triclops_->input));
700  if (err != TriclopsErrorOk) {
701  logger->log_warn(name(), "Rectifying the image failed (%s), skipping loop",
702  triclopsErrorToString(err));
703  bb2_->dispose_buffer();
704  return;
705  }
706 
707  // get rectified images and publish if requested
708  TriclopsImage image_right, image_left;
709  err = triclopsGetImage(triclops_->context, TriImg_RECTIFIED, TriCam_RIGHT, &image_right);
710  if (err != TriclopsErrorOk) {
711  logger->log_warn(name(), "Retrieving right rectified image failed (%s), skipping loop",
712  triclopsErrorToString(err));
713  bb2_->dispose_buffer();
714  return;
715  }
716  err = triclopsGetImage(triclops_->context, TriImg_RECTIFIED, TriCam_LEFT, &image_left);
717  if (err != TriclopsErrorOk) {
718  logger->log_warn(name(), "Retrieving left rectified image failed (%s), skipping loop",
719  triclopsErrorToString(err));
720  bb2_->dispose_buffer();
721  return;
722  }
723 
724  if (shm_img_rectified_right_->num_attached() > 1) {
725  shm_img_rectified_right_->lock_for_write();
726  memcpy(shm_img_rectified_right_->buffer(), image_right.data,
727  colorspace_buffer_size(MONO8, width_, height_));
728  shm_img_rectified_right_->set_capture_time(&capture_ts);
729  shm_img_rectified_right_->unlock();
730  }
731  if (shm_img_rectified_left_->num_attached() > 1) {
732  shm_img_rectified_left_->lock_for_write();
733  memcpy(shm_img_rectified_left_->buffer(), image_left.data,
734  colorspace_buffer_size(MONO8, width_, height_));
735  shm_img_rectified_left_->set_capture_time(&capture_ts);
736  shm_img_rectified_left_->unlock();
737  }
738 
739  TIMETRACK_INTER(ttc_rectify_, ttc_stereo_match_);
740 
741  // stereo correspondence matching
742  short int *dispdata = NULL;
743  if (uses_triclops) {
744  err = triclopsStereo(triclops_->context);
745  if (err != TriclopsErrorOk) {
746  logger->log_warn(name(), "Calculating the disparity image failed (%s), skipping loop",
747  triclopsErrorToString(err));
748  bb2_->dispose_buffer();
749  return;
750  }
751 
752  triclopsGetImage16(triclops_->context, TriImg16_DISPARITY, TriCam_REFERENCE,
753  &(triclops_->disparity_image_hires));
754  dispdata = (short int *)triclops_->disparity_image_hires.data;
755 
756  } else if (uses_opencv) {
757  // Get Images and wrap with OpenCV data structures
758  cv::Mat img_r(height_, width_, CV_8UC1, image_right.data);
759  cv::Mat img_l(height_, width_, CV_8UC1, image_left.data);
760 
761  // Calculate disparity
762  if (cfg_opencv_stereo_algorithm_ == OPENCV_STEREO_BM) {
763  cv::StereoBM block_matcher(cv::StereoBM::BASIC_PRESET,
764  cfg_bm_num_disparities_, cfg_bm_sad_window_size_);
765  block_matcher.state->preFilterType = cfg_bm_pre_filter_type_;
766  block_matcher.state->preFilterSize = cfg_bm_pre_filter_size_;
767  block_matcher.state->preFilterCap = cfg_bm_pre_filter_cap_;
768  block_matcher.state->minDisparity = cfg_bm_min_disparity_;
769  block_matcher.state->textureThreshold = cfg_bm_texture_threshold_;
770  block_matcher.state->uniquenessRatio = cfg_bm_uniqueness_ratio_;
771  block_matcher.state->speckleWindowSize = cfg_bm_speckle_window_size_;
772  block_matcher.state->speckleRange = cfg_bm_speckle_range_;
773  block_matcher.state->trySmallerWindows = cfg_bm_try_smaller_windows_ ? 1 : 0;
774 
775  block_matcher(img_l, img_r, *cv_disparity_);
776 
777  if (shm_img_prefiltered_right_->num_attached() > 1) {
778  shm_img_prefiltered_right_->lock_for_write();
779  memcpy(shm_img_prefiltered_right_->buffer(),
780  block_matcher.state->preFilteredImg0->data.ptr,
781  colorspace_buffer_size(MONO8, width_, height_));
782  shm_img_prefiltered_right_->set_capture_time(&capture_ts);
783  shm_img_prefiltered_right_->unlock();
784  }
785  if (shm_img_prefiltered_left_->num_attached() > 1) {
786  shm_img_prefiltered_left_->lock_for_write();
787  memcpy(shm_img_prefiltered_left_->buffer(),
788  block_matcher.state->preFilteredImg0->data.ptr,
789  colorspace_buffer_size(MONO8, width_, height_));
790  shm_img_prefiltered_left_->set_capture_time(&capture_ts);
791  shm_img_prefiltered_left_->unlock();
792  }
793  } else {
794  int cn = img_l.channels();
795 
796  cv::StereoSGBM block_matcher;
797  block_matcher.minDisparity = cfg_bm_min_disparity_;
798  block_matcher.numberOfDisparities = cfg_bm_num_disparities_;
799  block_matcher.SADWindowSize = cfg_bm_sad_window_size_;
800  block_matcher.preFilterCap = cfg_bm_pre_filter_cap_;
801  block_matcher.uniquenessRatio = cfg_bm_uniqueness_ratio_;
802  if (cfg_sgbm_p1_auto_) {
803  block_matcher.P1 = 8 * cn * block_matcher.SADWindowSize * block_matcher.SADWindowSize;
804  } else {
805  block_matcher.P1 = cfg_sgbm_p1_;
806  }
807  if (cfg_sgbm_p2_auto_) {
808  block_matcher.P2 = 32 * cn * block_matcher.SADWindowSize * block_matcher.SADWindowSize;
809  } else {
810  block_matcher.P2 = cfg_sgbm_p2_;
811  }
812  if (block_matcher.P1 >= block_matcher.P2) {
813  logger->log_warn(name(), "SGBM P1 >= P2 (%i <= %i), skipping loop",
814  block_matcher.P1, block_matcher.P2);
815  bb2_->dispose_buffer();
816  return;
817  }
818 
819  block_matcher.speckleWindowSize = cfg_bm_speckle_window_size_;
820  block_matcher.speckleRange = cfg_bm_speckle_range_;
821  block_matcher.disp12MaxDiff = 1;
822  block_matcher. fullDP = false;
823 
824  block_matcher(img_l, img_r, *cv_disparity_);
825  }
826 
827  dispdata = (short int *)(cv_disparity_->data);
828  }
829 
830  if (shm_img_disparity_->num_attached() > 1) {
831  cv::Mat normalized_disparity(height_, width_, CV_16SC1);
832  cv::Mat original_disparity(height_, width_, uses_triclops ? CV_16UC1 : CV_16SC1, dispdata);
833  cv::normalize(original_disparity, normalized_disparity, 0, 256, cv::NORM_MINMAX);
834  shm_img_disparity_->lock_for_write();
835  unsigned char *buffer = shm_img_disparity_->buffer();
836  for (unsigned int i = 0; i < width_ * height_; ++i) {
837  buffer[i] = (unsigned char)((short int *)(normalized_disparity.data))[i];
838  }
839  shm_img_disparity_->set_capture_time(&capture_ts);
840  shm_img_disparity_->unlock();
841  }
842 
843  TIMETRACK_END(ttc_stereo_match_);
844 
845  // 2 is us and the PCL manager of the PointCloudAspect
846  bool want_xyzrgb = (pcl_xyzrgb_.use_count() > 2);
847  bool want_xyz = (pcl_xyz_.use_count() > 2);
848 
849  TriclopsColorImage img_right_rect_color;
850  if (shm_img_rgb_rect_right_->num_attached() > 1 || (want_xyzrgb && uses_triclops)) {
851  convert(RGB, RGB_PLANAR, buffer_rgb_right_, buffer_rgb_planar_right_ , width_, height_);
852  TriclopsInput img_rgb_right;
853  img_rgb_right.inputType = TriInp_RGB;
854  img_rgb_right.nrows = height_;
855  img_rgb_right.ncols = width_;
856  img_rgb_right.rowinc = width_;
857  img_rgb_right.u.rgb.red = buffer_rgb_planar_right_;
858  img_rgb_right.u.rgb.green = buffer_rgb_planar_right_ + (width_ * height_ );
859  img_rgb_right.u.rgb.blue = buffer_rgb_planar_right_ + (width_ * height_ * 2);
860  err = triclopsRectifyColorImage(triclops_->context, TriCam_RIGHT,
861  &img_rgb_right, &img_right_rect_color);
862  if (err != TriclopsErrorOk) {
863  logger->log_warn(name(), "Rectifying right color image failed (%s), skipping loop",
864  triclopsErrorToString(err));
865  bb2_->dispose_buffer();
866  return;
867  }
868  if (shm_img_rgb_rect_right_->num_attached() > 1) {
869  shm_img_rgb_rect_right_->lock_for_write();
870  memcpy(shm_img_rgb_rect_right_->buffer(), img_right_rect_color.red, width_ * height_);
871  memcpy(shm_img_rgb_rect_right_->buffer() + width_ * height_,
872  img_right_rect_color.green, width_ * height_);
873  memcpy(shm_img_rgb_rect_right_->buffer() + width_ * height_ * 2,
874  img_right_rect_color.blue, width_ * height_);
875  shm_img_rgb_rect_right_->set_capture_time(&capture_ts);
876  shm_img_rgb_rect_right_->unlock();
877  }
878  }
879 
880  TriclopsColorImage img_left_rect_color;
881  if (shm_img_rgb_rect_left_->num_attached() > 1 || (want_xyzrgb && uses_opencv)) {
882  convert(RGB, RGB_PLANAR, buffer_rgb_left_, buffer_rgb_planar_left_ , width_, height_);
883  TriclopsInput img_rgb_left;
884  img_rgb_left.inputType = TriInp_RGB;
885  img_rgb_left.nrows = height_;
886  img_rgb_left.ncols = width_;
887  img_rgb_left.rowinc = width_;
888  img_rgb_left.u.rgb.red = buffer_rgb_planar_left_;
889  img_rgb_left.u.rgb.green = buffer_rgb_planar_left_ + (width_ * height_ );
890  img_rgb_left.u.rgb.blue = buffer_rgb_planar_left_ + (width_ * height_ * 2);
891  err = triclopsRectifyColorImage(triclops_->context, TriCam_LEFT,
892  &img_rgb_left, &img_left_rect_color);
893  if (err != TriclopsErrorOk) {
894  logger->log_warn(name(), "Rectifying left color image failed (%s), skipping loop",
895  triclopsErrorToString(err));
896  bb2_->dispose_buffer();
897  return;
898  }
899  if (shm_img_rgb_rect_left_->num_attached() > 1) {
900  shm_img_rgb_rect_left_->lock_for_write();
901  memcpy(shm_img_rgb_rect_left_->buffer(), img_left_rect_color.red, width_ * height_);
902  memcpy(shm_img_rgb_rect_left_->buffer() + width_ * height_,
903  img_left_rect_color.green, width_ * height_);
904  memcpy(shm_img_rgb_rect_left_->buffer() + width_ * height_ * 2,
905  img_left_rect_color.blue, width_ * height_);
906  shm_img_rgb_rect_left_->set_capture_time(&capture_ts);
907  shm_img_rgb_rect_left_->unlock();
908  }
909  }
910 
911  // Triclops' reference image is the right camera, OpenCV uses left
912  TriclopsColorImage *img_reference_rect_color =
913  (uses_triclops) ? &img_right_rect_color : &img_left_rect_color;
914 
915  // Calculate 3D point cloud from data
916  pcl::PointCloud<pcl::PointXYZ> &pcl_xyz = **pcl_xyz_;
917  pcl::PointCloud<pcl::PointXYZRGB> &pcl_xyzrgb = **pcl_xyzrgb_;
918 
919  if (want_xyz && want_xyzrgb) {
920  fill_xyz_xyzrgb(dispdata, img_reference_rect_color, pcl_xyz, pcl_xyzrgb);
921 
922  pcl_xyz.header.seq += 1;
923  pcl_xyzrgb.header.seq += 1;
924  pcl_utils::set_time(pcl_xyz_, capture_ts);
925  pcl_utils::set_time(pcl_xyzrgb_, capture_ts);
926  } else if (want_xyz) {
927  fill_xyz(dispdata, pcl_xyz);
928  pcl_xyz.header.seq += 1;
929  pcl_utils::set_time(pcl_xyz_, capture_ts);
930  } else if (want_xyzrgb) {
931  fill_xyzrgb(dispdata, img_reference_rect_color, pcl_xyzrgb);
932  pcl_xyzrgb.header.seq += 1;
933  pcl_utils::set_time(pcl_xyzrgb_, capture_ts);
934  }
935 
936  bb2_->dispose_buffer();
937 
938  TIMETRACK_END(ttc_full_loop_);
939 
940 #ifdef USE_TIMETRACKER
941  if ((++tt_loopcount_ % 30) == 0) {
942  tt_->print_to_stdout();
943  }
944  if (tt_loopcount_ >= 150) {
945  tt_loopcount_ = 0;
946  tt_->reset();
947  }
948 #endif
949 
950 }
951 
952 
953 // Methods to fill in point clouds
954 // Z = fB/d
955 // where
956 // Z = distance along the camera Z axis
957 // f = focal length (in pixels)
958 // B = baseline (in metres)
959 // d = disparity (in pixels)
960 // After Z is determined, X and Y can be calculated using
961 // the usual projective camera equations:
962 //
963 // X = uZ/f
964 // Y = vZ/f
965 
966 // where
967 // u and v are the pixel location in the 2D image
968 // X, Y, Z is the real 3d position
969 
970 // Note: u and v are not the same as row and column. You must
971 // account for the image center. You can get the image center using
972 // the triclopsGetImageCenter() function. Then you find u and v by:
973 
974 // u = col - centerCol
975 // v = row - centerRow
976 
977 // Note: If u, v, f, and d are all in pixels and X,Y,Z are all in
978 // the meters, the units will always work i.e. pixel/pixel =
979 // no-unit-ratio = m/m.
980 
981 
982 void
983 Bumblebee2Thread::fill_xyz_xyzrgb(const short int *dispdata,
984  const TriclopsColorImage *img_rect_color,
987 {
988  float bad_point = std::numeric_limits<float>::quiet_NaN();
989 
990  unsigned int idx = 0;
991  for (unsigned int h = 0; h < height_; ++h) {
992  for (unsigned int w = 0; w < width_; ++w, ++idx, ++dispdata) {
993 
994  pcl::PointXYZ &xyz = pcl_xyz.points[idx];
995  pcl::PointXYZRGB &xyzrgb = pcl_xyzrgb.points[idx];
996 
997  float d = *dispdata * disparity_scale_factor_;
998  if (d <= cfg_bm_min_disparity_) {
999  xyz.x = xyz.y = xyz.z = xyzrgb.x = xyzrgb.y = xyzrgb.z = bad_point;
1000  continue;
1001  }
1002 
1003  float b_by_d = baseline_/d;
1004  xyz.z = xyzrgb.z = focal_length_ * b_by_d;
1005  xyz.x = xyzrgb.x = ((float)w - center_col_) * b_by_d;
1006  xyz.y = xyzrgb.y = ((float)h - center_row_) * b_by_d;
1007 
1008  xyzrgb.r = img_rect_color->red[idx];
1009  xyzrgb.g = img_rect_color->green[idx];
1010  xyzrgb.b = img_rect_color->blue[idx];
1011  }
1012  }
1013 }
1014 
1015 
1016 void
1017 Bumblebee2Thread::fill_xyzrgb(const short int *dispdata,
1018  const TriclopsColorImage *img_rect_color,
1020 {
1021  TIMETRACK_START(ttc_pcl_xyzrgb_);
1022  float bad_point = std::numeric_limits<float>::quiet_NaN ();
1023 
1024  unsigned int idx = 0;
1025  for (unsigned int h = 0; h < height_; ++h) {
1026  for (unsigned int w = 0; w < width_; ++w, ++idx, ++dispdata) {
1027 
1028  pcl::PointXYZRGB &xyzrgb = pcl_xyzrgb.points[idx];
1029 
1030  float d = *dispdata * disparity_scale_factor_;
1031  if (d <= cfg_bm_min_disparity_) {
1032  xyzrgb.x = xyzrgb.y = xyzrgb.z = bad_point;
1033  continue;
1034  }
1035 
1036  float b_by_d = baseline_/d;
1037  xyzrgb.z = focal_length_ * b_by_d;
1038  xyzrgb.x = ((float)w - center_col_) * b_by_d;
1039  xyzrgb.y = ((float)h - center_row_) * b_by_d;
1040 
1041  xyzrgb.r = img_rect_color->red[idx];
1042  xyzrgb.g = img_rect_color->green[idx];
1043  xyzrgb.b = img_rect_color->blue[idx];
1044  }
1045  }
1046  TIMETRACK_END(ttc_pcl_xyzrgb_);
1047 }
1048 
1049 void
1050 Bumblebee2Thread::fill_xyz(const short int *dispdata,
1052 {
1053  TIMETRACK_START(ttc_pcl_xyz_);
1054  float bad_point = std::numeric_limits<float>::quiet_NaN ();
1055 
1056  unsigned int idx = 0;
1057  for (unsigned int h = 0; h < height_; ++h) {
1058  for (unsigned int w = 0; w < width_; ++w, ++idx, ++dispdata) {
1059 
1060  pcl::PointXYZ &xyz = pcl_xyz.points[idx];
1061 
1062  // OpenCV disparity data is scaled by factor 16, always
1063  float d = *dispdata * disparity_scale_factor_;
1064  if (d <= cfg_bm_min_disparity_) {
1065  xyz.x = xyz.y = xyz.z = bad_point;
1066  continue;
1067  }
1068 
1069  float b_by_d = baseline_/d;
1070  xyz.z = focal_length_ * b_by_d;
1071  xyz.x = ((float)w - center_col_) * b_by_d;
1072  xyz.y = ((float)h - center_row_) * b_by_d;
1073  }
1074  }
1075  TIMETRACK_END(ttc_pcl_xyz_);
1076 }
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:71
void set_try_smaller_windows(const bool new_try_smaller_windows)
Set try_smaller_windows value.
SetSADWindowSizeMessage Fawkes BlackBoard Interface Message.
void set_pre_filter_type(const PreFilterType new_pre_filter_type)
Set pre_filter_type value.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
void remove_pointcloud(const char *id)
Remove the point cloud.
void set_sad_window_size(const uint32_t new_sad_window_size)
Set sad_window_size value.
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:918
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
Definition: bumblebee2.cpp:592
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void decode_bayer()
Extract RGB color image from the bayer tile image.
Definition: bumblebee2.cpp:477
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:100
STL namespace.
A class for handling time.
Definition: time.h:91
void set_min_disparity(const int32_t new_min_disparity)
Set min_disparity value.
SetPreFilterTypeMessage Fawkes BlackBoard Interface Message.
SetPreFilterSizeMessage Fawkes BlackBoard Interface Message.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual ~Bumblebee2Thread()
Destructor.
virtual void dispose_buffer()
Dispose current buffer.
Definition: firewire.cpp:402
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
SetTextureThresholdMessage Fawkes BlackBoard Interface Message.
Thread aspect to access the transform system.
Definition: tf.h:42
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:235
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
SetTrySmallerWindowsMessage Fawkes BlackBoard Interface Message.
SetSpeckleRangeMessage Fawkes BlackBoard Interface Message.
void reset()
Reset pointer.
Definition: refptr.h:464
Thread aspect to use blocked timing.
void set_uniqueness_ratio(const uint32_t new_uniqueness_ratio)
Set uniqueness_ratio value.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
virtual void capture()
Capture an image.
Definition: bumblebee2.cpp:416
void set_pre_filter_cap(const uint32_t new_pre_filter_cap)
Set pre_filter_cap value.
virtual void stop()
Stop image transfer from the camera.
Definition: firewire.cpp:255
SetSpeckleWindowSizeMessage Fawkes BlackBoard Interface Message.
Bumblebee2 camera.
Definition: bumblebee2.h:37
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
void set_enabled(const bool new_enabled)
Set enabled value.
OpenCVStereoParamsInterface Fawkes BlackBoard Interface.
virtual void close()
Close camera.
Definition: bumblebee2.cpp:400
virtual void finalize()
Finalize the thread.
int use_count() const
Get current reference count.
Definition: refptr.h:229
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
void set_pre_filter_size(const uint32_t new_pre_filter_size)
Set pre_filter_size value.
Shared memory image buffer.
Definition: shm_image.h:181
Time tracking utility.
Definition: tracker.h:38
SetPreFilterCapMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:714
const char * name() const
Get name of thread.
Definition: thread.h:95
void set_texture_threshold(const uint32_t new_texture_threshold)
Set texture_threshold value.
void set_num_disparities(const uint32_t new_num_disparities)
Set num_disparities value.
SetUniquenessRatioMessage Fawkes BlackBoard Interface Message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool is_enabled() const
Get enabled value.
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
Definition: bumblebee2.cpp:464
EnableSwitchMessage Fawkes BlackBoard Interface Message.
Bumblebee2Thread()
Constructor.
void set_speckle_window_size(const uint32_t new_speckle_window_size)
Set speckle_window_size value.
virtual void init()
Initialize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:302
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
SetNumDisparitiesMessage Fawkes BlackBoard Interface Message.
SetMinDisparityMessage Fawkes BlackBoard Interface Message.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:205
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void unlock()
Unlock memory.
Definition: shm.cpp:985
void set_speckle_range(const uint32_t new_speckle_range)
Set speckle_range value.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.