From d6da9224dc10e5d0ce3304f1d7817512a6f88e34 Mon Sep 17 00:00:00 2001 From: Sebastian Hahta <joseha@utu.fi> Date: Fri, 8 Nov 2019 08:11:59 +0200 Subject: [PATCH] operator: disparity to depth --- components/operators/CMakeLists.txt | 4 +- .../include/ftl/operators/disparity.hpp | 23 +++++- .../{optflow_smoothing.hpp => cuda.hpp} | 5 +- .../src/disparity}/disp2depth.cu | 8 +- .../src/disparity/disparity_to_depth.cpp | 18 ++++ .../src/disparity/optflow_smoothing.cpp | 82 ++++++++++++++----- .../src/disparity/optflow_smoothing.cu | 2 +- components/rgbd-sources/CMakeLists.txt | 1 - .../rgbd-sources/src/cuda_algorithms.hpp | 3 - .../sources/middlebury/middlebury_source.cpp | 2 +- .../src/sources/stereovideo/stereovideo.cpp | 31 +++---- 11 files changed, 125 insertions(+), 54 deletions(-) rename components/operators/src/disparity/{optflow_smoothing.hpp => cuda.hpp} (62%) rename components/{rgbd-sources/src/algorithms => operators/src/disparity}/disp2depth.cu (74%) create mode 100644 components/operators/src/disparity/disparity_to_depth.cpp diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt index e4f7fa533..da52eac6a 100644 --- a/components/operators/CMakeLists.txt +++ b/components/operators/CMakeLists.txt @@ -9,8 +9,10 @@ add_library(ftloperators src/filling.cpp src/filling.cu src/nvopticalflow.cpp - src/disparity/optflow_smoothing.cpp src/disparity/optflow_smoothing.cu + src/disparity/optflow_smoothing.cpp + src/disparity/disp2depth.cu + src/disparity/disparity_to_depth.cpp ) # These cause errors in CI build and are being removed from PCL in newer versions diff --git a/components/operators/include/ftl/operators/disparity.hpp b/components/operators/include/ftl/operators/disparity.hpp index 8e7758107..7588a883f 100644 --- a/components/operators/include/ftl/operators/disparity.hpp +++ b/components/operators/include/ftl/operators/disparity.hpp @@ -1,6 +1,5 @@ #pragma once - #include <ftl/operators/operator.hpp> #include <opencv2/cudaoptflow.hpp> @@ -8,7 +7,20 @@ namespace ftl { namespace operators { /* - * Optical flow smoothing for disparity (or depth) + * Calculate depth from disparity + */ +class DisparityToDepth : public ftl::operators::Operator { + public: + explicit DisparityToDepth(ftl::Configurable* cfg) : + ftl::operators::Operator(cfg) {} + + ~DisparityToDepth() {}; + inline Operator::Type type() const override { return Operator::Type::OneToOne; } + bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override; +}; + +/* + * Optical flow smoothing for depth */ class DisparitySmoothingOF : public ftl::operators::Operator { public: @@ -20,9 +32,14 @@ class DisparitySmoothingOF : public ftl::operators::Operator { bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override; private: + void init(); + + const ftl::codecs::Channel channel_ = ftl::codecs::Channel::Depth; cv::cuda::GpuMat history_; + cv::Size size_; + int n_max_; + float threshold_; }; - } } diff --git a/components/operators/src/disparity/optflow_smoothing.hpp b/components/operators/src/disparity/cuda.hpp similarity index 62% rename from components/operators/src/disparity/optflow_smoothing.hpp rename to components/operators/src/disparity/cuda.hpp index 8deb30f4c..d4cf06aae 100644 --- a/components/operators/src/disparity/optflow_smoothing.hpp +++ b/components/operators/src/disparity/cuda.hpp @@ -3,10 +3,13 @@ namespace ftl { namespace cuda { +void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth, + const ftl::rgbd::Camera &c, cudaStream_t &stream); + + void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow, cv::cuda::GpuMat &history, int n_max, float threshold, cv::cuda::Stream &stream); - } } diff --git a/components/rgbd-sources/src/algorithms/disp2depth.cu b/components/operators/src/disparity/disp2depth.cu similarity index 74% rename from components/rgbd-sources/src/algorithms/disp2depth.cu rename to components/operators/src/disparity/disp2depth.cu index 4b707c479..86a3fc290 100644 --- a/components/rgbd-sources/src/algorithms/disp2depth.cu +++ b/components/operators/src/disparity/disp2depth.cu @@ -16,12 +16,12 @@ __global__ void d2d_kernel(cv::cuda::PtrStepSz<float> disp, cv::cuda::PtrStepSz< namespace ftl { namespace cuda { void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth, - const ftl::rgbd::Camera &c, cv::cuda::Stream &stream) { + const ftl::rgbd::Camera &c, cudaStream_t &stream) { dim3 grid(1,1,1); - dim3 threads(128, 1, 1); - grid.x = cv::cuda::device::divUp(disparity.cols, 128); + dim3 threads(128, 1, 1); + grid.x = cv::cuda::device::divUp(disparity.cols, 128); grid.y = cv::cuda::device::divUp(disparity.rows, 1); - d2d_kernel<<<grid, threads, 0, cv::cuda::StreamAccessor::getStream(stream)>>>( + d2d_kernel<<<grid, threads, 0, stream>>>( disparity, depth, c); cudaSafeCall( cudaGetLastError() ); } diff --git a/components/operators/src/disparity/disparity_to_depth.cpp b/components/operators/src/disparity/disparity_to_depth.cpp new file mode 100644 index 000000000..baed8acfd --- /dev/null +++ b/components/operators/src/disparity/disparity_to_depth.cpp @@ -0,0 +1,18 @@ +#include "ftl/operators/disparity.hpp" +#include "disparity/cuda.hpp" + +using ftl::operators::DisparityToDepth; +using ftl::codecs::Channel; + +using cv::cuda::GpuMat; + +bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, + ftl::rgbd::Source *src, cudaStream_t stream) { + + const auto params = src->parameters(); + const GpuMat &disp = in.get<GpuMat>(Channel::Disparity); + GpuMat depth = out.get<GpuMat>(Channel::Depth); + + ftl::cuda::disparity_to_depth(disp, depth, params, stream); + return true; +} \ No newline at end of file diff --git a/components/operators/src/disparity/optflow_smoothing.cpp b/components/operators/src/disparity/optflow_smoothing.cpp index fcfa1aa50..8dffdaa4c 100644 --- a/components/operators/src/disparity/optflow_smoothing.cpp +++ b/components/operators/src/disparity/optflow_smoothing.cpp @@ -2,7 +2,7 @@ #include "ftl/operators/disparity.hpp" #include "ftl/offilter.hpp" -#include "disparity/optflow_smoothing.hpp" +#include "disparity/cuda.hpp" #ifdef HAVE_OPTFLOW @@ -21,29 +21,73 @@ template<typename T> static bool inline isValidDisparity(T d) { return (0.0 < d) DisparitySmoothingOF::DisparitySmoothingOF(ftl::Configurable* cfg) : ftl::operators::Operator(cfg) { - // TODO read params - /* - CHECK((n_max_ > 1) && (n_max_ <= 32)) << "History length must be between 0 and 31!"; - disp_old_ = cv::cuda::GpuMat(cv::Size(size.width * n_max_, size.height), CV_32FC1); - */ + + size_ = Size(0, 0); + + n_max_ = cfg->value("history_size", 7); + if (n_max_ < 1) { + LOG(WARNING) << "History length must be larger than 0"; + n_max_ = 7; + } + + if (n_max_ > 32) { + // TODO: cuda kernel uses fixed size buffer + LOG(WARNING) << "History length can't be larger than 32 (TODO)"; + n_max_ = 32; + } + + threshold_ = cfg->value("threshold", 1.0); + + cfg->on("threshold", [this, &cfg](const ftl::config::Event&) { + float threshold = cfg->value("threshold", 1.0); + if (threshold < 0.0) { + LOG(WARNING) << "invalid threshold " << threshold << ", value must be positive"; + } + else { + threshold_ = threshold; + init(); + } + }); + + cfg->on("history_size", [this, &cfg](const ftl::config::Event&) { + int n_max = cfg->value("history_size", 1.0); + + if (n_max < 1) { + LOG(WARNING) << "History length must be larger than 0"; + } + else if (n_max_ > 32) { + // TODO: cuda kernel uses fixed size buffer + LOG(WARNING) << "History length can't be larger than 32 (TODO)"; + } + else { + n_max_ = n_max; + init(); + } + }); } DisparitySmoothingOF::~DisparitySmoothingOF() {} -bool DisparitySmoothingOF::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) { -/* -void OFDisparityFilter::filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream) -{ - frame.upload(Channel::Flow, stream); - const cv::cuda::GpuMat &optflow = frame.get<cv::cuda::GpuMat>(Channel::Flow); - //frame.get<cv::cuda::GpuMat>(Channel::Disparity); - stream.waitForCompletion(); - if (optflow.empty()) { return; } - - cv::cuda::GpuMat &disp = frame.create<cv::cuda::GpuMat>(Channel::Disparity); - ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream); +void DisparitySmoothingOF::init() { + if (size_ == Size(0, 0)) { return; } + history_.create(cv::Size(size_.width * n_max_, size_.height), CV_32FC1); + history_.setTo(0.0); } -*/ + +bool DisparitySmoothingOF::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) { + if (!out.hasChannel(channel_) || !in.hasChannel(Channel::Flow)) { return true; } + + auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream); + const cv::cuda::GpuMat &optflow = in.get<cv::cuda::GpuMat>(Channel::Flow); + cv::cuda::GpuMat &data = out.get<cv::cuda::GpuMat>(channel_); + + if (data.size() != size_) { + size_ = data.size(); + init(); + } + + ftl::cuda::optflow_filter(data, optflow, history_, n_max_, threshold_, cvstream); + return true; } diff --git a/components/operators/src/disparity/optflow_smoothing.cu b/components/operators/src/disparity/optflow_smoothing.cu index b703a62aa..a7633036f 100644 --- a/components/operators/src/disparity/optflow_smoothing.cu +++ b/components/operators/src/disparity/optflow_smoothing.cu @@ -3,7 +3,7 @@ #include <opencv2/core/cuda_stream_accessor.hpp> #include "disparity/qsort.h" -#include "optflow_smoothing.hpp" +#include "disparity/cuda.hpp" __device__ void quicksort(float A[], size_t n) { diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 70b5f7afb..1e0f0de20 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/components/rgbd-sources/CMakeLists.txt @@ -39,7 +39,6 @@ endif (LIBSGM_FOUND) if (CUDA_FOUND) list(APPEND RGBDSRC - src/algorithms/disp2depth.cu # "src/algorithms/opencv_cuda_bm.cpp" # "src/algorithms/opencv_cuda_bp.cpp" # "src/algorithms/rtcensus.cu" diff --git a/components/rgbd-sources/src/cuda_algorithms.hpp b/components/rgbd-sources/src/cuda_algorithms.hpp index f07c698f1..43fff52d0 100644 --- a/components/rgbd-sources/src/cuda_algorithms.hpp +++ b/components/rgbd-sources/src/cuda_algorithms.hpp @@ -38,9 +38,6 @@ namespace cuda { void texture_map(const TextureObject<uchar4> &t, TextureObject<float> &f); - void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth, - const ftl::rgbd::Camera &c, cv::cuda::Stream &stream); - } } diff --git a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp index d152d25e5..229895049 100644 --- a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp +++ b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp @@ -149,7 +149,7 @@ void MiddleburySource::_performDisparity() { //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_); + //ftl::cuda::disparity_to_depth(disp_tmp_, depth_, params_, stream_); //left_.download(rgb_, stream_); //rgb_ = lsrc_->cachedLeft(); //depth_tmp_.download(depth_, stream_); diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp index a018859c8..473e66a49 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp @@ -1,19 +1,19 @@ #include <loguru.hpp> + #include "stereovideo.hpp" -#include <ftl/configuration.hpp> +#include "ftl/configuration.hpp" #ifdef HAVE_OPTFLOW -#include <ftl/operators/opticalflow.hpp> +#include "ftl/operators/opticalflow.hpp" #endif -#include <ftl/threads.hpp> +#include "ftl/operators/disparity.hpp" + +#include "ftl/threads.hpp" #include "calibrate.hpp" #include "local.hpp" #include "disparity.hpp" -#include "cuda_algorithms.hpp" - -#include "cuda_algorithms.hpp" using ftl::rgbd::detail::Calibrate; using ftl::rgbd::detail::LocalSource; @@ -38,8 +38,7 @@ StereoVideoSource::~StereoVideoSource() { delete lsrc_; } -void StereoVideoSource::init(const string &file) -{ +void StereoVideoSource::init(const string &file) { capabilities_ = kCapVideo | kCapStereo; if (ftl::is_video(file)) { @@ -127,6 +126,7 @@ void StereoVideoSource::init(const string &file) #endif pipeline_depth_ = ftl::config::create<ftl::operators::Graph>(host_, "pipeline_disparity"); + pipeline_depth_->append<ftl::operators::DisparityToDepth>("calculate_depth"); disp_ = Disparity::create(host_, "disparity"); if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << *host_->get<ftl::config::json_t>("disparity"); @@ -158,15 +158,6 @@ ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) { } } -static void disparityToDepth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth, - const cv::Mat &Q, cv::cuda::Stream &stream) { - // Q(3, 2) = -1/Tx - // Q(2, 3) = f - - double val = (1.0f / Q.at<double>(3, 2)) * Q.at<double>(2, 3); - cv::cuda::divide(val, disparity, depth, 1.0f / 1000.0f, -1, stream); -} - bool StereoVideoSource::capture(int64_t ts) { timestamp_ = ts; lsrc_->grab(); @@ -180,7 +171,7 @@ bool StereoVideoSource::retrieve() { auto &right = frame.create<cv::cuda::GpuMat>(Channel::Right); lsrc_->get(left, right, calib_, stream2_); - pipeline_input_->apply(frame, frame, (ftl::rgbd::Source*) lsrc_, cv::cuda::StreamAccessor::getStream(stream2_)); + pipeline_input_->apply(frame, frame, (ftl::rgbd::Source*) this, cv::cuda::StreamAccessor::getStream(stream2_)); stream2_.waitForCompletion(); return true; @@ -209,8 +200,8 @@ bool StereoVideoSource::compute(int n, int b) { auto &disp = frame.get<cv::cuda::GpuMat>(Channel::Disparity); auto &depth = frame.create<cv::cuda::GpuMat>(Channel::Depth); if (depth.empty()) depth = cv::cuda::GpuMat(left.size(), CV_32FC1); - - ftl::cuda::disparity_to_depth(disp, depth, params_, stream_); + pipeline_depth_->apply(frame, frame, (ftl::rgbd::Source*) this, cv::cuda::StreamAccessor::getStream(stream_)); + //ftl::cuda::disparity_to_depth(disp, depth, params_, stream_); //left.download(rgb_, stream_); //depth.download(depth_, stream_); -- GitLab