22 #include "bumblebee2_thread.h" 24 #include <pcl_utils/utils.h> 25 #include <pcl_utils/comparisons.h> 26 #include <utils/time/wait.h> 27 #include <utils/math/angle.h> 28 #include <fvcams/bumblebee2.h> 29 #include <fvutils/ipc/shm_image.h> 30 #include <fvutils/color/conversions.h> 31 #ifdef USE_TIMETRACKER 32 # include <utils/time/tracker.h> 34 #include <utils/time/tracker_macros.h> 36 #include <interfaces/SwitchInterface.h> 37 #include <interfaces/OpenCVStereoParamsInterface.h> 40 #include <opencv2/core/core.hpp> 41 #include <opencv2/calib3d/calib3d.hpp> 47 #define CFG_PREFIX "/bumblebee2/" 48 #define CFG_OPENCV_PREFIX CFG_PREFIX"opencv-stereo/" 65 TriclopsContext context;
67 TriclopsImage rectified_image;
68 TriclopsImage16 disparity_image_hires;
69 TriclopsImage disparity_image_lores;
70 TriclopsImage3d * image_3d;
77 :
Thread(
"Bumblebee2Thread",
Thread::OPMODE_WAITFORWAKEUP),
96 tf_left_ = tf_right_ = NULL;
99 shm_img_rgb_right_ = shm_img_rgb_left_ = shm_img_yuv_right_ = shm_img_yuv_left_ = NULL;
100 shm_img_rectified_right_ = shm_img_rectified_left_ = NULL;
101 shm_img_prefiltered_right_ = shm_img_prefiltered_left_ = NULL;
102 shm_img_rgb_rect_left_ = shm_img_rgb_rect_right_ = shm_img_disparity_ = NULL;
104 #ifdef USE_TIMETRACKER 113 std::string stereo_matcher =
config->
get_string(CFG_PREFIX
"stereo-matcher");
114 if (stereo_matcher ==
"opencv") {
115 cfg_stereo_matcher_ = STEREO_MATCHER_OPENCV;
116 }
else if (stereo_matcher ==
"triclops") {
117 cfg_stereo_matcher_ = STEREO_MATCHER_TRICLOPS;
119 throw Exception(
"Unknown stereo matcher %s", stereo_matcher.c_str());
128 bb2_->set_image_number(Bumblebee2Camera::RGB_IMAGE);
145 width_ = bb2_->pixel_width();
146 height_ = bb2_->pixel_height();
148 buffer_rgb_ = bb2_->buffer();
149 buffer_rgb_right_ = buffer_rgb_;
150 buffer_rgb_left_ = buffer_rgb_right_ + colorspace_buffer_size(RGB, width_, height_);
151 buffer_green_ = (
unsigned char *)malloc(width_ * height_ * 2);
152 buffer_yuv_right_ = malloc_buffer(YUV422_PLANAR, width_, height_);
153 buffer_yuv_left_ = malloc_buffer(YUV422_PLANAR, width_, height_);
154 buffer_rgb_planar_right_ = malloc_buffer(RGB_PLANAR, width_, height_);
155 buffer_rgb_planar_left_ = malloc_buffer(RGB_PLANAR, width_, height_);
157 triclops_ =
new TriclopsData();
159 triclops_->input.inputType = TriInp_RGB;
160 triclops_->input.nrows = height_;
161 triclops_->input.ncols = width_;
162 triclops_->input.rowinc = triclops_->input.ncols;
168 triclops_->input.u.rgb.red = buffer_green_;
169 triclops_->input.u.rgb.green = buffer_green_ + width_ * height_;
170 triclops_->input.u.rgb.blue = triclops_->input.u.rgb.green;
173 get_triclops_context_from_camera();
180 triclopsSetNumberOfROIs(triclops_->context, 0);
181 triclopsSetResolutionAndPrepare(triclops_->context,
182 height_, width_, height_, width_);
187 err = triclopsGetImageCenter(triclops_->context, ¢er_row_, ¢er_col_);
188 if (err != TriclopsErrorOk) {
190 throw Exception(
"Failed to get image center: %s", triclopsErrorToString(err));
193 err = triclopsGetFocalLength(triclops_->context, &focal_length_);
194 if (err != TriclopsErrorOk) {
196 throw Exception(
"Failed to get focal length: %s", triclopsErrorToString(err));
199 err = triclopsGetBaseline(triclops_->context, &baseline_);
200 if (err != TriclopsErrorOk) {
202 throw Exception(
"Failed to get baseline: %s", triclopsErrorToString(err));
205 std::string stereo_frame = cfg_frames_prefix_;
207 if (cfg_stereo_matcher_ == STEREO_MATCHER_TRICLOPS) {
208 triclopsCreateImage3d(triclops_->context, &(triclops_->image_3d));
211 triclopsSetSubpixelInterpolation(triclops_->context, 1);
213 triclopsSetEdgeCorrelation(triclops_->context, 1);
214 triclopsSetLowpass(triclops_->context, 1);
215 triclopsSetDisparity(triclops_->context, 5, 100);
216 triclopsSetEdgeMask(triclops_->context, 11);
217 triclopsSetStereoMask(triclops_->context, 23);
218 triclopsSetSurfaceValidation(triclops_->context, 1);
219 triclopsSetTextureValidation(triclops_->context, 0);
221 disparity_scale_factor_ = 1.0;
223 stereo_frame +=
"right";
225 }
else if (cfg_stereo_matcher_ == STEREO_MATCHER_OPENCV) {
230 if (algorithm ==
"bm") {
231 cfg_opencv_stereo_algorithm_ = OPENCV_STEREO_BM;
232 }
else if (algorithm ==
"sgbm") {
233 cfg_opencv_stereo_algorithm_ = OPENCV_STEREO_SGBM;
236 throw Exception(
"Unknown stereo algorithm '%s', use bm or sgbm", algorithm.c_str());
239 std::string pre_filter_type =
config->
get_string(CFG_OPENCV_PREFIX
"pre-filter-type");
240 cfg_bm_pre_filter_size_ =
config->
get_uint(CFG_OPENCV_PREFIX
"pre-filter-size");
241 cfg_bm_pre_filter_cap_ =
config->
get_uint(CFG_OPENCV_PREFIX
"pre-filter-cap");
244 cfg_bm_sad_window_size_ =
config->
get_uint(CFG_OPENCV_PREFIX
"sad-window-size");
245 cfg_bm_min_disparity_ =
config->
get_int(CFG_OPENCV_PREFIX
"min-disparity");
246 cfg_bm_num_disparities_ =
config->
get_uint(CFG_OPENCV_PREFIX
"num-disparities");
249 cfg_bm_texture_threshold_ =
config->
get_uint(CFG_OPENCV_PREFIX
"texture-threshold");
250 cfg_bm_uniqueness_ratio_ =
config->
get_uint(CFG_OPENCV_PREFIX
"uniqueness-ratio");
251 cfg_bm_speckle_window_size_ =
config->
get_uint(CFG_OPENCV_PREFIX
"speckle-window-size");
252 cfg_bm_speckle_range_ =
config->
get_uint(CFG_OPENCV_PREFIX
"speckle-range");
254 cfg_bm_try_smaller_windows_ =
config->
get_bool(CFG_OPENCV_PREFIX
"try-smaller-windows");
258 cfg_sgbm_p1_auto_ = (sgbm_p1 ==
"auto");
259 if (! cfg_sgbm_p1_auto_) {
263 cfg_sgbm_p2_auto_ = (sgbm_p2 ==
"auto");
264 if (! cfg_sgbm_p2_auto_) {
267 cfg_sgbm_disp_12_max_diff_ =
config->
get_int(CFG_OPENCV_PREFIX
"sgbm-disp12-max-diff");
270 if (pre_filter_type ==
"normalized_response") {
271 cfg_bm_pre_filter_type_ = CV_STEREO_BM_NORMALIZED_RESPONSE;
272 }
else if (pre_filter_type ==
"xsobel") {
273 cfg_bm_pre_filter_type_ = CV_STEREO_BM_XSOBEL;
275 throw Exception(
"Invalid OpenCV stereo matcher pre filter type");
278 if (cfg_bm_pre_filter_size_ < 5 || cfg_bm_pre_filter_size_ > 255 ||
279 cfg_bm_pre_filter_size_ % 2 == 0)
281 throw Exception(
"Pre filter size must be odd and be within 5..255");
284 if( cfg_bm_pre_filter_cap_ < 1 || cfg_bm_pre_filter_cap_ > 63 ) {
285 throw Exception(
"Pre filter cap must be within 1..63");
288 if( cfg_bm_sad_window_size_ < 5 || cfg_bm_sad_window_size_ > 255 ||
289 cfg_bm_sad_window_size_ % 2 == 0 ||
290 cfg_bm_sad_window_size_ >= std::min(width_, height_))
292 throw Exception(
"SAD window size must be odd, be within 5..255 and " 293 "be no larger than image width or height");
296 if( cfg_bm_num_disparities_ <= 0 || cfg_bm_num_disparities_ % 16 != 0 ) {
297 throw Exception(
"Number of disparities must be positive and divisble by 16");
305 std::max((
unsigned int)16, cfg_bm_min_disparity_ + cfg_bm_num_disparities_);
307 focal_length_ * baseline_ / max_disparity,
308 (cfg_bm_min_disparity_ == 0)
309 ? std::numeric_limits<float>::infinity()
310 : focal_length_ * baseline_ / cfg_bm_min_disparity_);
314 switch (cfg_bm_pre_filter_type_) {
315 case CV_STEREO_BM_NORMALIZED_RESPONSE:
318 case CV_STEREO_BM_XSOBEL:
322 throw Exception(
"No valid pre filter type set");
341 triclopsSetEdgeCorrelation(triclops_->context, 0);
342 triclopsSetLowpass(triclops_->context, 0);
344 cv_disparity_ =
new cv::Mat(height_, width_, CV_16SC1);
346 disparity_scale_factor_ = 1.f / 16.f;
348 stereo_frame +=
"left";
353 pcl_xyz_->is_dense =
false;
354 pcl_xyz_->width = width_;
355 pcl_xyz_->height = height_;
356 pcl_xyz_->points.resize(width_ * height_);
357 pcl_xyz_->header.frame_id = stereo_frame;
360 pcl_xyzrgb_->is_dense =
false;
361 pcl_xyzrgb_->width = width_;
362 pcl_xyzrgb_->height = height_;
363 pcl_xyzrgb_->points.resize(width_ * height_);
364 pcl_xyzrgb_->header.frame_id = stereo_frame;
377 shm_img_rgb_rect_right_ =
379 shm_img_rgb_rect_left_ =
381 shm_img_rectified_right_ =
383 shm_img_rectified_left_ =
385 shm_img_prefiltered_right_ =
387 shm_img_prefiltered_left_ =
394 tf::Quaternion q(-M_PI/2.f, 0, -M_PI/2.f);
395 tf::Transform t_left(q, tf::Vector3(0.0, 0.06, 0.018));
396 tf::Transform t_right(q, tf::Vector3(0.0, -0.06, 0.018));
407 #ifdef USE_TIMETRACKER 410 ttc_full_loop_ = tt_->add_class(
"Full Loop");
411 ttc_transforms_ = tt_->add_class(
"Transforms");
412 ttc_msgproc_ = tt_->add_class(
"Message Processing");
413 ttc_capture_ = tt_->add_class(
"Capture");
414 ttc_preprocess_ = tt_->add_class(
"Pre-processing");
415 ttc_rectify_ = tt_->add_class(
"Rectification");
416 ttc_stereo_match_ = tt_->add_class(
"Stereo Match");
417 ttc_pcl_xyzrgb_ = tt_->add_class(
"PCL XYZRGB");
418 ttc_pcl_xyz_ = tt_->add_class(
"PCL XYZ");
429 Bumblebee2Thread::get_triclops_context_from_camera()
431 char *tmpname = (
char *)malloc(strlen(
"triclops_cal_XXXXXX") + 1);
432 strcpy(tmpname,
"triclops_cal_XXXXXX");
433 char *tmpfile = mktemp(tmpname);
436 TriclopsError err = triclopsGetDefaultContextFromFile(&(triclops_->context), tmpfile);
437 if (err != TriclopsErrorOk) {
439 throw Exception(
"Fetching Triclops context from camera failed");
454 Bumblebee2Thread::deinterlace_green(
unsigned char* src,
unsigned char* dest,
455 unsigned int width,
unsigned int height)
457 register int i = (width*height)-2;
458 register int g = ((width*height)/3)-1;
461 dest[g--] = src[i-=3];
479 delete shm_img_rgb_right_;
480 delete shm_img_rgb_left_;
481 delete shm_img_yuv_right_;
482 delete shm_img_yuv_left_;
483 delete shm_img_rectified_right_;
484 delete shm_img_rectified_left_;
485 delete shm_img_prefiltered_right_;
486 delete shm_img_prefiltered_left_;
487 delete shm_img_rgb_rect_left_;
488 delete shm_img_rgb_rect_right_;
489 delete shm_img_disparity_;
492 delete cv_disparity_;
494 if (buffer_green_) free(buffer_green_);
495 if (buffer_yuv_right_) free(buffer_yuv_right_);
496 if (buffer_yuv_left_) free(buffer_yuv_left_);
497 if (buffer_rgb_planar_right_) free(buffer_rgb_planar_right_);
498 if (buffer_rgb_planar_left_) free(buffer_rgb_planar_left_);
510 #ifdef USE_TIMETRACKER 519 TIMETRACK_START(ttc_full_loop_);
522 if ((now - tf_last_publish_) > cfg_frames_interval_) {
523 TIMETRACK_START(ttc_transforms_);
524 tf_last_publish_->
stamp();
528 fawkes::Time timestamp = now + (cfg_frames_interval_ * 1.1);
530 tf_left_->
stamp = timestamp;
531 tf_right_->
stamp = timestamp;
535 TIMETRACK_END(ttc_transforms_);
538 bool uses_triclops = (cfg_stereo_matcher_ == STEREO_MATCHER_TRICLOPS);
539 bool uses_opencv = (cfg_stereo_matcher_ == STEREO_MATCHER_OPENCV);
542 TIMETRACK_START(ttc_msgproc_);
549 switch (msg->pre_filter_type()) {
550 case OpenCVStereoParamsInterface::PFT_NORMALIZED_RESPONSE:
551 cfg_bm_pre_filter_type_ = CV_STEREO_BM_NORMALIZED_RESPONSE;
553 case OpenCVStereoParamsInterface::PFT_XSOBEL:
554 cfg_bm_pre_filter_type_ = CV_STEREO_BM_XSOBEL;
562 cfg_bm_pre_filter_size_ = msg->pre_filter_size();
568 cfg_bm_pre_filter_cap_ = msg->pre_filter_cap();
574 cfg_bm_sad_window_size_ = msg->sad_window_size();
580 cfg_bm_min_disparity_ = msg->min_disparity();
586 cfg_bm_num_disparities_ = msg->num_disparities();
592 cfg_bm_texture_threshold_ = msg->texture_threshold();
598 cfg_bm_uniqueness_ratio_ = msg->uniqueness_ratio();
604 cfg_bm_speckle_window_size_ = msg->speckle_window_size();
610 cfg_bm_speckle_range_ = msg->speckle_range();
616 cfg_bm_try_smaller_windows_ = msg->is_try_smaller_windows();
640 TIMETRACK_END(ttc_msgproc_);
643 TIMETRACK_ABORT(ttc_full_loop_);
644 TimeWait::wait(250000);
648 TIMETRACK_START(ttc_capture_);
654 TIMETRACK_INTER(ttc_capture_, ttc_preprocess_)
662 memcpy(shm_img_rgb_right_->
buffer(), buffer_rgb_right_,
663 colorspace_buffer_size(RGB, width_, height_));
665 shm_img_rgb_right_->
unlock();
670 memcpy(shm_img_rgb_left_->
buffer(), buffer_rgb_left_,
671 colorspace_buffer_size(RGB, width_, height_));
673 shm_img_rgb_left_->
unlock();
678 convert(RGB, YUV422_PLANAR, buffer_rgb_right_,
679 shm_img_yuv_right_->
buffer(), width_, height_);
681 shm_img_yuv_right_->
unlock();
686 convert(RGB, YUV422_PLANAR, buffer_rgb_left_,
687 shm_img_yuv_left_->
buffer(), width_, height_);
689 shm_img_yuv_left_->
unlock();
695 deinterlace_green(buffer_rgb_, buffer_green_, width_, 6 * height_);
697 TIMETRACK_INTER(ttc_preprocess_, ttc_rectify_);
699 err = triclopsRectify(triclops_->context, &(triclops_->input));
700 if (err != TriclopsErrorOk) {
702 triclopsErrorToString(err));
708 TriclopsImage image_right, image_left;
709 err = triclopsGetImage(triclops_->context, TriImg_RECTIFIED, TriCam_RIGHT, &image_right);
710 if (err != TriclopsErrorOk) {
711 logger->
log_warn(
name(),
"Retrieving right rectified image failed (%s), skipping loop",
712 triclopsErrorToString(err));
716 err = triclopsGetImage(triclops_->context, TriImg_RECTIFIED, TriCam_LEFT, &image_left);
717 if (err != TriclopsErrorOk) {
718 logger->
log_warn(
name(),
"Retrieving left rectified image failed (%s), skipping loop",
719 triclopsErrorToString(err));
726 memcpy(shm_img_rectified_right_->
buffer(), image_right.data,
727 colorspace_buffer_size(MONO8, width_, height_));
729 shm_img_rectified_right_->
unlock();
733 memcpy(shm_img_rectified_left_->
buffer(), image_left.data,
734 colorspace_buffer_size(MONO8, width_, height_));
736 shm_img_rectified_left_->
unlock();
739 TIMETRACK_INTER(ttc_rectify_, ttc_stereo_match_);
742 short int *dispdata = NULL;
744 err = triclopsStereo(triclops_->context);
745 if (err != TriclopsErrorOk) {
746 logger->
log_warn(
name(),
"Calculating the disparity image failed (%s), skipping loop",
747 triclopsErrorToString(err));
752 triclopsGetImage16(triclops_->context, TriImg16_DISPARITY, TriCam_REFERENCE,
753 &(triclops_->disparity_image_hires));
754 dispdata = (
short int *)triclops_->disparity_image_hires.data;
756 }
else if (uses_opencv) {
758 cv::Mat img_r(height_, width_, CV_8UC1, image_right.data);
759 cv::Mat img_l(height_, width_, CV_8UC1, image_left.data);
762 if (cfg_opencv_stereo_algorithm_ == OPENCV_STEREO_BM) {
763 cv::StereoBM block_matcher(cv::StereoBM::BASIC_PRESET,
764 cfg_bm_num_disparities_, cfg_bm_sad_window_size_);
765 block_matcher.state->preFilterType = cfg_bm_pre_filter_type_;
766 block_matcher.state->preFilterSize = cfg_bm_pre_filter_size_;
767 block_matcher.state->preFilterCap = cfg_bm_pre_filter_cap_;
768 block_matcher.state->minDisparity = cfg_bm_min_disparity_;
769 block_matcher.state->textureThreshold = cfg_bm_texture_threshold_;
770 block_matcher.state->uniquenessRatio = cfg_bm_uniqueness_ratio_;
771 block_matcher.state->speckleWindowSize = cfg_bm_speckle_window_size_;
772 block_matcher.state->speckleRange = cfg_bm_speckle_range_;
773 block_matcher.state->trySmallerWindows = cfg_bm_try_smaller_windows_ ? 1 : 0;
775 block_matcher(img_l, img_r, *cv_disparity_);
779 memcpy(shm_img_prefiltered_right_->
buffer(),
780 block_matcher.state->preFilteredImg0->data.ptr,
781 colorspace_buffer_size(MONO8, width_, height_));
783 shm_img_prefiltered_right_->
unlock();
787 memcpy(shm_img_prefiltered_left_->
buffer(),
788 block_matcher.state->preFilteredImg0->data.ptr,
789 colorspace_buffer_size(MONO8, width_, height_));
791 shm_img_prefiltered_left_->
unlock();
794 int cn = img_l.channels();
796 cv::StereoSGBM block_matcher;
797 block_matcher.minDisparity = cfg_bm_min_disparity_;
798 block_matcher.numberOfDisparities = cfg_bm_num_disparities_;
799 block_matcher.SADWindowSize = cfg_bm_sad_window_size_;
800 block_matcher.preFilterCap = cfg_bm_pre_filter_cap_;
801 block_matcher.uniquenessRatio = cfg_bm_uniqueness_ratio_;
802 if (cfg_sgbm_p1_auto_) {
803 block_matcher.P1 = 8 * cn * block_matcher.SADWindowSize * block_matcher.SADWindowSize;
805 block_matcher.P1 = cfg_sgbm_p1_;
807 if (cfg_sgbm_p2_auto_) {
808 block_matcher.P2 = 32 * cn * block_matcher.SADWindowSize * block_matcher.SADWindowSize;
810 block_matcher.P2 = cfg_sgbm_p2_;
812 if (block_matcher.P1 >= block_matcher.P2) {
814 block_matcher.P1, block_matcher.P2);
819 block_matcher.speckleWindowSize = cfg_bm_speckle_window_size_;
820 block_matcher.speckleRange = cfg_bm_speckle_range_;
821 block_matcher.disp12MaxDiff = 1;
822 block_matcher. fullDP =
false;
824 block_matcher(img_l, img_r, *cv_disparity_);
827 dispdata = (
short int *)(cv_disparity_->data);
831 cv::Mat normalized_disparity(height_, width_, CV_16SC1);
832 cv::Mat original_disparity(height_, width_, uses_triclops ? CV_16UC1 : CV_16SC1, dispdata);
833 cv::normalize(original_disparity, normalized_disparity, 0, 256, cv::NORM_MINMAX);
835 unsigned char *buffer = shm_img_disparity_->
buffer();
836 for (
unsigned int i = 0; i < width_ * height_; ++i) {
837 buffer[i] = (
unsigned char)((
short int *)(normalized_disparity.data))[i];
840 shm_img_disparity_->
unlock();
843 TIMETRACK_END(ttc_stereo_match_);
846 bool want_xyzrgb = (pcl_xyzrgb_.
use_count() > 2);
847 bool want_xyz = (pcl_xyz_.
use_count() > 2);
849 TriclopsColorImage img_right_rect_color;
850 if (shm_img_rgb_rect_right_->
num_attached() > 1 || (want_xyzrgb && uses_triclops)) {
851 convert(RGB, RGB_PLANAR, buffer_rgb_right_, buffer_rgb_planar_right_ , width_, height_);
852 TriclopsInput img_rgb_right;
853 img_rgb_right.inputType = TriInp_RGB;
854 img_rgb_right.nrows = height_;
855 img_rgb_right.ncols = width_;
856 img_rgb_right.rowinc = width_;
857 img_rgb_right.u.rgb.red = buffer_rgb_planar_right_;
858 img_rgb_right.u.rgb.green = buffer_rgb_planar_right_ + (width_ * height_ );
859 img_rgb_right.u.rgb.blue = buffer_rgb_planar_right_ + (width_ * height_ * 2);
860 err = triclopsRectifyColorImage(triclops_->context, TriCam_RIGHT,
861 &img_rgb_right, &img_right_rect_color);
862 if (err != TriclopsErrorOk) {
864 triclopsErrorToString(err));
870 memcpy(shm_img_rgb_rect_right_->
buffer(), img_right_rect_color.red, width_ * height_);
871 memcpy(shm_img_rgb_rect_right_->
buffer() + width_ * height_,
872 img_right_rect_color.green, width_ * height_);
873 memcpy(shm_img_rgb_rect_right_->
buffer() + width_ * height_ * 2,
874 img_right_rect_color.blue, width_ * height_);
876 shm_img_rgb_rect_right_->
unlock();
880 TriclopsColorImage img_left_rect_color;
881 if (shm_img_rgb_rect_left_->
num_attached() > 1 || (want_xyzrgb && uses_opencv)) {
882 convert(RGB, RGB_PLANAR, buffer_rgb_left_, buffer_rgb_planar_left_ , width_, height_);
883 TriclopsInput img_rgb_left;
884 img_rgb_left.inputType = TriInp_RGB;
885 img_rgb_left.nrows = height_;
886 img_rgb_left.ncols = width_;
887 img_rgb_left.rowinc = width_;
888 img_rgb_left.u.rgb.red = buffer_rgb_planar_left_;
889 img_rgb_left.u.rgb.green = buffer_rgb_planar_left_ + (width_ * height_ );
890 img_rgb_left.u.rgb.blue = buffer_rgb_planar_left_ + (width_ * height_ * 2);
891 err = triclopsRectifyColorImage(triclops_->context, TriCam_LEFT,
892 &img_rgb_left, &img_left_rect_color);
893 if (err != TriclopsErrorOk) {
895 triclopsErrorToString(err));
901 memcpy(shm_img_rgb_rect_left_->
buffer(), img_left_rect_color.red, width_ * height_);
902 memcpy(shm_img_rgb_rect_left_->
buffer() + width_ * height_,
903 img_left_rect_color.green, width_ * height_);
904 memcpy(shm_img_rgb_rect_left_->
buffer() + width_ * height_ * 2,
905 img_left_rect_color.blue, width_ * height_);
907 shm_img_rgb_rect_left_->
unlock();
912 TriclopsColorImage *img_reference_rect_color =
913 (uses_triclops) ? &img_right_rect_color : &img_left_rect_color;
919 if (want_xyz && want_xyzrgb) {
920 fill_xyz_xyzrgb(dispdata, img_reference_rect_color, pcl_xyz, pcl_xyzrgb);
922 pcl_xyz.header.seq += 1;
923 pcl_xyzrgb.header.seq += 1;
924 pcl_utils::set_time(pcl_xyz_, capture_ts);
925 pcl_utils::set_time(pcl_xyzrgb_, capture_ts);
926 }
else if (want_xyz) {
927 fill_xyz(dispdata, pcl_xyz);
928 pcl_xyz.header.seq += 1;
929 pcl_utils::set_time(pcl_xyz_, capture_ts);
930 }
else if (want_xyzrgb) {
931 fill_xyzrgb(dispdata, img_reference_rect_color, pcl_xyzrgb);
932 pcl_xyzrgb.header.seq += 1;
933 pcl_utils::set_time(pcl_xyzrgb_, capture_ts);
938 TIMETRACK_END(ttc_full_loop_);
940 #ifdef USE_TIMETRACKER 941 if ((++tt_loopcount_ % 30) == 0) {
942 tt_->print_to_stdout();
944 if (tt_loopcount_ >= 150) {
983 Bumblebee2Thread::fill_xyz_xyzrgb(
const short int *dispdata,
984 const TriclopsColorImage *img_rect_color,
988 float bad_point = std::numeric_limits<float>::quiet_NaN();
990 unsigned int idx = 0;
991 for (
unsigned int h = 0; h < height_; ++h) {
992 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++dispdata) {
994 pcl::PointXYZ &xyz = pcl_xyz.points[idx];
995 pcl::PointXYZRGB &xyzrgb = pcl_xyzrgb.points[idx];
997 float d = *dispdata * disparity_scale_factor_;
998 if (d <= cfg_bm_min_disparity_) {
999 xyz.x = xyz.y = xyz.z = xyzrgb.x = xyzrgb.y = xyzrgb.z = bad_point;
1003 float b_by_d = baseline_/d;
1004 xyz.z = xyzrgb.z = focal_length_ * b_by_d;
1005 xyz.x = xyzrgb.x = ((float)w - center_col_) * b_by_d;
1006 xyz.y = xyzrgb.y = ((float)h - center_row_) * b_by_d;
1008 xyzrgb.r = img_rect_color->red[idx];
1009 xyzrgb.g = img_rect_color->green[idx];
1010 xyzrgb.b = img_rect_color->blue[idx];
1017 Bumblebee2Thread::fill_xyzrgb(
const short int *dispdata,
1018 const TriclopsColorImage *img_rect_color,
1021 TIMETRACK_START(ttc_pcl_xyzrgb_);
1022 float bad_point = std::numeric_limits<float>::quiet_NaN ();
1024 unsigned int idx = 0;
1025 for (
unsigned int h = 0; h < height_; ++h) {
1026 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++dispdata) {
1028 pcl::PointXYZRGB &xyzrgb = pcl_xyzrgb.points[idx];
1030 float d = *dispdata * disparity_scale_factor_;
1031 if (d <= cfg_bm_min_disparity_) {
1032 xyzrgb.x = xyzrgb.y = xyzrgb.z = bad_point;
1036 float b_by_d = baseline_/d;
1037 xyzrgb.z = focal_length_ * b_by_d;
1038 xyzrgb.x = ((float)w - center_col_) * b_by_d;
1039 xyzrgb.y = ((float)h - center_row_) * b_by_d;
1041 xyzrgb.r = img_rect_color->red[idx];
1042 xyzrgb.g = img_rect_color->green[idx];
1043 xyzrgb.b = img_rect_color->blue[idx];
1046 TIMETRACK_END(ttc_pcl_xyzrgb_);
1050 Bumblebee2Thread::fill_xyz(
const short int *dispdata,
1053 TIMETRACK_START(ttc_pcl_xyz_);
1054 float bad_point = std::numeric_limits<float>::quiet_NaN ();
1056 unsigned int idx = 0;
1057 for (
unsigned int h = 0; h < height_; ++h) {
1058 for (
unsigned int w = 0; w < width_; ++w, ++idx, ++dispdata) {
1060 pcl::PointXYZ &xyz = pcl_xyz.points[idx];
1063 float d = *dispdata * disparity_scale_factor_;
1064 if (d <= cfg_bm_min_disparity_) {
1065 xyz.x = xyz.y = xyz.z = bad_point;
1069 float b_by_d = baseline_/d;
1070 xyz.z = focal_length_ * b_by_d;
1071 xyz.x = ((float)w - center_col_) * b_by_d;
1072 xyz.y = ((float)h - center_row_) * b_by_d;
1075 TIMETRACK_END(ttc_pcl_xyz_);
void set_try_smaller_windows(const bool new_try_smaller_windows)
Set try_smaller_windows value.
SetSADWindowSizeMessage Fawkes BlackBoard Interface Message.
void set_pre_filter_type(const PreFilterType new_pre_filter_type)
Set pre_filter_type value.
bool msgq_empty()
Check if queue is empty.
void remove_pointcloud(const char *id)
Remove the point cloud.
void set_sad_window_size(const uint32_t new_sad_window_size)
Set sad_window_size value.
void lock_for_write()
Lock shared memory segment for writing.
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void decode_bayer()
Extract RGB color image from the bayer tile image.
A class for handling time.
void set_min_disparity(const int32_t new_min_disparity)
Set min_disparity value.
SetPreFilterTypeMessage Fawkes BlackBoard Interface Message.
SetPreFilterSizeMessage Fawkes BlackBoard Interface Message.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
virtual ~Bumblebee2Thread()
Destructor.
virtual void dispose_buffer()
Dispose current buffer.
void write()
Write from local copy into BlackBoard memory.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Logger * logger
This is the Logger member used to access the logger.
SetTextureThresholdMessage Fawkes BlackBoard Interface Message.
unsigned char * buffer() const
Get image buffer.
Clock * clock
By means of this member access to the clock is given.
SetTrySmallerWindowsMessage Fawkes BlackBoard Interface Message.
SetSpeckleRangeMessage Fawkes BlackBoard Interface Message.
void reset()
Reset pointer.
Thread aspect to use blocked timing.
void set_uniqueness_ratio(const uint32_t new_uniqueness_ratio)
Set uniqueness_ratio value.
void msgq_pop()
Erase first message from queue.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
virtual void capture()
Capture an image.
void set_pre_filter_cap(const uint32_t new_pre_filter_cap)
Set pre_filter_cap value.
virtual void stop()
Stop image transfer from the camera.
SetSpeckleWindowSizeMessage Fawkes BlackBoard Interface Message.
Base class for exceptions in Fawkes.
void set_enabled(const bool new_enabled)
Set enabled value.
OpenCVStereoParamsInterface Fawkes BlackBoard Interface.
virtual void close()
Close camera.
virtual void finalize()
Finalize the thread.
int use_count() const
Get current reference count.
void set_pre_filter_size(const uint32_t new_pre_filter_size)
Set pre_filter_size value.
Shared memory image buffer.
SetPreFilterCapMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
unsigned int num_attached() const
Get number of attached processes.
const char * name() const
Get name of thread.
void set_texture_threshold(const uint32_t new_texture_threshold)
Set texture_threshold value.
void set_num_disparities(const uint32_t new_num_disparities)
Set num_disparities value.
SetUniquenessRatioMessage Fawkes BlackBoard Interface Message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool is_enabled() const
Get enabled value.
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
Bumblebee2Thread()
Constructor.
void set_speckle_window_size(const uint32_t new_speckle_window_size)
Set speckle_window_size value.
virtual void init()
Initialize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Time & stamp()
Set this time to the current time.
SetNumDisparitiesMessage Fawkes BlackBoard Interface Message.
SetMinDisparityMessage Fawkes BlackBoard Interface Message.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Configuration * config
This is the Configuration member used to access the configuration.
void set_capture_time(fawkes::Time *time)
Set the capture time.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void unlock()
Unlock memory.
void set_speckle_range(const uint32_t new_speckle_range)
Set speckle_range value.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.