Support both Mat and GpuMat with automatic uploads/downloads

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

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