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 }
firevision::FirewireCamera::pixel_width
virtual unsigned int pixel_width()
Definition: firewire.cpp:408
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
fawkes::OpenCVStereoParamsInterface::SetNumDisparitiesMessage
Definition: OpenCVStereoParamsInterface.h:241
firevision::Bumblebee2Camera::close
virtual void close()
Definition: bumblebee2.cpp:393
fawkes::RefPtr::use_count
int use_count() const
Get current reference count.
Definition: refptr.h:248
fawkes::OpenCVStereoParamsInterface::set_pre_filter_type
void set_pre_filter_type(const PreFilterType new_pre_filter_type)
Set pre_filter_type value.
Definition: OpenCVStereoParamsInterface.cpp:130
fawkes::OpenCVStereoParamsInterface::set_uniqueness_ratio
void set_uniqueness_ratio(const uint32_t new_uniqueness_ratio)
Set uniqueness_ratio value.
Definition: OpenCVStereoParamsInterface.cpp:377
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
Bumblebee2Thread::Bumblebee2Thread
Bumblebee2Thread()
Constructor.
Definition: bumblebee2_thread.cpp:73
fawkes::SwitchInterface
Definition: SwitchInterface.h:37
fawkes::Interface::msgq_first_safe
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:302
fawkes::PointCloudManager::remove_pointcloud
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud_manager.cpp:69
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
firevision::SharedMemoryImageBuffer::set_capture_time
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:197
Bumblebee2Thread::loop
virtual void loop()
Code to execute in the thread.
Definition: bumblebee2_thread.cpp:509
fawkes::SharedMemory::num_attached
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:767
fawkes::tf::StampedTransform::stamp
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:102
fawkes::OpenCVStereoParamsInterface::SetSpeckleRangeMessage
Definition: OpenCVStereoParamsInterface.h:356
fawkes::OpenCVStereoParamsInterface::set_speckle_window_size
void set_speckle_window_size(const uint32_t new_speckle_window_size)
Set speckle_window_size value.
Definition: OpenCVStereoParamsInterface.cpp:412
fawkes::SharedMemory::unlock
void unlock()
Unlock memory.
Definition: shm.cpp:1029
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
fawkes::OpenCVStereoParamsInterface::set_sad_window_size
void set_sad_window_size(const uint32_t new_sad_window_size)
Set sad_window_size value.
Definition: OpenCVStereoParamsInterface.cpp:235
firevision::SharedMemoryImageBuffer
Definition: shm_image.h:182
fawkes::SwitchInterface::DisableSwitchMessage
Definition: SwitchInterface.h:144
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::ClockAspect::clock
Clock * clock
Definition: clock.h:53
firevision::FirewireCamera::dispose_buffer
virtual void dispose_buffer()
Definition: firewire.cpp:400
firevision::FirewireCamera::pixel_height
virtual unsigned int pixel_height()
Definition: firewire.cpp:430
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
Bumblebee2Thread::init
virtual void init()
Initialize the thread.
Definition: bumblebee2_thread.cpp:86
fawkes::OpenCVStereoParamsInterface::SetSADWindowSizeMessage
Definition: OpenCVStereoParamsInterface.h:187
fawkes::OpenCVStereoParamsInterface::set_pre_filter_cap
void set_pre_filter_cap(const uint32_t new_pre_filter_cap)
Set pre_filter_cap value.
Definition: OpenCVStereoParamsInterface.cpp:200
firevision::FirewireCamera::stop
virtual void stop()
Definition: firewire.cpp:254
fawkes::SwitchInterface::EnableSwitchMessage
Definition: SwitchInterface.h:124
firevision::Bumblebee2Camera::set_image_number
virtual void set_image_number(unsigned int image_num)
Definition: bumblebee2.cpp:431
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::SharedMemory::lock_for_write
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:963
fawkes::SwitchInterface::set_enabled
void set_enabled(const bool new_enabled)
Set enabled value.
Definition: SwitchInterface.cpp:109
fawkes::OpenCVStereoParamsInterface
Definition: OpenCVStereoParamsInterface.h:37
fawkes
fawkes::tf::TransformPublisher::send_transform
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
Definition: transform_publisher.cpp:122
fawkes::TransformAspect
Definition: tf.h:41
pcl::PointCloud< pcl::PointXYZ >
firevision::Bumblebee2Camera
Definition: bumblebee2.h:38
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::OpenCVStereoParamsInterface::set_try_smaller_windows
void set_try_smaller_windows(const bool new_try_smaller_windows)
Set try_smaller_windows value.
Definition: OpenCVStereoParamsInterface.cpp:484
firevision::Bumblebee2Camera::write_triclops_config_from_camera_to_file
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
Definition: bumblebee2.cpp:572
firevision::Bumblebee2Camera::deinterlace_stereo
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
Definition: bumblebee2.cpp:455
fawkes::OpenCVStereoParamsInterface::SetSpeckleWindowSizeMessage
Definition: OpenCVStereoParamsInterface.h:328
fawkes::OpenCVStereoParamsInterface::SetUniquenessRatioMessage
Definition: OpenCVStereoParamsInterface.h:298
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
fawkes::OpenCVStereoParamsInterface::set_min_disparity
void set_min_disparity(const int32_t new_min_disparity)
Set min_disparity value.
Definition: OpenCVStereoParamsInterface.cpp:266
firevision::Bumblebee2Camera::decode_bayer
void decode_bayer()
Extract RGB color image from the bayer tile image.
Definition: bumblebee2.cpp:466
fawkes::OpenCVStereoParamsInterface::SetTrySmallerWindowsMessage
Definition: OpenCVStereoParamsInterface.h:384
firevision::SharedMemoryImageBuffer::buffer
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:227
fawkes::Time
Definition: time.h:96
firevision::Bumblebee2Camera::open
virtual void open()
Definition: bumblebee2.cpp:345
fawkes::TimeTracker
Definition: tracker.h:40
fawkes::OpenCVStereoParamsInterface::SetPreFilterTypeMessage
Definition: OpenCVStereoParamsInterface.h:105
Bumblebee2Thread::~Bumblebee2Thread
virtual ~Bumblebee2Thread()
Destructor.
Definition: bumblebee2_thread.cpp:81
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::Thread
Definition: thread.h:44
fawkes::TransformAspect::tf_publisher
tf::TransformPublisher * tf_publisher
Definition: tf.h:71
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:47
fawkes::PointCloudManager::add_pointcloud
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Definition: pointcloud_manager.h:77
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::OpenCVStereoParamsInterface::set_num_disparities
void set_num_disparities(const uint32_t new_num_disparities)
Set num_disparities value.
Definition: OpenCVStereoParamsInterface.cpp:301
fawkes::OpenCVStereoParamsInterface::SetMinDisparityMessage
Definition: OpenCVStereoParamsInterface.h:215
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:709
fawkes::OpenCVStereoParamsInterface::SetPreFilterCapMessage
Definition: OpenCVStereoParamsInterface.h:159
fawkes::SwitchInterface::is_enabled
bool is_enabled() const
Get enabled value.
Definition: SwitchInterface.cpp:87
fawkes::PointCloudAspect::pcl_manager
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:46
fawkes::OpenCVStereoParamsInterface::set_texture_threshold
void set_texture_threshold(const uint32_t new_texture_threshold)
Set texture_threshold value.
Definition: OpenCVStereoParamsInterface.cpp:338
fawkes::RefPtr::reset
void reset()
Reset pointer.
Definition: refptr.h:459
Bumblebee2Thread::finalize
virtual void finalize()
Finalize the thread.
Definition: bumblebee2_thread.cpp:454
firevision::Bumblebee2Camera::capture
virtual void capture()
Definition: bumblebee2.cpp:409
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:497
fawkes::OpenCVStereoParamsInterface::SetTextureThresholdMessage
Definition: OpenCVStereoParamsInterface.h:269
fawkes::OpenCVStereoParamsInterface::SetPreFilterSizeMessage
Definition: OpenCVStereoParamsInterface.h:131
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::OpenCVStereoParamsInterface::set_speckle_range
void set_speckle_range(const uint32_t new_speckle_range)
Set speckle_range value.
Definition: OpenCVStereoParamsInterface.cpp:447
firevision::FirewireCamera::start
virtual void start()
Definition: firewire.cpp:224
firevision::Bumblebee2Camera::buffer
virtual unsigned char * buffer()
Definition: bumblebee2.cpp:425
fawkes::OpenCVStereoParamsInterface::set_pre_filter_size
void set_pre_filter_size(const uint32_t new_pre_filter_size)
Set pre_filter_size value.
Definition: OpenCVStereoParamsInterface.cpp:165
fawkes::Exception
Definition: exception.h:39