Merge branch 'feature/vision-parameters' into 'master'

feature/vision parameters

Closes #226

See merge request nicolas.pope/ftl!157

applications/reconstruct/src/main.cpp

@@ -72,6 +72,48 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
 	return rz * rx * ry;
 }
 
+// TODO:	Remove this class (requires more general solution). Also does not
+//		 	process disconnections/reconnections/types etc. correctly.
+class ConfigProxy {
+	private:
+	vector<ftl::UUID> peers_;
+	vector<std::string> uris_;
+	ftl::net::Universe *net_;
+	
+	public:
+	ConfigProxy(ftl::net::Universe *net) {
+		net_ = net;
+
+		auto response = net_->findAll<std::string>("node_details");
+		for (auto &r : response) {
+			auto r_json = json_t::parse(r);
+			peers_.push_back(ftl::UUID(r_json["id"].get<std::string>()));
+			uris_.push_back(r_json["title"].get<std::string>());
+		}
+	}
+
+	void add(ftl::Configurable *root, const std::string &uri, const std::string &name) {
+		auto config = json_t::parse(net_->call<string>(peers_[0], "get_cfg", uris_[0] + "/" + uri));
+		auto *proxy = ftl::create<ftl::Configurable>(root, name);
+		
+		for (auto &itm : config.get<json::object_t>()) {
+			auto key = itm.first;
+			auto value = itm.second;
+			if (*key.begin() == '$') { continue; }
+
+			proxy->set(key, value);
+			proxy->on(key, [this, uri, key, value, proxy](const ftl::config::Event&) {
+				for (size_t i = 0; i < uris_.size(); i++) {
+					// TODO: check that config exists?
+					auto peer = peers_[i];
+					std::string name = uris_[i] + "/" + uri + "/" + key;
+					net_->send(peer, "update_cfg", name, proxy->getConfig()[key].dump());
+				}
+			});
+		}
+	}
+};
+
 static void run(ftl::Configurable *root) {
 	Universe *net = ftl::create<Universe>(root, "net");
 	ftl::ctrl::Slave slave(net, root);
@@ -122,6 +164,9 @@ static void run(ftl::Configurable *root) {
 		return;
 	}
 
+	auto configproxy = ConfigProxy(net);
+	configproxy.add(root, "source/disparity", "disparity");
+
 	// Create scene transform, intended for axis aligning the walls and floor
 	Eigen::Matrix4d transform;
 	if (root->getConfig()["transform"].is_object()) {

components/rgbd-sources/include/ftl/offilter.hpp

@@ -17,6 +17,7 @@ public:
 	OFDisparityFilter() : n_max_(0), threshold_(0.0) {}
 	OFDisparityFilter(cv::Size size, int n_frames, float threshold);
 	void filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream);
+	void filter(cv::cuda::GpuMat &disp, cv::cuda::GpuMat &optflow, cv::cuda::Stream &stream);
 
 private:
 	int n_max_;

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,22 +102,72 @@ void FixstarsSGM::init(const cv::Size size) {
 	);
 }
 
-void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
-{
-	/*if (!frame.hasChannel(ftl::rgbd::kChanLeftGray))
-	{
-		auto &rgb = frame.getChannel<GpuMat>(ftl::rgbd::kChanLeft, stream);
-		auto &gray = frame.setChannel<GpuMat>(ftl::rgbd::kChanLeftGray);
-		cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream);
+bool FixstarsSGM::updateParameters() {
+	if (ssgm_ == nullptr) { return false; }
+	return this->ssgm_->updateParameters(
+		sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true));
 }
 
-	if (!frame.hasChannel(ftl::rgbd::kChanRightGray))
-	{
-		auto &rgb = frame.getChannel<GpuMat>(ftl::rgbd::kChanRight, stream);
-		auto &gray = frame.setChannel<GpuMat>(ftl::rgbd::kChanRightGray);
-		cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream);
-	}*/
+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) {
+		LOG(INFO) << "Optical flow filter, size: " << off_size << ", threshold: " << off_threshold;
+		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)
+{
 	const auto &l = frame.get<GpuMat>(Channel::Left);
 	const auto &r = frame.get<GpuMat>(Channel::Right);
 	auto &disp = frame.create<GpuMat>(Channel::Disparity, Format<float>(l.size()));
@@ -100,21 +188,10 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
 
 	stream.waitForCompletion();
 	ssgm_->execute(lbw_.data, rbw_.data, dispt_.data);
-	GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows));
-	left_pixels.setTo(0);
+	// 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);
 
-	// TODO: filter could be applied after upscaling (to the upscaled disparity image)
-	if (use_filter_)
-	{
-		filter_->apply(
-			dispt_,
-			(l.size() == size_) ? l : l_scaled,
-			dispt_,
-			stream
-		);
-	}
-
 	GpuMat dispt_scaled;
 	if (l.size() != size_)
 	{
@@ -125,10 +202,26 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
 		dispt_scaled = dispt_;
 	}
 
+	// TODO: filter could be applied after upscaling (to the upscaled disparity image)
+	if (use_filter_)
+	{
+		filter_->apply(
+			dispt_,
+			l,
+			dispt_,
+			stream
+		);
+	}
+	
 	dispt_scaled.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
 
 #ifdef HAVE_OPTFLOW
-	if (use_off_) { off_.filter(frame, stream); }
+	// TODO: Optical flow filter expects CV_32F
+	if (use_off_) {
+		frame.upload(Channel::Flow, stream);
+		stream.waitForCompletion();
+		off_.filter(disp, frame.get<GpuMat>(Channel::Flow), stream);
+	}
 #endif
 }
 

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

@@ -41,6 +41,11 @@ namespace ftl {
 
 		private:
 			void init(const cv::Size size);
+			bool updateParameters();
+			bool updateBilateralFilter();
+			#ifdef HAVE_OPTFLOW
+			bool updateOFDisparityFilter();
+			#endif
 
 			float uniqueness_;
 			int P1_;

components/rgbd-sources/src/offilter.cpp

@@ -39,4 +39,14 @@ void OFDisparityFilter::filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream
 	ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream);
 }
 
+void OFDisparityFilter::filter(cv::cuda::GpuMat &disp, cv::cuda::GpuMat &optflow, cv::cuda::Stream &stream)
+{
+	if (disp.type() != CV_32FC1) {
+		LOG(ERROR) << "Optical flow filter expects CV_32FC1 (TODO)";
+		return;
+	}
+
+	ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream);
+}
+
 #endif  // HAVE_OPTFLOW