diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 726e9960a5d5080a77b68cc5343f33530026ca7b..ad4a65457df9f35525d244dbc10b760ea1aaeeeb 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/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 diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp index 83792c34cc16f157691d86cf72dfe64aa576e741..1a8fdb6ffe797a8441e3ffd224f91fc3a5c9ea84 100644 --- a/components/rgbd-sources/src/source.cpp +++ b/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; } diff --git a/components/rgbd-sources/src/sources/middlebury/loader.cpp b/components/rgbd-sources/src/sources/middlebury/loader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..28cb107da3ddeffe903490a7e358722ee2d12721 --- /dev/null +++ b/components/rgbd-sources/src/sources/middlebury/loader.cpp @@ -0,0 +1,165 @@ +#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)); +} diff --git a/components/rgbd-sources/src/sources/middlebury/loader.hpp b/components/rgbd-sources/src/sources/middlebury/loader.hpp new file mode 100644 index 0000000000000000000000000000000000000000..3bfdfd886546932280a75adbc682c16e3fef96a5 --- /dev/null +++ b/components/rgbd-sources/src/sources/middlebury/loader.hpp @@ -0,0 +1,34 @@ +#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); diff --git a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp index eee884d2f9e62c6c8a42b98d681b19b2463ac14b..f1a6cd7ccd935cd001deca0a5414b0b2e6469b35 100644 --- a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp +++ b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp @@ -1,13 +1,17 @@ +#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; +} diff --git a/components/rgbd-sources/src/sources/middlebury/middlebury_source.hpp b/components/rgbd-sources/src/sources/middlebury/middlebury_source.hpp index 051df8d2c686ba768776a8b40d4aecc9248f930d..ba4ec112964ca1e22fb4e4fb4532a218e72fe798 100644 --- a/components/rgbd-sources/src/sources/middlebury/middlebury_source.hpp +++ b/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; }; }