From 306102c9bf5dcc56ac7fdbf656d98fd7f937807a Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Wed, 26 Jun 2019 18:24:00 +0300
Subject: [PATCH] Middlebury RGBD Source
---
.../reconstruct/include/ftl/ray_cast_sdf.hpp | 8 +
components/rgbd-sources/CMakeLists.txt | 1 +
.../rgbd-sources/src/middlebury_source.cpp | 155 ++++++++++++++++++
.../rgbd-sources/src/middlebury_source.hpp | 44 +++++
components/rgbd-sources/src/source.cpp | 10 +-
components/rgbd-sources/test/source_unit.cpp | 11 ++
6 files changed, 228 insertions(+), 1 deletion(-)
create mode 100644 components/rgbd-sources/src/middlebury_source.cpp
create mode 100644 components/rgbd-sources/src/middlebury_source.hpp
diff --git a/applications/reconstruct/include/ftl/ray_cast_sdf.hpp b/applications/reconstruct/include/ftl/ray_cast_sdf.hpp
index 4f263065f..06e5f2e59 100644
--- a/applications/reconstruct/include/ftl/ray_cast_sdf.hpp
+++ b/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;
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 96c709b6e..33f555f92 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/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
diff --git a/components/rgbd-sources/src/middlebury_source.cpp b/components/rgbd-sources/src/middlebury_source.cpp
new file mode 100644
index 000000000..c539acf0c
--- /dev/null
+++ b/components/rgbd-sources/src/middlebury_source.cpp
@@ -0,0 +1,155 @@
+#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 ¶ms, 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;
+}
+
diff --git a/components/rgbd-sources/src/middlebury_source.hpp b/components/rgbd-sources/src/middlebury_source.hpp
new file mode 100644
index 000000000..10cd47d44
--- /dev/null
+++ b/components/rgbd-sources/src/middlebury_source.hpp
@@ -0,0 +1,44 @@
+#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_
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 7beea7519..a22c3e630 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/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)) {
- return new StereoVideoSource(this, 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;
}
diff --git a/components/rgbd-sources/test/source_unit.cpp b/components/rgbd-sources/test/source_unit.cpp
index baea61824..8454c011b 100644
--- a/components/rgbd-sources/test/source_unit.cpp
+++ b/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"
--
GitLab