feature/vision parameters

Closes #226 (closed)

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

@@ -27,28 +27,66 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
 	CHECK(P1_ >= 0);
 	CHECK(P2_ > P1_);
 
-	use_filter_ = value("use_filter", false);
-	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";
+	updateBilateralFilter();
 
-		filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter);
-	}
+	on("use_filter", [this](const ftl::config::Event&) {
+		updateBilateralFilter();
+	});
+
+	on("filter_radius", [this](const ftl::config::Event&) {
+		updateBilateralFilter();
+	});
+
+	on("filter_iter", [this](const ftl::config::Event&) {
+		updateBilateralFilter();
+	});
 	
 #ifdef HAVE_OPTFLOW
-	use_off_ = value("use_off", false);
+	updateOFDisparityFilter();
 
-	if (use_off_)
-	{
-		int off_size = value("off_size", 9);
-		double off_threshold = value("off_threshold", 0.9);
-		off_ = ftl::rgbd::OFDisparityFilter(size_, off_size, off_threshold);
-	}
+	on("use_off", [this](const ftl::config::Event&) {
+		updateOFDisparityFilter();
+	});
+
+	on("use_off", [this](const ftl::config::Event&) {
+		updateOFDisparityFilter();
+	});
 #endif
 
 	init(size_);
+
+	on("uniqueness", [this](const ftl::config::Event&) {
+		float uniqueness = value("uniqueness", uniqueness_);
+		if ((uniqueness >= 0.0) && (uniqueness <= 1.0)) {
+			uniqueness_ = uniqueness;
+			updateParameters();
+		}
+		else {
+			LOG(WARNING) << "invalid uniquness: " << uniqueness;
+		}
+	});
+
+	on("P1", [this](const ftl::config::Event&) {
+		int P1 = value("P1", P1_);
+		if (P1 <= P2_) {
+			P1_ = P1;
+			updateParameters();
+		}
+		else {
+			LOG(WARNING) << "invalid P1: " << P1 << ", (P1 <= P2), P2 is " << P2_;
+		}
+	});
+
+	on("P2", [this](const ftl::config::Event&) {
+		int P2 = value("P2", P2_);
+		if (P2 >= P1_) {
+			P2_ = P2;
+			updateParameters();
+		}
+		else {
+			LOG(WARNING) << "invalid P2: " << P2 << ", (P1 <= P2), P1 is " << P1_;
+		}
+	});
 }
 
 void FixstarsSGM::init(const cv::Size size) {
@@ -64,6 +102,69 @@ void FixstarsSGM::init(const cv::Size size) {
 	);
 }
 
+bool FixstarsSGM::updateParameters() {
+	if (ssgm_ == nullptr) { return false; }
+	return this->ssgm_->updateParameters(
+		sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true));
+}
+
+bool FixstarsSGM::updateBilateralFilter() {
+	bool enable = value("use_filter", false);
+	int radius = value("filter_radius", 25);
+	int iter = value("filter_iter", 1);
+
+	if (enable) {
+		if (radius <= 0) {
+			LOG(WARNING) << "filter_radius must be greater than 0";
+			enable = false;
+		}
+		if (iter <= 0) {
+			LOG(WARNING) << "filter_iter must be greater than 0";
+			enable = false;
+		}
+	}
+	
+	if (enable) {
+		filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter);
+		use_filter_ = true;
+	}
+	else {
+		use_filter_ = false;
+	}
+
+	return use_filter_;
+}
+
+#ifdef HAVE_OPTFLOW
+bool FixstarsSGM::updateOFDisparityFilter() {
+	bool enable = value("use_off", false);
+	int off_size = value("off_size", 9);
+	double off_threshold = value("off_threshold", 0.9);
+
+	if (enable) {
+		if (off_size <= 0) {
+			LOG(WARNING) << "bad off_size: " << off_size;
+			enable = false;
+		}
+
+		if (off_threshold <= 0.0) {
+			LOG(WARNING) << "bad off_threshold: " << off_threshold;
+			enable = false;
+		}
+	}
+	
+	if (enable) {
+		off_ = ftl::rgbd::OFDisparityFilter(size_, off_size, off_threshold);
+		use_off_ = true;
+	}
+	else {
+		use_off_ = false;
+	}
+	
+	return use_off_;
+}
+#endif
+
 void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
 {
 	/*if (!frame.hasChannel(ftl::rgbd::kChanLeftGray))