diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
index 17205d4c6fa1b1e352bc130e3932a010911cb295..227913e5de056f15380390201da96b66ec23e27f 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
@@ -19,20 +19,25 @@ static const channel_t kChanDisparity = 0x0008; // CV_32FC1
static const channel_t kChanDeviation = 0x0010;
static const channel_t kChanNormals = 0x0020;
static const channel_t kChanConfidence = 0x0040;
-static const channel_t kChanFlow = 0x0080; // CV_32FC2 or CV_16FC2 (fix)
+static const channel_t kChanFlow = 0x0080; // CV_16SC2 (format 10.5) from NVOF
static const channel_t kChanEnergy = 0x0100;
-//static const channel_t kChanLeftGray = 0x0200; // not used // CV_8UC1
-//static const channel_t kChanRightGray = 0x0400; // not used // CV_8UC1
+
+// should l/r gray be removed (not that expensive to re-calculate if needed)?
+static const channel_t kChanLeftGray = 0x0200; // CV_8UC1
+static const channel_t kChanRightGray = 0x0400; // CV_8UC1
static const channel_t kChanOverlay1 = 0x1000;
// maximum number of available channels
-static const unsigned int n_channels = 11;
+static const unsigned int n_channels = 13;
inline bool isFloatChannel(ftl::rgbd::channel_t chan) {
return (chan == ftl::rgbd::kChanDepth || chan == ftl::rgbd::kChanEnergy);
}
+// TODO: interpolation for scaling depends on channel type;
+// NN for depth/disparity/optflow, linear/cubic/etc. for RGB
+
class Frame;
class Frame {
@@ -100,8 +105,8 @@ private:
case kChanConfidence: return 7;
case kChanFlow: return 8;
case kChanEnergy: return 9;
-// case kChanLeftGray: return 11;
-// case kChanRightGray: return 12;
+ case kChanLeftGray: return 11;
+ case kChanRightGray: return 12;
// should not happen (error); returned index is kChanNone
default: return 0;
}
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index 0d8cfa877e730b0b9b7e847eb2f4cf09a524fc86..782338fc2be41d56a4d6fcd4f4920f6d920e663f 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -12,11 +12,9 @@ using cv::cuda::GpuMat;
FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
ssgm_ = nullptr;
+ const int width = size_.width;
+ const int height = size_.height;
- int width = value("width", 1280);
- int height = value("height", 720);
-
- size_ = cv::Size(width, height);
CHECK((width >= 480) && (height >= 360));
uniqueness_ = value("uniqueness", 0.95f);
@@ -66,8 +64,22 @@ void FixstarsSGM::init(const cv::Size size) {
void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
{
- const auto &l = frame.getChannel<GpuMat>(ftl::rgbd::kChanLeft);
- const auto &r = frame.getChannel<GpuMat>(ftl::rgbd::kChanRight);
+ /*if (!frame.hasChannel(ftl::rgbd::kChanLeftGray))
+ {
+ auto &rgb = frame.getChannel<GpuMat>(ftl::rgbd::kChanLeft, stream);
+ auto &gray = frame.setChannel<GpuMat>(ftl::rgbd::kChanLeftGray);
+ cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream);
+ }
+
+ if (!frame.hasChannel(ftl::rgbd::kChanRightGray))
+ {
+ auto &rgb = frame.getChannel<GpuMat>(ftl::rgbd::kChanRight, stream);
+ auto &gray = frame.setChannel<GpuMat>(ftl::rgbd::kChanRightGray);
+ cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream);
+ }*/
+
+ const auto &l = frame.getChannel<GpuMat>(ftl::rgbd::kChanLeft, stream);
+ const auto &r = frame.getChannel<GpuMat>(ftl::rgbd::kChanRight, stream);
auto &disp = frame.setChannel<GpuMat>(ftl::rgbd::kChanDisparity);
if (disp.size() != l.size())
@@ -75,45 +87,48 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
disp = GpuMat(l.size(), CV_32FC1);
}
- if (l.size() != size_) {
- // re-use same buffer for l/r
- cv::cuda::resize(r, l_downscaled_, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
- cv::cuda::cvtColor(l_downscaled_, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
- cv::cuda::resize(l, l_downscaled_, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
- cv::cuda::cvtColor(l_downscaled_, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
+ GpuMat l_scaled;
+ if (l.size() != size_)
+ {
+ GpuMat _r;
+ scaleInput(l, r, l_scaled, _r, stream);
+ cv::cuda::cvtColor(l_scaled, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
+ cv::cuda::cvtColor(_r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
}
- else {
+ else
+ {
cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
}
stream.waitForCompletion();
-
ssgm_->execute(lbw_.data, rbw_.data, dispt_.data);
-
GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows));
left_pixels.setTo(0);
cv::cuda::threshold(dispt_, dispt_, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, stream);
// TODO: filter could be applied after upscaling (to the upscaled disparity image)
- if (use_filter_) {
- filter_->apply(dispt_,
- l.size() != dispt_.size() ? l_downscaled_ : l,
+ if (use_filter_)
+ {
+ filter_->apply(
+ dispt_,
+ (l.size() == size_) ? l : l_scaled,
dispt_,
stream
);
}
- if (l.size() != size_) {
- cv::cuda::multiply(dispt_, (double)l.cols / (double)size_.width, dispt_);
- // invalid areas (bad values) have to be taken into account in interpolation
- cv::cuda::resize(dispt_, dispt_full_res_, l.size(), 0.0, 0.0, cv::INTER_NEAREST, stream);
+ GpuMat dispt_scaled;
+ if (l.size() != size_)
+ {
+ scaleDisparity(l.size(), dispt_, dispt_scaled, stream);
}
- else {
- dispt_full_res_ = dispt_;
+ else
+ {
+ dispt_scaled = dispt_;
}
- dispt_full_res_.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
+ dispt_scaled.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
#ifdef HAVE_OPTFLOW
if (use_off_)
@@ -127,9 +142,7 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
void FixstarsSGM::setMask(Mat &mask) {
return; // TODO(Nick) Not needed, but also code below does not work with new GPU pipeline
CHECK(mask.type() == CV_8UC1) << "mask type must be CV_8U";
-
if (!ssgm_) { init(size_); }
-
- mask_l_ = mask;
+ mask_l_ = GpuMat(mask);
ssgm_->setMask((uint8_t*)mask.data, mask.cols);
}
\ No newline at end of file
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
index 14f80bf89408bec04a4556c0d2592fe0b09a3681..039db0c874d8d816b91d447a6254c79fd5a43304 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -44,7 +44,6 @@ namespace ftl {
float uniqueness_;
int P1_;
int P2_;
- cv::Size size_;
bool use_filter_;
bool use_off_;
cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
@@ -53,9 +52,6 @@ namespace ftl {
cv::cuda::GpuMat rbw_;
cv::cuda::GpuMat dispt_;
- cv::cuda::GpuMat l_downscaled_;
- cv::cuda::GpuMat dispt_full_res_;
-
#ifdef HAVE_OPTFLOW
ftl::rgbd::OFDisparityFilter off_;
#endif
diff --git a/components/rgbd-sources/src/disparity.cpp b/components/rgbd-sources/src/disparity.cpp
index e92c72de765c7dcd69da1aa279de9181e3170087..7d9089c1ab5a2e47bd26ed87591ddb411dabf7f8 100644
--- a/components/rgbd-sources/src/disparity.cpp
+++ b/components/rgbd-sources/src/disparity.cpp
@@ -15,7 +15,11 @@ std::map<std::string, std::function<Disparity*(ftl::Configurable *, const std::s
Disparity::Disparity(nlohmann::json &config)
: ftl::Configurable(config),
min_disp_(value("minimum",0)),
- max_disp_(value("maximum", 256)) {}
+ max_disp_(value("maximum", 256)),
+ size_(value("width", 1280), value("height", 720))
+ {
+
+ }
Disparity *Disparity::create(ftl::Configurable *parent, const std::string &name) {
nlohmann::json &config = ftl::config::resolve((!parent->getConfig()[name].is_null()) ? parent->getConfig()[name] : ftl::config::resolve(parent->getConfig())[name]); // ftl::config::resolve(parent->getConfig()[name]);
@@ -37,6 +41,28 @@ void Disparity::_register(const std::string &n,
(*algorithms__)[n] = f;
}
+void Disparity::scaleInput( const cv::cuda::GpuMat& left_in,
+ const cv::cuda::GpuMat& right_in,
+ cv::cuda::GpuMat& left_out,
+ cv::cuda::GpuMat& right_out,
+ cv::cuda::Stream &stream)
+{
+ cv::cuda::resize(left_in, left_scaled_, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
+ left_out = left_scaled_;
+ cv::cuda::resize(right_in, right_scaled_, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
+ right_out = right_scaled_;
+}
+
+void Disparity::scaleDisparity( const cv::Size& new_size,
+ cv::cuda::GpuMat& in,
+ cv::cuda::GpuMat& out,
+ cv::cuda::Stream& stream)
+{
+ cv::cuda::multiply(in, (double) new_size.width / (double) in.cols, in);
+ cv::cuda::resize(in, dispt_scaled_, new_size, 0.0, 0.0, cv::INTER_NEAREST, stream);
+ out = dispt_scaled_;
+}
+
// TODO:(Nick) Add remaining algorithms
/*
#include "algorithms/rtcensus.hpp"
diff --git a/components/rgbd-sources/src/disparity.hpp b/components/rgbd-sources/src/disparity.hpp
index a5e7efea79e9572f095f01346a9574257da332fd..2ab8223ca97f14752265be86ccf0ace0554eb20e 100644
--- a/components/rgbd-sources/src/disparity.hpp
+++ b/components/rgbd-sources/src/disparity.hpp
@@ -27,8 +27,20 @@ class Disparity : public ftl::Configurable {
virtual void setMinDisparity(size_t min) { min_disp_ = min; }
virtual void setMaxDisparity(size_t max) { max_disp_ = max; }
- virtual void setMask(cv::Mat &mask) { mask_l_ = mask; }
+ virtual void setMask(cv::Mat &mask) { mask_l_ = cv::cuda::GpuMat(mask); }
+ virtual void setMask(cv::cuda::GpuMat &mask) { mask_l_ = mask; }
+ void scaleInput(const cv::cuda::GpuMat& left_in,
+ const cv::cuda::GpuMat& right_in,
+ cv::cuda::GpuMat& left_out,
+ cv::cuda::GpuMat& right_out,
+ cv::cuda::Stream &stream);
+
+ void scaleDisparity(const cv::Size &new_size,
+ cv::cuda::GpuMat& in,
+ cv::cuda::GpuMat& out,
+ cv::cuda::Stream &stream);
+
/**
* Pure virtual function representing the actual computation of
* disparity from left and right images to be implemented.
@@ -42,7 +54,6 @@ class Disparity : public ftl::Configurable {
frame.setChannel<cv::cuda::GpuMat>(kChanDisparity) = disp;
}
-
/**
* Factory registration class.
*/
@@ -63,11 +74,15 @@ class Disparity : public ftl::Configurable {
protected:
static void _register(const std::string &n, std::function<Disparity*(ftl::Configurable *, const std::string &)> f);
- protected:
- //nlohmann::json &config_;
+protected:
int min_disp_;
int max_disp_;
- cv::Mat mask_l_;
+ cv::Size size_;
+
+ cv::cuda::GpuMat left_scaled_;
+ cv::cuda::GpuMat right_scaled_;
+ cv::cuda::GpuMat dispt_scaled_;
+ cv::cuda::GpuMat mask_l_;
private:
static std::map<std::string,std::function<Disparity*(ftl::Configurable *, const std::string &)>> *algorithms__;
@@ -78,4 +93,3 @@ class Disparity : public ftl::Configurable {
}
#endif // _FTL_DISPARITY_HPP_
-
diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp
index 05f0f67654569ff726948d1d10543191ebdff550..ebc72d854e8d35a480f3d4ef5873e8b58d5bb086 100644
--- a/components/rgbd-sources/src/stereovideo.cpp
+++ b/components/rgbd-sources/src/stereovideo.cpp
@@ -61,14 +61,13 @@ void StereoVideoSource::init(const string &file)
frames_ = std::vector<Frame>(2);
#ifdef HAVE_OPTFLOW
-/*
- use_optflow_ = host_->value("use_optflow", false);
+ use_optflow_ = host_->value("use_optflow", false);
LOG(INFO) << "Using optical flow: " << (use_optflow_ ? "true" : "false");
nvof_ = cv::cuda::NvidiaOpticalFlow_1_0::create(size.width, size.height,
cv::cuda::NvidiaOpticalFlow_1_0::NV_OF_PERF_LEVEL_SLOW,
true, false, false, 0);
-*/
+
#endif
calib_ = ftl::create<Calibrate>(host_, "calibration", size, stream_);
@@ -181,7 +180,6 @@ bool StereoVideoSource::retrieve() {
lsrc_->get(left, right, calib_, stream2_);
#ifdef HAVE_OPTFLOW
-/*
// see comments in https://gitlab.utu.fi/nicolas.pope/ftl/issues/155
if (use_optflow_)
@@ -198,10 +196,10 @@ bool StereoVideoSource::retrieve() {
auto &optflow = frame.setChannel<cv::cuda::GpuMat>(kChanFlow);
nvof_->calc(left_gray, left_gray_prev, optflow_, stream2_);
// nvof_->upSampler() isn't implemented with CUDA
- cv::cuda::resize(optflow_, optflow, left.size(), 0.0, 0.0, cv::INTER_NEAREST, stream2_);
+ // cv::cuda::resize() does not work wiht 2-channel input
+ // cv::cuda::resize(optflow_, optflow, left.size(), 0.0, 0.0, cv::INTER_NEAREST, stream2_);
}
}
-*/
#endif
stream2_.waitForCompletion();
diff --git a/components/rgbd-sources/src/stereovideo.hpp b/components/rgbd-sources/src/stereovideo.hpp
index 226f1c548826d10fd0e38f8c9da84344ab4105e4..9fe3ca529f2f32300cb85aa47bd958b78f78984c 100644
--- a/components/rgbd-sources/src/stereovideo.hpp
+++ b/components/rgbd-sources/src/stereovideo.hpp
@@ -51,11 +51,9 @@ class StereoVideoSource : public detail::Source {
cv::Mat mask_l_;
#ifdef HAVE_OPTFLOW
-/*
// see comments in https://gitlab.utu.fi/nicolas.pope/ftl/issues/155
cv::Ptr<cv::cuda::NvidiaOpticalFlow_1_0> nvof_;
cv::cuda::GpuMat optflow_;
-*/
#endif
void init(const std::string &);