diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..41eb7509541f40dd15d9ae121a30139edf311311
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <ftl/rgbd/detail/source.hpp>
+#include <ftl/configuration.hpp>
+#include <opencv2/core.hpp>
+
+// https://stackoverflow.com/a/31718095/
+static uint32_t msbDeBruijn32(uint32_t v)
+{
+ static const int MultiplyDeBruijnBitPosition[32] =
+ {
+ 0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30,
+ 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31
+ };
+
+ v |= v >> 1; // first round down to one less than a power of 2
+ v |= v >> 2;
+ v |= v >> 4;
+ v |= v >> 8;
+ v |= v >> 16;
+
+ return MultiplyDeBruijnBitPosition[(uint32_t)(v * 0x07C4ACDDU) >> 27];
+}
+
+namespace ftl {
+namespace rgbd {
+
+class Frame {
+public:
+ /* @todo REMOVE HARDCODED CHANNEL COUNT!
+ */
+ Frame() : channels_(11), available_(11, false) {}
+
+ /* @brief Reset all channels without releasing memory
+ */
+ void reset()
+ {
+ std::fill(available_.begin(), available_.end(), false);
+ }
+
+ /* @brief Is there valid data in channel
+ */
+ bool hasChannel(const ftl::rgbd::channel_t& channel)
+ {
+ return available_[msbDeBruijn32(channel)];
+ }
+
+ /* @brief Get reference to the channel GpuMat
+ */
+ cv::cuda::GpuMat& getChannel(const ftl::rgbd::channel_t& channel)
+ {
+ return channels_[msbDeBruijn32(channel)];
+ }
+
+private:
+ std::vector<cv::cuda::GpuMat> channels_;
+ std::vector<bool> available_;
+};
+
+}
+}
\ No newline at end of file
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 4ddf48cdb50e463619113e31edfe79d2ec7a6807..1106d76220e676a3e7e36c5db18db9596a0f91d9 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -12,6 +12,7 @@
#include <string>
#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/frame.hpp>
namespace ftl {
diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp
index b162eb90b8ec92c0082eacf471fb02091d1cce0f..58a513a08f93619b22233478c79cff3a85600c0f 100644
--- a/components/rgbd-sources/src/stereovideo.cpp
+++ b/components/rgbd-sources/src/stereovideo.cpp
@@ -58,7 +58,7 @@ void StereoVideoSource::init(const string &file)
}
cv::Size size = cv::Size(lsrc_->width(), lsrc_->height());
- frames_ = std::vector<StereoVideoSource::Frame>(2); // triple-buffering for optical flow
+ frames_ = std::vector<Frame>(2);
#ifdef HAVE_OPTFLOW
// TODO make optional, can be calculated at later step
@@ -173,9 +173,12 @@ bool StereoVideoSource::capture(int64_t ts) {
bool StereoVideoSource::retrieve() {
auto &frame = frames_[0];
frame.reset();
- lsrc_->get(frame.left, frame.right, calib_, stream2_);
+ cv::cuda::GpuMat& left = frame.getChannel(ftl::rgbd::kChanLeft);
+ cv::cuda::GpuMat& right = frame.getChannel(ftl::rgbd::kChanRight);
+ lsrc_->get(left, right, calib_, stream2_);
#ifdef HAVE_OPTFLOW
+/*
if (nvof_)
{
cv::cuda::cvtColor(frame.left, frame.left_gray, cv::COLOR_BGR2GRAY, 0, stream2_);
@@ -190,6 +193,7 @@ bool StereoVideoSource::retrieve() {
frame.optflow_ready = true;
}
}
+*/
#endif
stream2_.waitForCompletion();
@@ -204,24 +208,31 @@ 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);
+
const ftl::rgbd::channel_t chan = host_->getChannel();
- if (frame.left.empty() || frame.right.empty()) return false;
+ if (left.empty() || right.empty()) return false;
+
+
if (chan == ftl::rgbd::kChanDepth) {
- if (frame.depth.empty()) frame.depth = cv::cuda::GpuMat(frame.left.size(), CV_32FC1);
- if (frame.disp.empty()) frame.disp = cv::cuda::GpuMat(frame.left.size(), CV_32FC1);
- disp_->compute(frame.left, frame.right, frame.disp, stream_);
- ftl::cuda::disparity_to_depth(frame.disp, frame.depth, params_, stream_);
- frame.left.download(rgb_, stream_);
- frame.depth.download(depth_, stream_);
+ 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_);
+ ftl::cuda::disparity_to_depth(disp, depth, params_, stream_);
+ left.download(rgb_, stream_);
+ depth.download(depth_, stream_);
stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
} else if (chan == ftl::rgbd::kChanRight) {
- frame.left.download(rgb_, stream_);
- frame.right.download(depth_, stream_);
+ left.download(rgb_, stream_);
+ right.download(depth_, stream_);
stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
} else {
- frame.left.download(rgb_, stream_);
+ left.download(rgb_, stream_);
stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
}
diff --git a/components/rgbd-sources/src/stereovideo.hpp b/components/rgbd-sources/src/stereovideo.hpp
index 18790991cf67b483d28b06e8e4288fc36785b7d1..cd5a0c4884ca533ad9b185fafce47b2dd4722dd9 100644
--- a/components/rgbd-sources/src/stereovideo.hpp
+++ b/components/rgbd-sources/src/stereovideo.hpp
@@ -45,33 +45,6 @@ class StereoVideoSource : public detail::Source {
cv::cuda::Stream stream_;
cv::cuda::Stream stream2_;
- // possibly should be made available for other users as well
- // (should be called StereoVideoFrame)
- class Frame {
- public:
- bool left_gray_ready;
- bool right_gray_ready;
- bool optflow_ready;
-
- cv::cuda::GpuMat left;
- cv::cuda::GpuMat right;
- cv::cuda::GpuMat left_gray;
- cv::cuda::GpuMat right_gray;
- cv::cuda::GpuMat disp;
- cv::cuda::GpuMat depth;
- cv::cuda::GpuMat optflow;
- cv::cuda::GpuMat optflow_; // original lower res optical flow
-
- Frame() { reset(); }
-
- void reset()
- {
- left_gray_ready = false;
- right_gray_ready = false;
- optflow_ready = false;
- }
- };
-
std::vector<Frame> frames_;
cv::Mat mask_l_;