From 96e7502d060ae13cf0f1ed6d6db526aa07c1dfd3 Mon Sep 17 00:00:00 2001 From: Sebastian Hahta <joseha@utu.fi> Date: Tue, 6 Aug 2019 18:34:18 +0300 Subject: [PATCH] sgm: support for different resolution disparity --- .../src/algorithms/fixstars_sgm.cpp | 71 +++++++++++++------ .../src/algorithms/fixstars_sgm.hpp | 1 + 2 files changed, 51 insertions(+), 21 deletions(-) diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp index 8be3530fa..6392add64 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp @@ -12,10 +12,17 @@ using cv::cuda::GpuMat; FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) { ssgm_ = nullptr; + + int width = value("width", 1280); + int height = value("height", 720); + + size_ = cv::Size(width, height); + CHECK((width >= 480) && (height >= 360)); + uniqueness_ = value("uniqueness", 0.95f); P1_ = value("P1", 10); P2_ = value("P2", 120); - + CHECK((uniqueness_ >= 0.0) && (uniqueness_ <= 1.0)); CHECK(P1_ >= 0); CHECK(P2_ > P1_); @@ -26,7 +33,7 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) { int iter = value("filter_iter", 1); CHECK(radius > 0) << "filter_radius must be greater than 0"; CHECK(iter > 0) << "filter_iter must be greater than 0"; - + filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter); } } @@ -34,42 +41,64 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) { void FixstarsSGM::init(const cv::Size size) { if (ssgm_) { delete ssgm_; } dispt_ = GpuMat(size, CV_16SC1); - ssgm_ = new sgm::StereoSGM( size.width, size.height, max_disp_, 8, 16, - lbw_.step, dispt_.step / sizeof(short), - sgm::EXECUTE_INOUT_CUDA2CUDA, - sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true) + ssgm_ = new sgm::StereoSGM(size.width, size.height, max_disp_, 8, 16, + lbw_.step, dispt_.step / sizeof(short), + sgm::EXECUTE_INOUT_CUDA2CUDA, + sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true) ); } -void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) { - cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream); - cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream); +void FixstarsSGM::compute( const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, + cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) +{ + if (l.size() != size_) { + GpuMat tmp; + cv::cuda::resize(l, tmp, size_, 0.0, 0.0, cv::INTER_CUBIC, stream); + cv::cuda::cvtColor(tmp, lbw_, cv::COLOR_BGR2GRAY, 0, stream); + + cv::cuda::resize(r, tmp, size_, 0.0, 0.0, cv::INTER_CUBIC, stream); + cv::cuda::cvtColor(tmp, rbw_, cv::COLOR_BGR2GRAY, 0, stream); + } + else { + cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream); + cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream); + } + + if (!ssgm_) { + // issue #145 has to be fixed before can be moved into constructor + init(size_); + } stream.waitForCompletion(); - if (!ssgm_) { init(l.size()); } - //auto start = std::chrono::high_resolution_clock::now(); ssgm_->execute(lbw_.data, rbw_.data, dispt_.data); - //std::chrono::duration<double> elapsed = - // std::chrono::high_resolution_clock::now() - start; - //LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s"; - + GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows)); left_pixels.setTo(0); cv::cuda::threshold(dispt_, dispt_, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, stream); - if (use_filter_) { filter_->apply(dispt_, l, dispt_, stream); } + GpuMat dispt; + if (l.size() != size_) + { + cv::cuda::multiply(dispt_, (double) l.cols / (double)size_.width, dispt_); + + // invalid areas (bad values) have to be taken into account in interpolation + cv::cuda::resize(dispt_, dispt, l.size(), 0.0, 0.0, cv::INTER_NEAREST, stream); + } + else { dispt = dispt_; } + + if (use_filter_) { filter_->apply(dispt, l, dispt, stream); } - dispt_.convertTo(disp, CV_32F, 1.0f/16.0f, stream); + dispt.convertTo(disp, CV_32F, 1.0f / 16.0f, stream); } void FixstarsSGM::setMask(Mat &mask) { return; // TODO(Nick) Not needed, but also code below does not work with new GPU pipeline CHECK(mask.type() == CV_8UC1) << "mask type must be CV_8U"; - - if (!ssgm_) { init(mask.size()); } - + + if (!ssgm_) { init(size_); } + mask_l_ = mask; - ssgm_->setMask((uint8_t*) mask.data, mask.cols); + ssgm_->setMask((uint8_t*)mask.data, mask.cols); } \ No newline at end of file diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp index 610fa91d9..67221fe8a 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp @@ -42,6 +42,7 @@ class FixstarsSGM : public ftl::rgbd::detail::Disparity { float uniqueness_; int P1_; int P2_; + cv::Size size_; bool use_filter_; cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_; sgm::StereoSGM *ssgm_; -- GitLab