diff --git a/components/rgbd-sources/src/sources/realsense/realsense_source.cpp b/components/rgbd-sources/src/sources/realsense/realsense_source.cpp index 982f4209ad220bb484524742761ca276fc83ddad..3ecfa3ec313c3bf3ee35c25947b5f2364882a0e7 100644 --- a/components/rgbd-sources/src/sources/realsense/realsense_source.cpp +++ b/components/rgbd-sources/src/sources/realsense/realsense_source.cpp @@ -110,18 +110,23 @@ bool RealsenseSource::retrieve(ftl::rgbd::Frame &frame) { cv::Mat tmp_rgb(cv::Size(w, h), CV_8UC4, (void*)cframe.get_data(), cv::Mat::AUTO_STEP); frame.create<GpuMat>(Channel::Colour).upload(tmp_rgb); } else { + auto cframe = frames.get_color_frame(); //first(RS2_STREAM_COLOR); + size_t w = cframe.get_width(); + size_t h = cframe.get_height(); + cv::Mat wrap_rgb(cv::Size(w, h), CV_8UC4, (void*)cframe.get_data(), cv::Mat::AUTO_STEP); + frame.create<GpuMat>(Channel::Colour).upload(wrap_rgb, stream_); + frames = align_to_depth_.process(frames); rs2::depth_frame depth = frames.get_depth_frame(); - float w = depth.get_width(); - float h = depth.get_height(); - rscolour_ = frames.first(RS2_STREAM_COLOR); //.get_color_frame(); - - cv::Mat tmp_depth(cv::Size((int)w, (int)h), CV_16UC1, (void*)depth.get_data(), depth.get_stride_in_bytes()); - tmp_depth.convertTo(tmp_depth, CV_32FC1, scale_); - frame.create<GpuMat>(Channel::Depth).upload(tmp_depth); - cv::Mat tmp_rgb(cv::Size(w, h), CV_8UC4, (void*)rscolour_.get_data(), cv::Mat::AUTO_STEP); - frame.create<GpuMat>(Channel::Colour).upload(tmp_rgb); + w = depth.get_width(); + h = depth.get_height(); + + cv::Mat wrap_depth(cv::Size((int)w, (int)h), CV_16UC1, (void*)depth.get_data(), depth.get_stride_in_bytes()); + tmp_depth_.upload(wrap_depth, stream_); + tmp_depth_.convertTo(frame.create<GpuMat>(Channel::Depth), CV_32FC1, scale_, stream_); + + stream_.waitForCompletion(); } return true; diff --git a/components/rgbd-sources/src/sources/realsense/realsense_source.hpp b/components/rgbd-sources/src/sources/realsense/realsense_source.hpp index ec95ca0067d31733ef02d2e04a9694b65ba551f6..952d93c468c880d8d7697b4e79948d8e74bec7a6 100644 --- a/components/rgbd-sources/src/sources/realsense/realsense_source.hpp +++ b/components/rgbd-sources/src/sources/realsense/realsense_source.hpp @@ -32,6 +32,8 @@ class RealsenseSource : public ftl::rgbd::BaseSourceImpl { rs2::frame rscolour_; ftl::rgbd::Camera params_; std::string name_; + cv::cuda::GpuMat tmp_depth_; + cv::cuda::Stream stream_; }; }