libSGM: new parameters and parameter validation

* libSGM initialization to separate method
* P1 and P2 parameters
* parameter checks

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

@@ -13,8 +13,32 @@ using cv::cuda::GpuMat;
 FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
 	ssgm_ = nullptr;
 	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_);
+
 	use_filter_ = value("use_filter", false);
-	filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, value("filter_radius", 25), value("filter_iter", 1));
+	if (use_filter_) {
+		int radius = value("filter_radius", 25);
+		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);
+	}
+}
+
+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)
+	);
 }
 
 void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) {
@@ -22,11 +46,7 @@ void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
 	cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
 
 	stream.waitForCompletion();
-	if (!ssgm_) { // todo: move to constructor
-		dispt_ = GpuMat(l.rows, l.cols, CV_16SC1);
-		ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 16, lbw_.step, dispt_.step / sizeof(short),
-			sgm::EXECUTE_INOUT_CUDA2CUDA, sgm::StereoSGM::Parameters(10, 120, uniqueness_, true));
-	}
+	if (!ssgm_) { init(l.size()); }
 
 	//auto start = std::chrono::high_resolution_clock::now();
 	ssgm_->execute(lbw_.data, rbw_.data, dispt_.data);
@@ -34,22 +54,12 @@ void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
 	//		std::chrono::high_resolution_clock::now() - start;
 	//LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s";
 	
-	// todo: fix libSGM (return float data or provide mask separately)
-	// disparity values set to (256 << 5) in libSGM consistency check 
-	//Mat bad_pixels = (disp == (256 << 5)); 
-	
-	//disp.setTo(0, bad_pixels, stream_);
 	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_) {
-		// parameters need benchmarking, impact of image
-		// quick tests show with parameters (max_disp_, 25, 3)
-		// roughly 50% in disparity calculation and 50% in filter;
-		filter_->apply(dispt_, l, dispt_, stream);
-	}
+	if (use_filter_) { filter_->apply(dispt_, l, dispt_, stream); }
 	
 	dispt_.convertTo(disp, CV_32F, 1.0f/16.0f, stream);
 }
@@ -58,11 +68,7 @@ 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_) { // todo: move to constructor
-		ssgm_ = new sgm::StereoSGM(mask.cols, mask.rows, max_disp_, 8, 16,
-			sgm::EXECUTE_INOUT_HOST2HOST,
-			sgm::StereoSGM::Parameters(10, 120, uniqueness_, true));
-	}
+	if (!ssgm_) { init(mask.size()); }
 	
 	mask_l_ = mask;
 	ssgm_->setMask((uint8_t*) mask.data, mask.cols);

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

@@ -37,7 +37,11 @@ class FixstarsSGM : public ftl::rgbd::detail::Disparity {
 	}
 
 	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_;