Merge branch 'feature/middleburysrc' into 'master'

Middlebury RGBD Source

See merge request nicolas.pope/ftl!49

applications/reconstruct/include/ftl/ray_cast_sdf.hpp

@@ -33,6 +33,14 @@ public:
 			m_params.m_width = value("width", 640);
 		});
 
+		on("max_depth", [this](const ftl::config::Event &e) {
+			m_params.m_maxDepth = value("max_depth", 20.0f);
+		});
+
+		on("min_depth", [this](const ftl::config::Event &e) {
+			m_params.m_minDepth = value("min_depth", 20.0f);
+		});
+
 		on("showBlockBorders", [this](const ftl::config::Event &e) {
 			if (value("showBlockBorders", false)) m_params.m_flags |= kShowBlockBorders;
 			else m_params.m_flags &= ~kShowBlockBorders;

components/rgbd-sources/CMakeLists.txt

@@ -4,6 +4,7 @@ set(RGBDSRC
 	src/disparity.cpp
 	src/source.cpp
 	src/stereovideo.cpp
+	src/middlebury_source.cpp
 	src/net.cpp
 	src/streamer.cpp
 	src/colour.cpp

components/rgbd-sources/src/middlebury_source.cpp

+#include "middlebury_source.hpp"
+
+#include "disparity.hpp"
+
+using ftl::rgbd::detail::MiddleburySource;
+using ftl::rgbd::detail::Disparity;
+using std::string;
+
+MiddleburySource::MiddleburySource(ftl::rgbd::Source *host)
+		: ftl::rgbd::detail::Source(host), ready_(false) {
+	// Not VALID
+}
+
+static bool loadMiddleburyCalib(const std::string &filename, ftl::rgbd::Camera &params, double scaling) {
+	FILE* fp = fopen(filename.c_str(), "r");
+	char buff[512];
+	
+	float cam0[3][3];
+	float cam1[3][3];
+	float doffs;
+	float baseline;
+	int width;
+	int height;
+	int ndisp;
+	int isint;
+	int vmin;
+	int vmax;
+	float dyavg;
+	float dymax;
+
+	if (fp != nullptr)
+	{
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "cam0 = [%f %f %f; %f %f %f; %f %f %f]\n", &cam0[0][0], &cam0[0][1], &cam0[0][2], &cam0[1][0], &cam0[1][1], &cam0[1][2], &cam0[2][0], &cam0[2][1], &cam0[2][2]);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "cam1 = [%f %f %f; %f %f %f; %f %f %f]\n", &cam1[0][0], &cam1[0][1], &cam1[0][2], &cam1[1][0], &cam1[1][1], &cam1[1][2], &cam1[2][0], &cam1[2][1], &cam1[2][2]);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "doffs = %f\n", &doffs);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "baseline = %f\n", &baseline);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "width = %d\n", &width);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "height = %d\n", &height);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "ndisp = %d\n", &ndisp);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "isint = %d\n", &isint);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "vmin = %d\n", &vmin);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "vmax = %d\n", &vmax);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "dyavg = %f\n", &dyavg);
+		if (fgets(buff, sizeof(buff), fp) != nullptr) sscanf(buff, "dymax = %f\n", &dymax);
+		fclose(fp);
+
+		params.fx = cam0[0][0] * scaling;
+		params.fy = params.fx;
+		params.cx = -cam0[0][2] * scaling;
+		params.cy = -cam0[1][2] * scaling;
+		params.width = width * scaling;
+		params.height = height * scaling;
+		params.baseline = baseline;
+
+		return true;
+	}
+
+	return false;
+}
+
+MiddleburySource::MiddleburySource(ftl::rgbd::Source *host, const string &dir)
+		: ftl::rgbd::detail::Source(host), ready_(false) {
+
+	double scaling = host->value("scaling", 0.5);
+
+	// 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;
+	}
+
+
+	// Add calibration to config object
+	host_->getConfig()["focal"] = params_.fx;
+	host_->getConfig()["centre_x"] = params_.cx;
+	host_->getConfig()["centre_y"] = params_.cy;
+	host_->getConfig()["baseline"] = params_.baseline;
+
+	// Add event handlers to allow calibration changes...
+	host_->on("baseline", [this](const ftl::config::Event &e) {
+		params_.baseline = host_->value("baseline", params_.baseline);
+	});
+
+	host_->on("focal", [this](const ftl::config::Event &e) {
+		params_.fx = host_->value("focal", params_.fx);
+		params_.fy = params_.fx;
+	});
+
+	// 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);
+	
+	//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"] = {{"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_);
+
+	// Load image files...
+	cv::Mat right_tmp;
+	rgb_ = cv::imread(dir+"/im0.png", cv::IMREAD_COLOR);
+	right_tmp = cv::imread(dir+"/im1.png", cv::IMREAD_COLOR);
+
+	cv::resize(rgb_, rgb_, cv::Size(params_.width, params_.height));
+	cv::resize(right_tmp, right_tmp, cv::Size(params_.width, params_.height));
+
+	left_.upload(rgb_);
+	right_.upload(right_tmp);
+
+	_performDisparity();
+	ready_ = true;
+}
+
+static void disparityToDepth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
+							 const ftl::rgbd::Camera &c, cv::cuda::Stream &stream) {
+	double val = c.baseline * c.fx;
+	cv::cuda::divide(val, disparity, depth, 1.0f / 1000.0f, -1, stream);
+}
+
+void MiddleburySource::_performDisparity() {
+	if (depth_tmp_.empty()) depth_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
+	if (disp_tmp_.empty()) disp_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
+	//calib_->rectifyStereo(left_, right_, stream_);
+	disp_->compute(left_, right_, disp_tmp_, stream_);
+	disparityToDepth(disp_tmp_, depth_tmp_, params_, stream_);
+	//left_.download(rgb_, stream_);
+	//rgb_ = lsrc_->cachedLeft();
+	depth_tmp_.download(depth_, stream_);
+
+	stream_.waitForCompletion();
+}
+
+bool MiddleburySource::grab() {
+	//_performDisparity();
+	return true;
+}
+

