Merge branch 'feature/visionperf' into 'master'

Make capture and compute parallel

See merge request nicolas.pope/ftl!80

components/rgbd-sources/include/ftl/rgbd/detail/source.hpp

@@ -49,10 +49,22 @@ class Source {
 	virtual ~Source() {}
 
 	/**
+	 * Perform hardware data capture.
+	 */
+	virtual bool capture() { return true; };
+
+	/**
+	 * Do any processing from previously captured frames...
 	 * @param n Number of frames to request in batch. Default -1 means automatic (10)
 	 * @param b Bit rate setting. -1 = automatic, 0 = best quality, 9 = lowest quality
 	 */
-	virtual bool grab(int n, int b)=0;
+	virtual bool compute(int n, int b)=0;
+
+	/**
+	 * Between frames, or before next frame, do any buffer swapping operations.
+	 */
+	virtual void swap() {}
+
 	virtual bool isReady() { return false; };
 	virtual void setPose(const Eigen::Matrix4d &pose) { };
 

components/rgbd-sources/include/ftl/rgbd/source.hpp

@@ -71,19 +71,34 @@ class Source : public ftl::Configurable {
 	/**
 	 * Perform the hardware or virtual frame grab operation. 
 	 */
-	bool grab(int N=-1, int B=-1);
+	bool capture();
+
+	/**
+	 * Between frames, do any required buffer swaps.
+	 */
+	void swap() { if (impl_) impl_->swap(); }
 
 	/**
 	 * Do any post-grab processing. This function
 	 * may take considerable time to return, especially for sources requiring
-	 * software stereo correspondance. If `process` is not called manually
-	 * after a `grab` and before a `get`, then it will be called automatically
-	 * on first `get`.
+	 * software stereo correspondance.
 	 */
-	//void process();
+	bool compute(int N=-1, int B=-1);
+
+	/**
+	 * Wrapper grab that performs capture, swap and computation steps in one.
+	 * It is more optimal to perform capture and compute in parallel.
+	 */
+	bool grab(int N=-1, int B=-1) {
+		bool c = capture();
+		swap();
+		return c && compute(N,B);
+	}
 
 	/**
-	 * Get a copy of both colour and depth frames.
+	 * Get a copy of both colour and depth frames. Note that this does a buffer
+	 * swap rather than a copy, so the parameters should be persistent buffers for
+	 * best performance.
 	 */
 	void getFrames(cv::Mat &c, cv::Mat &d);
 

components/rgbd-sources/src/group.cpp

@@ -60,7 +60,9 @@ void Group::addSource(ftl::rgbd::Source *src) {
 // Callback returns true if it wishes to continue receiving frames.
 void Group::sync(int N, int B) {
 	for (auto s : sources_) {
-		s->grab(N,B);
+		s->capture();
+		s->swap();
+		s->compute(N,B);
 	}
 }
 

components/rgbd-sources/src/image.hpp

@@ -14,7 +14,7 @@ class ImageSource : public ftl::rgbd::detail::Source {
 
 	}
 
-	bool grab(int n, int b) { return false; };
+	bool compute(int n, int b) { return false; };
 	bool isReady() { return false; };
 };
 

components/rgbd-sources/src/middlebury_source.cpp

@@ -160,7 +160,7 @@ void MiddleburySource::_performDisparity() {
 	//disparityToDepthTRUE(depth_, depth_, params_);
 }
 
-bool MiddleburySource::grab(int n, int b) {
+bool MiddleburySource::compute(int n, int b) {
 	//_performDisparity();
 	return true;
 }

components/rgbd-sources/src/middlebury_source.hpp

@@ -19,7 +19,7 @@ class MiddleburySource : public detail::Source {
 	MiddleburySource(ftl::rgbd::Source *, const std::string &dir);
 	~MiddleburySource() {};
 
-	bool grab(int n, int b);
+	bool compute(int n, int b);
 	bool isReady() { return ready_; }
 
 	private:

components/rgbd-sources/src/net.cpp

@@ -302,7 +302,7 @@ void NetSource::_updateURI() {
 	}
 }
 
-bool NetSource::grab(int n, int b) {
+bool NetSource::compute(int n, int b) {
 	// Choose highest requested number of frames
 	maxN_ = std::max(maxN_,(n == -1) ? ftl::rgbd::detail::kDefaultFrameCount : n);
 

components/rgbd-sources/src/net.hpp

@@ -24,7 +24,7 @@ class NetSource : public detail::Source {
 	explicit NetSource(ftl::rgbd::Source *);
 	~NetSource();
 
-	bool grab(int n, int b);
+	bool compute(int n, int b);
 	bool isReady();
 
 	void setPose(const Eigen::Matrix4d &pose);

components/rgbd-sources/src/realsense_source.cpp

@@ -41,7 +41,7 @@ RealsenseSource::~RealsenseSource() {
 
 }
 
-bool RealsenseSource::grab(int n, int b) {
+bool RealsenseSource::compute(int n, int b) {
     rs2::frameset frames;
 	if (!pipe_.poll_for_frames(&frames)) return false;  //wait_for_frames();
 

components/rgbd-sources/src/realsense_source.hpp

@@ -17,7 +17,7 @@ class RealsenseSource : public ftl::rgbd::detail::Source {
 	RealsenseSource(ftl::rgbd::Source *host);
 	~RealsenseSource();
 
-	bool grab(int n=-1, int b=-1);
+	bool compute(int n=-1, int b=-1);
 	bool isReady();
 
 	private:

components/rgbd-sources/src/snapshot_source.hpp

@@ -17,7 +17,7 @@ class SnapshotSource : public detail::Source {
 	SnapshotSource(ftl::rgbd::Source *, ftl::rgbd::SnapshotReader &reader, const std::string &id);
 	~SnapshotSource() {};
 
-	bool grab(int n, int b) override { return true; };
+	bool compute(int n, int b) override { return true; };
 	bool isReady() { return true; }
 
 	//void reset();

components/rgbd-sources/src/source.cpp

@@ -167,8 +167,16 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
 	SHARED_LOCK(mutex_,lk);
 	//rgb_.copyTo(rgb);
 	//depth_.copyTo(depth);
+	//rgb = rgb_;
+	//depth = depth_;
+
+	cv::Mat tmp;
+	tmp = rgb;
 	rgb = rgb_;
+	rgb_ = tmp;
+	tmp = depth;
 	depth = depth_;
+	depth_ = tmp;
 }
 
 Eigen::Vector4d Source::point(uint ux, uint uy) {
@@ -222,16 +230,21 @@ void Source::reset() {
 	impl_ = _createImplementation();
 }
 
-bool Source::grab(int N, int B) {
+bool Source::capture() {
+	if (impl_) return impl_->capture();
+	else return true;
+}
+
+bool Source::compute(int N, int B) {
 	UNIQUE_LOCK(mutex_,lk);
 	if (!impl_ && stream_ != 0) {
 		cudaSafeCall(cudaStreamSynchronize(stream_));
 		if (depth_.type() == CV_32SC1) depth_.convertTo(depth_, CV_32F, 1.0f / 1000.0f);
 		stream_ = 0;
 		return true;
-	} else if (impl_ && impl_->grab(N,B)) {
+	} else if (impl_ && impl_->compute(N,B)) {
 		timestamp_ = impl_->timestamp_;
-		/*cv::Mat tmp;
+		cv::Mat tmp;
 		rgb_.create(impl_->rgb_.size(), impl_->rgb_.type());
 		depth_.create(impl_->depth_.size(), impl_->depth_.type());
 		tmp = rgb_;
@@ -239,9 +252,10 @@ bool Source::grab(int N, int B) {
 		impl_->rgb_ = tmp;
 		tmp = depth_;
 		depth_ = impl_->depth_;
-		impl_->depth_ = tmp;*/
-		impl_->rgb_.copyTo(rgb_);
-		impl_->depth_.copyTo(depth_);
+		impl_->depth_ = tmp;
+
+		//impl_->rgb_.copyTo(rgb_);
+		//impl_->depth_.copyTo(depth_);
 		return true;
 	}
 	return false;
@@ -314,7 +328,9 @@ bool Source::thumbnail(cv::Mat &t) {
 		return true;
 	} else if (impl_) {
 		UNIQUE_LOCK(mutex_,lk);
-		impl_->grab(1, 9);
+		impl_->capture();
+		impl_->swap();
+		impl_->compute(1, 9);
 		impl_->rgb_.copyTo(rgb_);
 		impl_->depth_.copyTo(depth_);
 	}

components/rgbd-sources/src/stereovideo.cpp

@@ -153,11 +153,29 @@ static void disparityToDepth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat
 	cv::cuda::divide(val, disparity, depth, 1.0f / 1000.0f, -1, stream);
 }
 
-bool StereoVideoSource::grab(int n, int b) {	
+bool StereoVideoSource::capture() {
+	lsrc_->get(cap_left_, cap_right_, stream2_);
+	stream2_.waitForCompletion();
+	return true;
+}
+
+void StereoVideoSource::swap() {
+	cv::cuda::GpuMat tmp;
+	tmp = left_;
+	left_ = cap_left_;
+	cap_left_ = tmp;
+	tmp = right_;
+	right_ = cap_right_;
+	cap_right_ = tmp;
+}
+
+bool StereoVideoSource::compute(int n, int b) {	
 	const ftl::rgbd::channel_t chan = host_->getChannel();
 
+	if (left_.empty() || right_.empty()) return false;
+
 	if (chan == ftl::rgbd::kChanDepth) {
-		lsrc_->get(left_, right_, stream_);
+		//lsrc_->get(left_, right_, stream_);
 		if (depth_tmp_.empty()) depth_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
 		if (disp_tmp_.empty()) disp_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
 		calib_->rectifyStereo(left_, right_, stream_);
@@ -169,13 +187,13 @@ bool StereoVideoSource::grab(int n, int b) {
 
 		stream_.waitForCompletion();  // TODO:(Nick) Move to getFrames
 	} else if (chan == ftl::rgbd::kChanRight) {
-		lsrc_->get(left_, right_, stream_);
+		//lsrc_->get(left_, right_, stream_);
 		calib_->rectifyStereo(left_, right_, stream_);
 		left_.download(rgb_, stream_);
 		right_.download(depth_, stream_);
 		stream_.waitForCompletion();  // TODO:(Nick) Move to getFrames
 	} else {
-		lsrc_->get(left_, right_, stream_);
+		//lsrc_->get(left_, right_, stream_);
 		calib_->rectifyStereo(left_, right_, stream_);
 		//rgb_ = lsrc_->cachedLeft();
 		left_.download(rgb_, stream_);

components/rgbd-sources/src/stereovideo.hpp

@@ -26,7 +26,9 @@ class StereoVideoSource : public detail::Source {
 	StereoVideoSource(ftl::rgbd::Source*, const std::string &);
 	~StereoVideoSource();
 
-	bool grab(int n, int b);
+	void swap();
+	bool capture();
+	bool compute(int n, int b);
 	bool isReady();
 	Camera parameters(channel_t chan);
 
@@ -40,9 +42,12 @@ class StereoVideoSource : public detail::Source {
 	bool ready_;
 	
 	cv::cuda::Stream stream_;
+	cv::cuda::Stream stream2_;
 
 	cv::cuda::GpuMat left_;
 	cv::cuda::GpuMat right_;
+	cv::cuda::GpuMat cap_left_;
+	cv::cuda::GpuMat cap_right_;
 	cv::cuda::GpuMat disp_tmp_;
 	cv::cuda::GpuMat depth_tmp_;
 	

components/rgbd-sources/src/streamer.cpp

@@ -274,6 +274,7 @@ void Streamer::_swap(StreamSource *src) {
 		}
 
 		src->src->getFrames(src->rgb, src->depth);
+		src->src->swap();
 
 		//if (!src->rgb.empty() && src->prev_depth.empty()) {
 			//src->prev_depth = cv::Mat(src->rgb.size(), CV_16UC1, cv::Scalar(0));
@@ -325,14 +326,14 @@ void Streamer::_schedule() {
 
 		// There will be two jobs for this source...
 		//UNIQUE_LOCK(job_mtx_,lk);
-		jobs_ += 1 + kChunkCount;
+		jobs_ += 2 + kChunkCount;
 		//lk.unlock();
 
 		StreamSource *src = sources_[uri];
 		if (src == nullptr || src->jobs != 0) continue;
-		src->jobs = 1 + kChunkCount;
+		src->jobs = 2 + kChunkCount;
 
-		// Grab job
+		// Grab / capture job
 		ftl::pool.push([this,src](int id) {
 			//auto start = std::chrono::high_resolution_clock::now();
 
@@ -362,7 +363,7 @@ void Streamer::_schedule() {
 			last_frame_ = now/40;
 
 			try {
-				src->src->grab();
+				src->src->capture();
 			} catch (std::exception &ex) {
 				LOG(ERROR) << "Exception when grabbing frame";
 				LOG(ERROR) << ex.what();
@@ -387,6 +388,27 @@ void Streamer::_schedule() {
 			job_cv_.notify_one();
 		});
 
+		// Compute job
+		ftl::pool.push([this,src](int id) {
+			try {
+				src->src->compute();
+			} catch (std::exception &ex) {
+				LOG(ERROR) << "Exception when computing frame";
+				LOG(ERROR) << ex.what();
+			}
+			catch (...) {
+				LOG(ERROR) << "Unknown exception when computing frame";
+			}
+
+			src->jobs--;
+			_swap(src);
+
+			// Mark job as finished
+			std::unique_lock<std::mutex> lk(job_mtx_);
+			--jobs_;
+			job_cv_.notify_one();
+		});
+
 		// Create jobs for each chunk
 		for (int i=0; i<kChunkCount; ++i) {
 			// Add chunk job to thread pool

components/rgbd-sources/test/source_unit.cpp

@@ -26,7 +26,7 @@ class ImageSource : public ftl::rgbd::detail::Source {
 		last_type = "image";
 	}
 
-	bool grab(int n, int b) { return true; };
+	bool compute(int n, int b) { return true; };
 	bool isReady() { return true; };
 };
 
@@ -39,7 +39,7 @@ class StereoVideoSource : public ftl::rgbd::detail::Source {
 		last_type = "video";
 	}
 
-	bool grab(int n, int b) { return true; };
+	bool compute(int n, int b) { return true; };
 	bool isReady() { return true; };
 };
 
@@ -49,7 +49,7 @@ class NetSource : public ftl::rgbd::detail::Source {
 		last_type = "net";
 	}
 
-	bool grab(int n, int b) { return true; };
+	bool compute(int n, int b) { return true; };
 	bool isReady() { return true; };
 };
 
@@ -59,7 +59,7 @@ class SnapshotSource : public ftl::rgbd::detail::Source {
 		last_type = "snapshot";
 	}
 
-	bool grab(int n, int b) { return true; };
+	bool compute(int n, int b) { return true; };
 	bool isReady() { return true; };
 };
 
@@ -69,7 +69,7 @@ class RealsenseSource : public ftl::rgbd::detail::Source {
 		last_type = "realsense";
 	}
 
-	bool grab(int n, int b) { return true; };
+	bool compute(int n, int b) { return true; };
 	bool isReady() { return true; };
 };
 
@@ -79,7 +79,7 @@ class MiddleburySource : public ftl::rgbd::detail::Source {
 		last_type = "middlebury";
 	}
 
-	bool grab(int n, int b) { return true; };
+	bool compute(int n, int b) { return true; };
 	bool isReady() { return true; };
 };