From 6371f13814d2beefa7dbe6b3c07ed6e87fef4483 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Wed, 7 Aug 2019 15:16:23 +0300
Subject: [PATCH] sgm: support for different resolution disparity
---
.../src/algorithms/fixstars_sgm.cpp | 79 +++++++++++++------
.../src/algorithms/fixstars_sgm.hpp | 74 +++++++++--------
2 files changed, 95 insertions(+), 58 deletions(-)
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index 8be3530fa..b6a860db2 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -12,10 +12,17 @@ using cv::cuda::GpuMat;
FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
ssgm_ = nullptr;
+
+ 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);
P1_ = value("P1", 10);
P2_ = value("P2", 120);
-
+
CHECK((uniqueness_ >= 0.0) && (uniqueness_ <= 1.0));
CHECK(P1_ >= 0);
CHECK(P2_ > P1_);
@@ -26,50 +33,76 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
int iter = value("filter_iter", 1);
CHECK(radius > 0) << "filter_radius must be greater than 0";
CHECK(iter > 0) << "filter_iter must be greater than 0";
-
+
filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter);
}
+
+ init(size_);
}
void FixstarsSGM::init(const cv::Size size) {
if (ssgm_) { delete ssgm_; }
+ lbw_ = GpuMat(size, CV_8UC1);
+ rbw_ = GpuMat(size, CV_8UC1);
dispt_ = GpuMat(size, CV_16SC1);
- ssgm_ = new sgm::StereoSGM( size.width, size.height, max_disp_, 8, 16,
- lbw_.step, dispt_.step / sizeof(short),
- sgm::EXECUTE_INOUT_CUDA2CUDA,
- sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true)
+
+ ssgm_ = new sgm::StereoSGM(size.width, size.height, max_disp_, 8, 16,
+ lbw_.step, dispt_.step / sizeof(short),
+ sgm::EXECUTE_INOUT_CUDA2CUDA,
+ sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true)
);
}
-void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) {
- cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
- cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
+void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
+ cv::cuda::GpuMat &disp, cv::cuda::Stream &stream)
+{
+ 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);
+ }
+ else {
+ cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
+ cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
+ }
stream.waitForCompletion();
- if (!ssgm_) { init(l.size()); }
- //auto start = std::chrono::high_resolution_clock::now();
ssgm_->execute(lbw_.data, rbw_.data, dispt_.data);
- //std::chrono::duration<double> elapsed =
- // std::chrono::high_resolution_clock::now() - start;
- //LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s";
-
+
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);
- if (use_filter_) { filter_->apply(dispt_, l, dispt_, stream); }
-
- dispt_.convertTo(disp, CV_32F, 1.0f/16.0f, 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,
+ 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);
+ }
+ else {
+ dispt_full_res_ = dispt_;
+ }
+
+ dispt_full_res_.convertTo(disp, CV_32F, 1.0f / 16.0f, 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(mask.size()); }
-
+
+ if (!ssgm_) { init(size_); }
+
mask_l_ = mask;
- ssgm_->setMask((uint8_t*) mask.data, mask.cols);
+ 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 610fa91d9..08d01a535 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -15,41 +15,45 @@
#include "ftl/cb_segmentation.hpp"
namespace ftl {
-namespace algorithms {
-
-/**
- * Fixstars libSGM stereo matcher.
- * @see https://github.com/fixstars/libSGM
- *
- * NOTE: We are using a modified version that supports disparity of 256.
- * @see https://github.com/knicos/libSGM
- */
-class FixstarsSGM : public ftl::rgbd::detail::Disparity {
- public:
- explicit FixstarsSGM(nlohmann::json &config);
-
- void compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) override;
- void setMask(cv::Mat &mask) override;
-
- /* Factory creator */
- static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
- return ftl::create<FixstarsSGM>(p, name);
- }
-
- private:
- void init(const cv::Size size);
-
- float uniqueness_;
- int P1_;
- int P2_;
- bool use_filter_;
- cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
- sgm::StereoSGM *ssgm_;
- cv::cuda::GpuMat lbw_;
- cv::cuda::GpuMat rbw_;
- cv::cuda::GpuMat dispt_;
-};
-};
+ namespace algorithms {
+
+ /**
+ * Fixstars libSGM stereo matcher.
+ * @see https://github.com/fixstars/libSGM
+ *
+ * NOTE: We are using a modified version that supports disparity of 256.
+ * @see https://github.com/knicos/libSGM
+ */
+ class FixstarsSGM : public ftl::rgbd::detail::Disparity {
+ public:
+ explicit FixstarsSGM(nlohmann::json &config);
+
+ void compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) override;
+ void setMask(cv::Mat &mask) override;
+
+ /* Factory creator */
+ static inline Disparity *create(ftl::Configurable *p, const std::string &name) {
+ return ftl::create<FixstarsSGM>(p, name);
+ }
+
+ private:
+ void init(const cv::Size size);
+
+ float uniqueness_;
+ int P1_;
+ int P2_;
+ cv::Size size_;
+ bool use_filter_;
+ cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
+ sgm::StereoSGM *ssgm_;
+ cv::cuda::GpuMat lbw_;
+ cv::cuda::GpuMat rbw_;
+ cv::cuda::GpuMat dispt_;
+
+ cv::cuda::GpuMat l_downscaled_;
+ cv::cuda::GpuMat dispt_full_res_;
+ };
+ };
};
#endif // _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
--
GitLab