22 #include "bumblebee2_thread.h"
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>
34 #include <interfaces/OpenCVStereoParamsInterface.h>
35 #include <interfaces/SwitchInterface.h>
36 #include <utils/time/tracker_macros.h>
38 #include <opencv2/calib3d/calib3d.hpp>
39 #include <opencv2/core/core.hpp>
43 using namespace firevision;
46 #define CFG_PREFIX "/bumblebee2/"
47 #define CFG_OPENCV_PREFIX CFG_PREFIX "opencv-stereo/"
64 TriclopsContext context;
66 TriclopsImage rectified_image;
67 TriclopsImage16 disparity_image_hires;
68 TriclopsImage disparity_image_lores;
69 TriclopsImage3d *image_3d;
75 :
Thread(
"Bumblebee2Thread",
Thread::OPMODE_WAITFORWAKEUP),
92 tf_left_ = tf_right_ = 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;
100 #ifdef USE_TIMETRACKER
107 cfg_frames_interval_ =
config->
get_float(CFG_PREFIX
"frames-interval");
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;
115 throw Exception(
"Unknown stereo matcher %s", stereo_matcher.c_str());
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_);
153 triclops_ =
new TriclopsData();
155 triclops_->input.inputType = TriInp_RGB;
156 triclops_->input.nrows = height_;
157 triclops_->input.ncols = width_;
158 triclops_->input.rowinc = triclops_->input.ncols;
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;
169 get_triclops_context_from_camera();
176 triclopsSetNumberOfROIs(triclops_->context, 0);
177 triclopsSetResolutionAndPrepare(triclops_->context, height_, width_, height_, width_);
181 err = triclopsGetImageCenter(triclops_->context, ¢er_row_, ¢er_col_);
182 if (err != TriclopsErrorOk) {
184 throw Exception(
"Failed to get image center: %s", triclopsErrorToString(err));
187 err = triclopsGetFocalLength(triclops_->context, &focal_length_);
188 if (err != TriclopsErrorOk) {
190 throw Exception(
"Failed to get focal length: %s", triclopsErrorToString(err));
193 err = triclopsGetBaseline(triclops_->context, &baseline_);
194 if (err != TriclopsErrorOk) {
196 throw Exception(
"Failed to get baseline: %s", triclopsErrorToString(err));
199 std::string stereo_frame = cfg_frames_prefix_;
201 if (cfg_stereo_matcher_ == STEREO_MATCHER_TRICLOPS) {
202 triclopsCreateImage3d(triclops_->context, &(triclops_->image_3d));
205 triclopsSetSubpixelInterpolation(triclops_->context, 1);
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);
215 disparity_scale_factor_ = 1.0;
217 stereo_frame +=
"right";
219 }
else if (cfg_stereo_matcher_ == STEREO_MATCHER_OPENCV) {
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;
230 throw Exception(
"Unknown stereo algorithm '%s', use bm or sgbm", algorithm.c_str());
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");
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");
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");
248 cfg_bm_try_smaller_windows_ =
config->
get_bool(CFG_OPENCV_PREFIX
"try-smaller-windows");
252 cfg_sgbm_p1_auto_ = (sgbm_p1 ==
"auto");
253 if (!cfg_sgbm_p1_auto_) {
257 cfg_sgbm_p2_auto_ = (sgbm_p2 ==
"auto");
258 if (!cfg_sgbm_p2_auto_) {
261 cfg_sgbm_disp_12_max_diff_ =
config->
get_int(CFG_OPENCV_PREFIX
"sgbm-disp12-max-diff");
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;
269 throw Exception(
"Invalid OpenCV stereo matcher pre filter type");
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");
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");
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");
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");
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_);
306 switch (cfg_bm_pre_filter_type_) {
307 case CV_STEREO_BM_NORMALIZED_RESPONSE:
310 case CV_STEREO_BM_XSOBEL:
313 default:
throw Exception(
"No valid pre filter type set");
332 triclopsSetEdgeCorrelation(triclops_->context, 0);
333 triclopsSetLowpass(triclops_->context, 0);
335 cv_disparity_ =
new cv::Mat(height_, width_, CV_16SC1);
337 disparity_scale_factor_ = 1.f / 16.f;
339 stereo_frame +=
"left";
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;
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;
366 shm_img_rgb_rect_right_ =
368 shm_img_rgb_rect_left_ =
370 shm_img_rectified_right_ =
372 shm_img_rectified_left_ =
374 shm_img_prefiltered_right_ =
376 shm_img_prefiltered_left_ =
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));
395 #ifdef USE_TIMETRACKER
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");
416 Bumblebee2Thread::get_triclops_context_from_camera()
418 char *tmpname = (
char *)malloc(strlen(
"triclops_cal_XXXXXX") + 1);
419 strcpy(tmpname,
"triclops_cal_XXXXXX");
420 char *tmpfile = mktemp(tmpname);
423 TriclopsError err = triclopsGetDefaultContextFromFile(&(triclops_->context), tmpfile);
424 if (err != TriclopsErrorOk) {
426 throw Exception(
"Fetching Triclops context from camera failed");
441 Bumblebee2Thread::deinterlace_green(
unsigned char *src,
446 register int i = (width * height) - 2;
447 register int g = ((width * height) / 3) - 1;
450 dest[g--] = src[i -= 3];
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_;
481 delete cv_disparity_;
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_);
504 #ifdef USE_TIMETRACKER
512 TIMETRACK_START(ttc_full_loop_);
515 if ((now - tf_last_publish_) > cfg_frames_interval_) {
516 TIMETRACK_START(ttc_transforms_);
517 tf_last_publish_->
stamp();
521 fawkes::Time timestamp = now + (cfg_frames_interval_ * 1.1);
523 tf_left_->
stamp = timestamp;
524 tf_right_->
stamp = timestamp;
528 TIMETRACK_END(ttc_transforms_);
531 bool uses_triclops = (cfg_stereo_matcher_ == STEREO_MATCHER_TRICLOPS);
532 bool uses_opencv = (cfg_stereo_matcher_ == STEREO_MATCHER_OPENCV);
534 TIMETRACK_START(ttc_msgproc_);
540 switch (msg->pre_filter_type()) {
541 case OpenCVStereoParamsInterface::PFT_NORMALIZED_RESPONSE:
542 cfg_bm_pre_filter_type_ = CV_STEREO_BM_NORMALIZED_RESPONSE;
544 case OpenCVStereoParamsInterface::PFT_XSOBEL:
545 cfg_bm_pre_filter_type_ = CV_STEREO_BM_XSOBEL;
552 cfg_bm_pre_filter_size_ = msg->pre_filter_size();
557 cfg_bm_pre_filter_cap_ = msg->pre_filter_cap();
562 cfg_bm_sad_window_size_ = msg->sad_window_size();
567 cfg_bm_min_disparity_ = msg->min_disparity();
572 cfg_bm_num_disparities_ = msg->num_disparities();
577 cfg_bm_texture_threshold_ = msg->texture_threshold();
582 cfg_bm_uniqueness_ratio_ = msg->uniqueness_ratio();
587 cfg_bm_speckle_window_size_ = msg->speckle_window_size();
592 cfg_bm_speckle_range_ = msg->speckle_range();
597 cfg_bm_try_smaller_windows_ = msg->is_try_smaller_windows();
617 TIMETRACK_END(ttc_msgproc_);
620 TIMETRACK_ABORT(ttc_full_loop_);
621 TimeWait::wait(250000);
625 TIMETRACK_START(ttc_capture_);
631 TIMETRACK_INTER(ttc_capture_, ttc_preprocess_)
639 memcpy(shm_img_rgb_right_->
buffer(),
641 colorspace_buffer_size(RGB, width_, height_));
643 shm_img_rgb_right_->
unlock();
648 memcpy(shm_img_rgb_left_->
buffer(),
650 colorspace_buffer_size(RGB, width_, height_));
652 shm_img_rgb_left_->
unlock();
657 convert(RGB, YUV422_PLANAR, buffer_rgb_right_, shm_img_yuv_right_->
buffer(), width_, height_);
659 shm_img_yuv_right_->
unlock();
664 convert(RGB, YUV422_PLANAR, buffer_rgb_left_, shm_img_yuv_left_->
buffer(), width_, height_);
666 shm_img_yuv_left_->
unlock();
672 deinterlace_green(buffer_rgb_, buffer_green_, width_, 6 * height_);
674 TIMETRACK_INTER(ttc_preprocess_, ttc_rectify_);
676 err = triclopsRectify(triclops_->context, &(triclops_->input));
677 if (err != TriclopsErrorOk) {
679 "Rectifying the image failed (%s), skipping loop",
680 triclopsErrorToString(err));
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));
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));
706 memcpy(shm_img_rectified_right_->
buffer(),
708 colorspace_buffer_size(MONO8, width_, height_));
710 shm_img_rectified_right_->
unlock();
714 memcpy(shm_img_rectified_left_->
buffer(),
716 colorspace_buffer_size(MONO8, width_, height_));
718 shm_img_rectified_left_->
unlock();
721 TIMETRACK_INTER(ttc_rectify_, ttc_stereo_match_);
724 short int *dispdata = NULL;
726 err = triclopsStereo(triclops_->context);
727 if (err != TriclopsErrorOk) {
729 "Calculating the disparity image failed (%s), skipping loop",
730 triclopsErrorToString(err));
735 triclopsGetImage16(triclops_->context,
738 &(triclops_->disparity_image_hires));
739 dispdata = (
short int *)triclops_->disparity_image_hires.data;
741 }
else if (uses_opencv) {
743 cv::Mat img_r(height_, width_, CV_8UC1, image_right.data);
744 cv::Mat img_l(height_, width_, CV_8UC1, image_left.data);
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;
761 block_matcher(img_l, img_r, *cv_disparity_);
765 memcpy(shm_img_prefiltered_right_->
buffer(),
766 block_matcher.state->preFilteredImg0->data.ptr,
767 colorspace_buffer_size(MONO8, width_, height_));
769 shm_img_prefiltered_right_->
unlock();
773 memcpy(shm_img_prefiltered_left_->
buffer(),
774 block_matcher.state->preFilteredImg0->data.ptr,
775 colorspace_buffer_size(MONO8, width_, height_));
777 shm_img_prefiltered_left_->
unlock();
780 int cn = img_l.channels();
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;
791 block_matcher.P1 = cfg_sgbm_p1_;
793 if (cfg_sgbm_p2_auto_) {
794 block_matcher.P2 = 32 * cn * block_matcher.SADWindowSize * block_matcher.SADWindowSize;
796 block_matcher.P2 = cfg_sgbm_p2_;
798 if (block_matcher.P1 >= block_matcher.P2) {
800 "SGBM P1 >= P2 (%i <= %i), skipping loop",
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;
812 block_matcher(img_l, img_r, *cv_disparity_);
815 dispdata = (
short int *)(cv_disparity_->data);
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);
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];
828 shm_img_disparity_->
unlock();
831 TIMETRACK_END(ttc_stereo_match_);
834 bool want_xyzrgb = (pcl_xyzrgb_.
use_count() > 2);
835 bool want_xyz = (pcl_xyz_.
use_count() > 2);
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,
851 &img_right_rect_color);
852 if (err != TriclopsErrorOk) {
854 "Rectifying right color image failed (%s), skipping loop",
855 triclopsErrorToString(err));
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,
865 memcpy(shm_img_rgb_rect_right_->
buffer() + width_ * height_ * 2,
866 img_right_rect_color.blue,
869 shm_img_rgb_rect_right_->
unlock();
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,
887 &img_left_rect_color);
888 if (err != TriclopsErrorOk) {
890 "Rectifying left color image failed (%s), skipping loop",
891 triclopsErrorToString(err));
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,
901 memcpy(shm_img_rgb_rect_left_->
buffer() + width_ * height_ * 2,
902 img_left_rect_color.blue,
905 shm_img_rgb_rect_left_->
unlock();
910 TriclopsColorImage *img_reference_rect_color =
911 (uses_triclops) ? &img_right_rect_color : &img_left_rect_color;
917 if (want_xyz && want_xyzrgb) {
918 fill_xyz_xyzrgb(dispdata, img_reference_rect_color, pcl_xyz, pcl_xyzrgb);
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);
936 TIMETRACK_END(ttc_full_loop_);
938 #ifdef USE_TIMETRACKER
939 if ((++tt_loopcount_ % 30) == 0) {
940 tt_->print_to_stdout();
942 if (tt_loopcount_ >= 150) {
978 Bumblebee2Thread::fill_xyz_xyzrgb(
const short int * dispdata,
979 const TriclopsColorImage * img_rect_color,
983 float bad_point = std::numeric_limits<float>::quiet_NaN();
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];
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;
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;
1002 xyzrgb.r = img_rect_color->red[idx];
1003 xyzrgb.g = img_rect_color->green[idx];
1004 xyzrgb.b = img_rect_color->blue[idx];
1010 Bumblebee2Thread::fill_xyzrgb(
const short int * dispdata,
1011 const TriclopsColorImage * img_rect_color,
1014 TIMETRACK_START(ttc_pcl_xyzrgb_);
1015 float bad_point = std::numeric_limits<float>::quiet_NaN();
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];
1022 float d = *dispdata * disparity_scale_factor_;
1023 if (d <= cfg_bm_min_disparity_) {
1024 xyzrgb.x = xyzrgb.y = xyzrgb.z = bad_point;
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;
1033 xyzrgb.r = img_rect_color->red[idx];
1034 xyzrgb.g = img_rect_color->green[idx];
1035 xyzrgb.b = img_rect_color->blue[idx];
1038 TIMETRACK_END(ttc_pcl_xyzrgb_);
1044 TIMETRACK_START(ttc_pcl_xyz_);
1045 float bad_point = std::numeric_limits<float>::quiet_NaN();
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];
1053 float d = *dispdata * disparity_scale_factor_;
1054 if (d <= cfg_bm_min_disparity_) {
1055 xyz.x = xyz.y = xyz.z = bad_point;
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;
1065 TIMETRACK_END(ttc_pcl_xyz_);
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.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void msgq_pop()
Erase first message from queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
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.
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.
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.
int use_count() const
Get current reference count.
void lock_for_write()
Lock shared memory segment for writing.
unsigned int num_attached() const
Get number of attached processes.
void unlock()
Unlock memory.
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.
const char * name() const
Get name of thread.
A class for handling time.
Time & stamp()
Set this time to the current time.
virtual void capture()
Capture an image.
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
void decode_bayer()
Extract RGB color image from the bayer tile image.
virtual void close()
Close camera.
virtual void open()
Open the camera.
virtual void set_image_number(unsigned int image_num)
Set image number to retrieve.
virtual unsigned char * buffer()
Get access to current image buffer.
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
virtual unsigned int pixel_width()
Width of image in pixels.
virtual unsigned int pixel_height()
Height of image in pixels.
virtual void start()
Start image transfer from the camera.
virtual void stop()
Stop image transfer from the camera.
virtual void dispose_buffer()
Dispose current buffer.
Shared memory image buffer.
void set_capture_time(fawkes::Time *time)
Set the capture time.
unsigned char * buffer() const
Get image buffer.
Fawkes library namespace.