sgm: support for different resolution disparity

components/rgbd-sources/src/algorithms/fixstars_sgm.cpp

@@ -51,26 +51,28 @@ void FixstarsSGM::init(const cv::Size size) {
 void FixstarsSGM::compute(	const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
 							cv::cuda::GpuMat &disp, cv::cuda::Stream &stream)
 {
+	GpuMat downscale_l;
+
 	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);
+		// re-use same buffer for l/r
+		cv::cuda::resize(r, downscale_l, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
+		cv::cuda::cvtColor(downscale_l, rbw_, 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);
+		cv::cuda::resize(l, downscale_l, size_, 0.0, 0.0, cv::INTER_CUBIC, stream);
+		cv::cuda::cvtColor(downscale_l, 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_) {
 		// issue #145 has to be fixed before can be moved into constructor
 		init(size_);
 	}
 
-	stream.waitForCompletion();
-
 	ssgm_->execute(lbw_.data, rbw_.data, dispt_.data);
 
 	GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows));
@@ -78,18 +80,25 @@ void FixstarsSGM::compute(	const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
 
 	cv::cuda::threshold(dispt_, dispt_, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, stream);
 
-	GpuMat dispt;
-	if (l.size() != size_)
-	{
-		cv::cuda::multiply(dispt_, (double) l.cols / (double)size_.width, dispt_);
+	// TODO: filter could be applied after upscaling (to the upscaled disparity image)
+	if (use_filter_) {
+		filter_->apply(dispt_,
+			l.size() != dispt_.size() ? downscale_l : 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_; }
+	else {
+		dispt = dispt_;
+	}
 
-	if (use_filter_) { filter_->apply(dispt, l, dispt, stream); }
-	
 	dispt.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
 }