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