From 96e7502d060ae13cf0f1ed6d6db526aa07c1dfd3 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Tue, 6 Aug 2019 18:34:18 +0300
Subject: [PATCH] sgm: support for different resolution disparity

---
 .../src/algorithms/fixstars_sgm.cpp           | 71 +++++++++++++------
 .../src/algorithms/fixstars_sgm.hpp           |  1 +
 2 files changed, 51 insertions(+), 21 deletions(-)

diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index 8be3530fa..6392add64 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,7 +33,7 @@ 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);
 	}
 }
@@ -34,42 +41,64 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
 void FixstarsSGM::init(const cv::Size size) {
 	if (ssgm_) { delete ssgm_; }
 	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_) {
+		GpuMat tmp;
+		cv::cuda::resize(l, tmp, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
+		cv::cuda::cvtColor(tmp, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
+
+		cv::cuda::resize(r, tmp, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
+		cv::cuda::cvtColor(tmp, rbw_, 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);
+	}
+
+	if (!ssgm_) {
+		// issue #145 has to be fixed before can be moved into constructor
+		init(size_);
+	}
 
 	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); }
+	GpuMat dispt;
+	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, l.size(), 0.0, 0.0, cv::INTER_NEAREST, stream);
+	}
+	else { dispt = dispt_; }
+
+	if (use_filter_) { filter_->apply(dispt, l, dispt, stream); }
 	
-	dispt_.convertTo(disp, CV_32F, 1.0f/16.0f, stream);
+	dispt.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..67221fe8a 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -42,6 +42,7 @@ class FixstarsSGM : public ftl::rgbd::detail::Disparity {
 	float uniqueness_;
 	int P1_;
 	int P2_;
+	cv::Size size_;
 	bool use_filter_;
 	cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
 	sgm::StereoSGM *ssgm_;
-- 
GitLab