Merge branch 'feature/middlebury-source' into 'master'

Update middlebury source

See merge request nicolas.pope/ftl!362

components/rgbd-sources/CMakeLists.txt

@@ -4,6 +4,8 @@ set(RGBDSRC
 	src/source.cpp
 	src/frame.cpp
 	#src/frameset.cpp
+    src/sources/middlebury/middlebury_source.cpp
+    src/sources/middlebury/loader.cpp
 	src/sources/stereovideo/stereovideo.cpp
 	#src/colour.cpp
 	#src/group.cpp

components/rgbd-sources/src/source.cpp

@@ -63,8 +63,8 @@ static ftl::rgbd::BaseSourceImpl *createFileImpl(const ftl::URI &uri, Source *ho
 		if (ftl::is_directory(path)) {
 			if (is_file(path + "/video.mp4")) {
 				return new StereoVideoSource(host, path);
-//			} else if (ftl::is_file(path + "/im0.png")) {
-//				return new MiddleburySource(this, path);
+			} else if (is_file(path + "/im0.png")) {
+				return new MiddleburySource(host, path);
 			} else {
 				LOG(ERROR) << "Directory is not a valid RGBD source: " << path;
 			}

components/rgbd-sources/src/sources/middlebury/loader.cpp

+#include "loader.hpp"
+
+#include <cmath>
+#include <cstdio>
+#include <fstream>
+#include <string>
+#include <iostream>
+#include <vector>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+
+#include <experimental/filesystem>
+namespace fs = std::experimental::filesystem;
+
+cv::Mat read_pfm(const std::string &filename) {
+	cv::Mat im;
+	FILE * fp;
+
+	fp = fopen(filename.c_str(), "rb");
+	char buf[32];
+	int fsize;
+	int width;
+	int height;
+	float scale;
+
+	if (fp == NULL) { return im; }
+
+	if (fscanf(fp, "%31s", buf) == 0 || strcmp(buf, "Pf")) { goto cleanup; }
+
+	if (fscanf(fp, "%31s", buf) == 0) { goto cleanup; }
+	width = atoi(buf);
+
+	if (fscanf(fp, "%31s", buf) == 0) { goto cleanup; }
+	height = atoi(buf);
+
+	if (fscanf(fp, " %31s", buf) == 0) { goto cleanup; }
+	scale = atof(buf);
+
+	im.create(height, width, CV_32FC1);
+
+	fseek(fp, 0, SEEK_END);
+	fseek(fp, ftell(fp)-width*height*sizeof(float), SEEK_SET);
+
+	for (int y = 0; y < height; y++) {
+		float* im_ptr = im.ptr<float>(height-y-1);
+		int nread = 0;
+		do {
+			nread += fread(im_ptr+nread, sizeof(float), width-nread, fp);
+			if (ferror(fp)) { goto cleanup; }
+		}
+		while (nread != width);
+	}
+
+	cleanup:
+	fclose(fp);
+	return im;
+}
+
+static void parse_camera_parameters(const std::string &v, MiddEvalCalib &calib) {
+	std::string mat = std::string(v.substr(1, v.size()-2));
+	std::string row;
+
+	std::vector<float> values;
+
+	for (int _ = 0; _ < 3; _++) {
+		size_t pos = mat.find(";");
+		row = mat.substr(0, pos);
+
+		std::istringstream sstr(row);
+		for(std::string val; sstr >> val;) {
+			values.push_back(atof(val.c_str()));
+		}
+
+		mat.erase(0, pos+1);
+	}
+
+	calib.f = values[0];
+	calib.cx = values[2];
+	calib.cy = values[5];
+}
+
+MiddEvalCalib read_calibration(const std::string &filename) {
+	MiddEvalCalib calib;
+	memset(&calib, 0, sizeof(calib));
+
+	std::ifstream f(filename);
+
+	for(std::string line; std::getline(f, line);) {
+		auto m = line.find("=");
+		if (m == std::string::npos) { continue; }
+		auto k = line.substr(0, m);
+		auto v = line.substr(m+1);
+
+		if (k == "baseline") {
+			calib.baseline = atof(v.c_str());
+		}
+		else if (k == "doffs") {
+			calib.doffs = atof(v.c_str());
+		}
+		else if (k == "ndisp") {
+			calib.ndisp = atoi(v.c_str());
+		}
+		else if (k == "vmin") {
+			calib.vmin = atoi(v.c_str());
+		}
+		else if (k == "vmax") {
+			calib.vmax = atoi(v.c_str());
+		}
+		else if (k == "width") {
+			calib.width = atoi(v.c_str());
+		}
+		else if (k == "height") {
+			calib.height = atoi(v.c_str());
+		}
+		else if (k == "cam0") {
+			parse_camera_parameters(v, calib);
+		}
+	}
+
+	if (calib.vmax == 0) {
+		calib.vmax = calib.ndisp;
+	}
+
+	return calib;
+}
+
+MiddleburyData load_input(const fs::path &path) {
+	cv::Mat imL;
+	cv::Mat imR;
+	cv::Mat gtL;
+	cv::Mat maskL;
+
+	imL = cv::imread(path/"im0.png", cv::IMREAD_COLOR);
+	imR = cv::imread(path/"im1.png", cv::IMREAD_COLOR);
+	gtL = read_pfm(path/"disp0.pfm");
+	if (gtL.empty()) {
+		gtL = read_pfm(path/std::string("disp0GT.pfm"));
+	}
+	if (gtL.empty()) {
+		gtL.create(imL.size(), CV_32FC1);
+		gtL.setTo(cv::Scalar(0.0f));
+	}
+
+	maskL = cv::imread(path/std::string("mask0nocc.png"), cv::IMREAD_GRAYSCALE);
+	if (maskL.empty()) {
+		maskL.create(imL.size(), CV_8UC1);
+		maskL.setTo(cv::Scalar(255));
+	}
+
+	auto calib = read_calibration(path/std::string("calib.txt"));
+	if (imL.empty() || imR.empty() || gtL.empty() || maskL.empty()) {
+		throw std::exception();
+	}
+
+	std::string name = path.filename() == "." ?
+		path.parent_path().filename().c_str() :
+		path.filename().c_str();
+
+	return {name, imL, imR, gtL, maskL, calib};
+}
+
+MiddleburyData load_input(const std::string &path) {
+	return load_input(fs::path(path));
+}

components/rgbd-sources/src/sources/middlebury/loader.hpp

+#pragma once
+
+#include <opencv2/core/mat.hpp>
+
+struct MiddEvalCalib {
+	float f;
+	float cx;
+	float cy;
+	float baseline;
+	float doffs;
+	int width;
+	int height;
+	int ndisp;
+	int vmin;
+	int vmax;
+};
+
+MiddEvalCalib read_calibration(const std::string &filename);
+
+cv::Mat read_pfm(const std::string &filename);
+
+struct MiddleburyData {
+	const std::string name;
+	const cv::Mat imL;
+	const cv::Mat imR;
+	const cv::Mat gtL;
+	const cv::Mat maskL;
+	const MiddEvalCalib calib;
+};
+
+/** Load one middlebury dataset image
+ * @param path path to image directory
+ */
+MiddleburyData load_input(const std::string &path);

components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp

+#include <ftl/rgbd/source.hpp>
 #include "middlebury_source.hpp"
-
-#include "disparity.hpp"
 #include "cuda_algorithms.hpp"
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include "loader.hpp"
 
-#include "cuda_algorithms.hpp"
+#include <loguru.hpp>
 
 using ftl::rgbd::detail::MiddleburySource;
-using ftl::rgbd::detail::Disparity;
 using std::string;
+using ftl::codecs::Channel;
+using cv::cuda::GpuMat;
+using ftl::rgbd::Capability;
 
 MiddleburySource::MiddleburySource(ftl::rgbd::Source *host)
 		: ftl::rgbd::BaseSourceImpl(host), ready_(false) {
@@ -61,30 +65,24 @@ static bool loadMiddleburyCalib(const std::string &filename, ftl::rgbd::Camera &
 	return false;
 }
 
+bool MiddleburySource::supported() {
+	return true;
+}
+
 MiddleburySource::MiddleburySource(ftl::rgbd::Source *host, const string &dir)
 		: ftl::rgbd::BaseSourceImpl(host), ready_(false) {
 
 	double scaling = host->value("scaling", 0.5);
 
-	capabilities_ = kCapStereo;
-
-	// Load params from txt file..
-	/*params_.fx = 3000.0 * scaling;
-	params_.width = 3000.0 * scaling;
-	params_.height = 1920.0 * scaling;
-	params_.baseline = 237.0; // * scaling;
-	params_.fy = params_.fx;
-	params_.cx = -1146.717 * scaling;
-	params_.cy = -975.476 * scaling;*/
-
 	if (!loadMiddleburyCalib(dir+"/calib.txt", params_, scaling)) {
 		LOG(ERROR) << "Could not load middlebury calibration";
 		return;
 	}
 
+    auto data = load_input(dir);
 
 	// Add calibration to config object
-	host_->getConfig()["focal"] = params_.fx;
+	/*host_->getConfig()["focal"] = params_.fx;
 	host_->getConfig()["centre_x"] = params_.cx;
 	host_->getConfig()["centre_y"] = params_.cy;
 	host_->getConfig()["baseline"] = params_.baseline;
@@ -106,57 +104,62 @@ MiddleburySource::MiddleburySource(ftl::rgbd::Source *host, const string &dir)
 
 	host_->on("centre_x", [this]() {
 		params_.cx = host_->value("centre_x", params_.cx);
-	});
+	});*/
 
 	// left and right masks (areas outside rectified images)
 	// only left mask used
-	cv::cuda::GpuMat mask_r_gpu(params_.height, params_.width, CV_8U, 255);
-	cv::cuda::GpuMat mask_l_gpu(params_.height, params_.width, CV_8U, 255);
+	//cv::cuda::GpuMat mask_r_gpu(params_.height, params_.width, CV_8U, 255);
+	//cv::cuda::GpuMat mask_l_gpu(params_.height, params_.width, CV_8U, 255);
 	
 	//calib_->rectifyStereo(mask_l_gpu, mask_r_gpu, stream_);
 	//stream_.waitForCompletion();
 
-	cv::Mat mask_l;
-	mask_l_gpu.download(mask_l);
-	mask_l_ = (mask_l == 0);
-
-	if (!host_->getConfig()["disparity"].is_object()) {
-		host_->getConfig()["disparity"] = ftl::config::json_t{{"algorithm","libsgm"}};
-	}
-	
-	disp_ = Disparity::create(host_, "disparity");
-    if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << *host_->get<ftl::config::json_t>("disparity");
-	disp_->setMask(mask_l_);
+	//cv::Mat mask_l;
+	//mask_l_gpu.download(mask_l);
+	//mask_l_ = (mask_l == 0);
 
 	// Load image files...
-	cv::Mat left_tmp, right_tmp;
-	left_tmp = cv::imread(dir+"/im0.png", cv::IMREAD_COLOR);
-	right_tmp = cv::imread(dir+"/im1.png", cv::IMREAD_COLOR);
+	cv::Mat left_tmp, right_tmp, d_temp;
+	// left_tmp = cv::imread(dir+"/im0.png", cv::IMREAD_COLOR);
+	// right_tmp = cv::imread(dir+"/im1.png", cv::IMREAD_COLOR);
+    cv::cvtColor(data.imL, left_tmp, cv::COLOR_BGR2BGRA);
+    cv::cvtColor(data.imR, right_tmp, cv::COLOR_BGR2BGRA);
 
 	cv::resize(left_tmp, left_tmp, cv::Size(params_.width, params_.height));
 	cv::resize(right_tmp, right_tmp, cv::Size(params_.width, params_.height));
+    cv::resize(data.gtL, d_temp, cv::Size(params_.width, params_.height));
 
-	rgb_.upload(left_tmp, stream_);
+	left_.upload(left_tmp, stream_);
 	right_.upload(right_tmp, stream_);
+    gt_.upload(d_temp, stream_);
+    stream_.waitForCompletion();
 
-	_performDisparity();
-	ready_ = true;
+    do_update_params_ = true;
 }
 
-void MiddleburySource::_performDisparity() {
-	depth_.create(left_.size(), CV_32FC1);
-	disp_tmp_.create(left_.size(), CV_32FC1);
-	//calib_->rectifyStereo(left_, right_, stream_);
-	disp_->compute(rgb_, right_, disp_tmp_, stream_);
-	//disparityToDepth(disp_tmp_, depth_tmp_, params_, stream_);
-	//ftl::cuda::disparity_to_depth(disp_tmp_, depth_, params_, stream_);
-	//left_.download(rgb_, stream_);
-	//rgb_ = lsrc_->cachedLeft();
-	//depth_tmp_.download(depth_, stream_);
-
-	stream_.waitForCompletion();
-
-	//disparityToDepthTRUE(depth_, depth_, params_);
+bool MiddleburySource::capture(int64_t ts) {
+	return true;
 }
 
+bool MiddleburySource::retrieve(ftl::rgbd::Frame &frame) {
+    if (do_update_params_) {
+		do_update_params_ = false;
+		frame.setLeft() = params_;
+		frame.setPose() = Eigen::Matrix4d::Identity();
+
+		auto &meta = frame.create<std::map<std::string,std::string>>(Channel::MetaData);
+		meta["name"] = host_->value("name", host_->getID());
+		meta["id"] = host_->getID();
+		meta["uri"] = host_->value("uri", std::string(""));
+		meta["device"] = "Middlebury";
+
+		if (!frame.has(Channel::Capabilities)) {
+			auto &cap = frame.create<std::unordered_set<Capability>>(Channel::Capabilities);
+		}
+	}
 
+    left_.copyTo(frame.create<GpuMat>(Channel::Colour));
+    right_.copyTo(frame.create<GpuMat>(Channel::Right));
+    gt_.copyTo(frame.create<GpuMat>(Channel::GroundTruth));
+    return true;
+}

components/rgbd-sources/src/sources/middlebury/middlebury_source.hpp

@@ -6,36 +6,35 @@
 
 #include "../../basesource.hpp"
 #include <ftl/cuda_common.hpp>
+//#include <ftl/rgbd/camera.hpp>
 
 namespace ftl {
 namespace rgbd {
 namespace detail {
 
-class Disparity;
-
 class MiddleburySource : public BaseSourceImpl {
 	public:
 	explicit MiddleburySource(ftl::rgbd::Source *);
 	MiddleburySource(ftl::rgbd::Source *, const std::string &dir);
 	~MiddleburySource() {};
 
-	bool capture(int64_t ts) { return true; }
-	bool retrieve(ftl::rgbd::Frame &) { return true; }
+	bool capture(int64_t ts);
+	bool retrieve(ftl::rgbd::Frame &);
 	bool isReady() { return ready_; }
 
+    static bool supported();
+
 	private:
 	bool ready_;
-	Disparity *disp_;
 
 	cv::cuda::Stream stream_;
 
 	cv::cuda::GpuMat left_;
 	cv::cuda::GpuMat right_;
-	cv::cuda::GpuMat disp_tmp_;
-	cv::cuda::GpuMat depth_tmp_;
+    cv::cuda::GpuMat gt_;
 	cv::Mat mask_l_;
-
-	void _performDisparity();
+    ftl::rgbd::Camera params_;
+    bool do_update_params_ = false;
 };
 
 }