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
42using namespace std;
43using namespace firevision;
44using 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
54using 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 */
61class TriclopsData
62{
63public:
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
86void
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_);
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 */
415void
416Bumblebee2Thread::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 */
440void
441Bumblebee2Thread::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
454void
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
509void
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) {
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) {
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) {
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) {
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) {
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) {
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) {
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
977void
978Bumblebee2Thread::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
1009void
1010Bumblebee2Thread::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
1041void
1042Bumblebee2Thread::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.