Disable sub pixel accuracy in libSGM if filter is used (accuracy is lost in...

Disable sub pixel accuracy in libSGM if filter is used (accuracy is lost in conversion when disparityBilateralFilter is used)

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

@@ -2,6 +2,7 @@
 
 #include "fixstars_sgm.hpp"
 #include <glog/logging.h>
+#include <opencv2/cudastereo.hpp>
 
 using ftl::algorithms::FixstarsSGM;
 using cv::Mat;
@@ -10,6 +11,8 @@ using cv::Mat;
 
 FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
 	ssgm_ = nullptr;
+	use_filter_ = config.value("use_filter", false);
+	filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_, config.value("filter_radius", 25), config.value("filter_iter", 1));
 }
 
 void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
@@ -26,7 +29,7 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
 			sgm::StereoSGM::Parameters(10,120,0.95f,true));
 	}
 
-	disp = Mat(cv::Size(l.cols, l.rows), CV_16UC1);
+	disp = Mat(cv::Size(l.cols, l.rows), CV_16SC1);
 
 	//auto start = std::chrono::high_resolution_clock::now();
 	ssgm_->execute(lbw.data, rbw.data, disp.data);
@@ -35,8 +38,20 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
 	//LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s";
 	
 	// todo: fix libSGM (return float data or provide mask separately)
-	// (256 << 5) coded in libSGM consistency check
+	// disparity values set to (256 << 5) in libSGM consistency check 
 	Mat bad_pixels = (disp == (256 << 5)); 
+	disp.setTo(0, bad_pixels);
+	
+	if (use_filter_) {
+		cv::cuda::GpuMat l_gpu, disp_gpu, disp_gpu_out;
+		// parameters need benchmarking, impact of image
+		// quick tests show with parameters (max_disp_, 25, 3)
+		// roughly 50% in disparity calculation and 50% in filter
+		disp_gpu.upload(disp);
+		l_gpu.upload(l);
+		filter_->apply(disp_gpu, l_gpu, disp_gpu_out);
+		disp_gpu_out.download(disp);
+	}
+	
 	disp.convertTo(disp, CV_32F, 1.0f/16.0f);
-	disp.setTo(0, bad_pixels); // decide how bad values should be represented
 }
\ No newline at end of file

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

@@ -9,6 +9,7 @@
 #include <opencv2/opencv.hpp>
 #include <libsgm.h>
 #include "../disparity.hpp"
+#include <opencv2/cudastereo.hpp>
 
 namespace ftl {
 namespace algorithms {
@@ -32,6 +33,8 @@ class FixstarsSGM : public ftl::Disparity {
 	}
 
 	private:
+	bool use_filter_;
+	cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
 	sgm::StereoSGM *ssgm_;
 };
 };

config/config.json

@@ -51,7 +51,10 @@
 			"gamma": 0.0,
 			"window_size": 5,
 			"sigma": 1.5,
-			"lambda": 8000.0
+			"lambda": 8000.0,
+			"use_filter": true,
+			"filter_radius": 20,
+			"filter_iter": 3
 		},
 		"display": {
 			"flip_vert": false,