diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 7557fecb3da30af7a818823904a9b71bba3bc057..b76dd27fac1a7f09a07612629c9d6aae8ef47eb3 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/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) { };
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 4efb97175e886ba821f752f7f154eddde0aa1f8f..55cb8554e9eded1b661a89cdf917025369467ad2 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/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);
diff --git a/components/rgbd-sources/src/group.cpp b/components/rgbd-sources/src/group.cpp
index f548806d5b78b00428269f1f4c7d7c5bced8a197..88b9afcdca48fd141ca6487637dc7c78f9181d87 100644
--- a/components/rgbd-sources/src/group.cpp
+++ b/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);
}
}
diff --git a/components/rgbd-sources/src/image.hpp b/components/rgbd-sources/src/image.hpp
index b389fd176ec246db3ca51180589673817897eb03..ccb6ede5c93ed32213a89cc65fe90ab5d1cfbc78 100644
--- a/components/rgbd-sources/src/image.hpp
+++ 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; };
};
diff --git a/components/rgbd-sources/src/middlebury_source.cpp b/components/rgbd-sources/src/middlebury_source.cpp
index d41d4e77b85d8320ef01572dcd6707af68b5b705..a707bf9fab06a212b4146065b414999d21bc9adb 100644
--- a/components/rgbd-sources/src/middlebury_source.cpp
+++ b/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;
}
diff --git a/components/rgbd-sources/src/middlebury_source.hpp b/components/rgbd-sources/src/middlebury_source.hpp
index 5f0e2be538a069747d633e3e4b9483eb8a7a75b2..25f58b05e7888ded29eed71454eb591fd30babed 100644
--- a/components/rgbd-sources/src/middlebury_source.hpp
+++ b/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:
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index 9a2fdf2af5b667630d9720102549e767b144c36d..135395c57a1bc1f9983308719e88f7f49d5df8cf 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/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);
diff --git a/components/rgbd-sources/src/net.hpp b/components/rgbd-sources/src/net.hpp
index b99dc3487a6f4ab1949779935e2ed41699e9f4b0..b3411f12cce2f1e0236475ca75f667429ac5e35d 100644
--- a/components/rgbd-sources/src/net.hpp
+++ b/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);
diff --git a/components/rgbd-sources/src/realsense_source.cpp b/components/rgbd-sources/src/realsense_source.cpp
index d6c6f487be89c3eafd4e3e8e6020272dd93e8e9d..df4c0fe2535426ac52808ea985911968efb74e15 100644
--- a/components/rgbd-sources/src/realsense_source.cpp
+++ b/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();
diff --git a/components/rgbd-sources/src/realsense_source.hpp b/components/rgbd-sources/src/realsense_source.hpp
index 2af26bbaffb99081e3311a70cb2c715c8fca5cee..3b2d70c76c28608a3faa96af7c927bc0011804f3 100644
--- a/components/rgbd-sources/src/realsense_source.hpp
+++ b/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:
diff --git a/components/rgbd-sources/src/snapshot_source.hpp b/components/rgbd-sources/src/snapshot_source.hpp
index 38b9d875ad9e9846d0c83e42d6b84668cfe7dd18..abc8fd76bb56d9790bb7af6e5098fab80120b8b7 100644
--- a/components/rgbd-sources/src/snapshot_source.hpp
+++ b/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();
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 1014e24401b4d2dccdc361de3d4331ff2815eacb..4a3b7093ba8a4a5863c67dc51693cdbe1276c1b4 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/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_);
}
diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp
index d7e24de78c41e5b126de3bea44efa176abf062ad..0d20780c275a43a2338fd9d81a6a98f13619c3a2 100644
--- a/components/rgbd-sources/src/stereovideo.cpp
+++ b/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_);
diff --git a/components/rgbd-sources/src/stereovideo.hpp b/components/rgbd-sources/src/stereovideo.hpp
index 7835742389330046a33b7f22289ac36e8cdab4d5..9d01da4cdebe0ca2bb2a1e91832ca016f7116645 100644
--- a/components/rgbd-sources/src/stereovideo.hpp
+++ b/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_;
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index 1d46ad7fd3c6ab9f35aa435bf6775927994eaceb..3fd69afe3fd6568f16ed9a9c9b489fde36bdc64a 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/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
diff --git a/components/rgbd-sources/test/source_unit.cpp b/components/rgbd-sources/test/source_unit.cpp
index b27b72e070f6e2116632b967699243a9e80926d8..1c80e8292ff27e3c2652e7453dff59a1acea7bda 100644
--- a/components/rgbd-sources/test/source_unit.cpp
+++ b/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; };
};