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_);