components/rgbd-sources/src/middlebury_source.hpp

+#pragma once
+#ifndef _FTL_RGBD_MIDDLEBURY_SOURCE_HPP_
+#define _FTL_RGBD_MIDDLEBURY_SOURCE_HPP_
+
+#include <loguru.hpp>
+
+#include <ftl/rgbd/source.hpp>
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace rgbd {
+namespace detail {
+
+class Disparity;
+
+class MiddleburySource : public detail::Source {
+	public:
+	MiddleburySource(ftl::rgbd::Source *);
+	MiddleburySource(ftl::rgbd::Source *, const std::string &dir);
+	~MiddleburySource() {};
+
+	bool grab();
+	bool isReady() { return ready_; }
+
+	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::Mat mask_l_;
+
+	void _performDisparity();
+};
+
+}
+}
+}
+
+#endif  // _FTL_RGBD_MIDDLEBURY_SOURCE_HPP_

components/rgbd-sources/src/source.cpp

@@ -5,6 +5,7 @@
 #include "net.hpp"
 #include "stereovideo.hpp"
 #include "image.hpp"
+#include "middlebury_source.hpp"
 
 #ifdef HAVE_LIBARCHIVE
 #include <ftl/rgbd/snapshot.hpp>
@@ -25,6 +26,7 @@ using std::shared_lock;
 using ftl::rgbd::detail::StereoVideoSource;
 using ftl::rgbd::detail::NetSource;
 using ftl::rgbd::detail::ImageSource;
+using ftl::rgbd::detail::MiddleburySource;
 using ftl::rgbd::capability_t;
 
 Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(nullptr) {
@@ -97,7 +99,13 @@ ftl::rgbd::detail::Source *Source::_createFileImpl(const ftl::URI &uri) {
 	if (eix == string::npos) {
 		// Might be a directory
 		if (ftl::is_directory(path)) {
+			if (ftl::is_file(path + "/video.mp4")) {
 				return new StereoVideoSource(this, path);
+			} else if (ftl::is_file(path + "/im0.png")) {
+				return new MiddleburySource(this, path);
+			} else {
+				LOG(ERROR) << "Directory is not a valid RGBD source: " << path;
+			}
 		} else {
 			return nullptr;
 		}

components/rgbd-sources/test/source_unit.cpp

@@ -73,6 +73,16 @@ class RealsenseSource : public ftl::rgbd::detail::Source {
 	bool isReady() { return true; };
 };
 
+class MiddleburySource : public ftl::rgbd::detail::Source {
+	public:
+	MiddleburySource(ftl::rgbd::Source *host, const std::string &dir) : ftl::rgbd::detail::Source(host) {
+		last_type = "middlebury";
+	}
+
+	bool grab() { return true; };
+	bool isReady() { return true; };
+};
+
 }	
 }	
 }
@@ -86,6 +96,7 @@ class RealsenseSource : public ftl::rgbd::detail::Source {
 #define _FTL_RGBD_SNAPSHOT_SOURCE_HPP_
 #define _FTL_RGBD_IMAGE_HPP_
 #define _FTL_RGBD_REALSENSE_HPP_
+#define _FTL_RGBD_MIDDLEBURY_SOURCE_HPP_
 
 #include "../src/source.cpp"