diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp index 5b004a3ad428bd92278b917b1bb7d3a0215f6109..15428e66539abe4a977cee9b50ab89a319979d7b 100644 --- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp @@ -11,13 +11,13 @@ class Frame { public: /* @todo REMOVE HARDCODED CHANNEL COUNT! */ - Frame() : channels_(11), available_(11, false) {} + Frame() : channels_host_(11), channels_gpu_(11), available_(11, 0) {} /* @brief Reset all channels without releasing memory */ void reset() { - std::fill(available_.begin(), available_.end(), false); + std::fill(available_.begin(), available_.end(), 0); } /* @brief Is there valid data in channel. Must be checked if user @@ -29,15 +29,52 @@ public: return available_[_channelIdx(channel)]; } - /* @brief Get reference to the channel GpuMat and marks it valid. - * If hasChannel() was false, user of this method must set - * valid data to the returned reference. + /* @brief Get reference to the channel. + * @param Channel type + * @param Output parameter + * @param User of the method sets data. NOTE: Only works on unused + * channels or if reset() was called previously (TODO). + * @returns True, if output parameter contains valid data */ - cv::cuda::GpuMat& getChannel(const ftl::rgbd::channel_t& channel) + + bool getChannel(const ftl::rgbd::channel_t& channel, cv::Mat& out, bool set=false) + { + auto idx = _channelIdx(channel); + bool retval = available_[idx] & mask_host; + if (!retval) + { + if (available_[idx] & mask_gpu) + { + channels_gpu_[idx].download(channels_host_[idx]); + retval = true; + set = true; + } + } + + if (set) { available_[idx] |= mask_host; } + + out = channels_host_[idx]; + return retval; + } + + bool getChannel(const ftl::rgbd::channel_t& channel, cv::cuda::GpuMat& out, bool set=false) { auto idx = _channelIdx(channel); - available_[idx] = true; - return channels_[idx]; + bool retval = available_[idx] & mask_gpu; + if (!retval) + { + if (available_[idx] & mask_host) + { + channels_gpu_[idx].upload(channels_host_[idx]); + retval = true; + set = true; + } + } + + if (set) { available_[idx] |= mask_host; } + + out = channels_gpu_[idx]; + return retval; } private: @@ -56,12 +93,18 @@ private: case kChanConfidence: return 7; case kChanFlow: return 8; case kChanEnergy: return 9; - default: return 0; // should not happen + default: return 0; // should not happen (error) } } - std::vector<cv::cuda::GpuMat> channels_; - std::vector<bool> available_; + std::vector<cv::Mat> channels_host_; + std::vector<cv::cuda::GpuMat> channels_gpu_; + + // bitmasks for each channel stored in available_ + static const uint mask_host = 1; + static const uint mask_gpu = 2; + + std::vector<uint> available_; }; } diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp index 58a513a08f93619b22233478c79cff3a85600c0f..a4c19767ca999c1ec964dc7bb24162a20ad46427 100644 --- a/components/rgbd-sources/src/stereovideo.cpp +++ b/components/rgbd-sources/src/stereovideo.cpp @@ -173,8 +173,9 @@ bool StereoVideoSource::capture(int64_t ts) { bool StereoVideoSource::retrieve() { auto &frame = frames_[0]; frame.reset(); - cv::cuda::GpuMat& left = frame.getChannel(ftl::rgbd::kChanLeft); - cv::cuda::GpuMat& right = frame.getChannel(ftl::rgbd::kChanRight); + cv::cuda::GpuMat left, right; + frame.getChannel(ftl::rgbd::kChanLeft, left, true); + frame.getChannel(ftl::rgbd::kChanRight, right, true); lsrc_->get(left, right, calib_, stream2_); #ifdef HAVE_OPTFLOW @@ -208,10 +209,9 @@ void StereoVideoSource::swap() { bool StereoVideoSource::compute(int n, int b) { auto &frame = frames_[1]; - cv::cuda::GpuMat& left = frame.getChannel(ftl::rgbd::kChanLeft); - cv::cuda::GpuMat& right = frame.getChannel(ftl::rgbd::kChanRight); - cv::cuda::GpuMat& depth = frame.getChannel(ftl::rgbd::kChanDepth); - cv::cuda::GpuMat& disp = frame.getChannel(ftl::rgbd::kChanDisparity); + cv::cuda::GpuMat left, right, disp, depth; + frame.getChannel(ftl::rgbd::kChanLeft, left); + frame.getChannel(ftl::rgbd::kChanRight, right); const ftl::rgbd::channel_t chan = host_->getChannel(); if (left.empty() || right.empty()) return false; @@ -219,6 +219,9 @@ bool StereoVideoSource::compute(int n, int b) { if (chan == ftl::rgbd::kChanDepth) { + frame.getChannel(ftl::rgbd::kChanDepth, depth, true); + frame.getChannel(ftl::rgbd::kChanDisparity, disp, true); + if (depth.empty()) depth = cv::cuda::GpuMat(left.size(), CV_32FC1); if (disp.empty()) disp = cv::cuda::GpuMat(left.size(), CV_32FC1); disp_->compute(left, right, disp, stream_);