diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp index b2c2f110c72945f38e4ae005eed5b52f4a41bf46..dd591f392d40183c939cac90d7d16d6fdc14b13a 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp @@ -17,7 +17,7 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) { filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, config.value("filter_radius", 25), config.value("filter_iter", 1)); } -void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, const cv::Mat &mask_l) { +void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) { Mat left_disp; Mat right_disp; @@ -25,7 +25,7 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, con cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY); cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY); - if (!ssgm_) { + if (!ssgm_) { // todo: move to constructor ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 16, sgm::EXECUTE_INOUT_HOST2HOST, sgm::StereoSGM::Parameters(10,120,0.95f,true)); @@ -43,7 +43,6 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, con // disparity values set to (256 << 5) in libSGM consistency check Mat bad_pixels = (disp == (256 << 5)); disp.setTo(0, bad_pixels); - disp.setTo(0, mask_l); if (use_filter_) { cv::cuda::GpuMat l_gpu, disp_gpu, disp_gpu_out; @@ -59,7 +58,15 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, con disp.convertTo(disp, CV_32F, 1.0f/16.0f); } -void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) { - // todo: allow using without mask - LOG(FATAL) << "libSGM: not using mask (required)!"; -} +void FixstarsSGM::setMask(Mat &mask) { + LOG_ASSERT(mask.type() == CV_8UC1) << "mask type must be CV_8U"; + + if (!ssgm_) { // todo: move to constructor + ssgm_ = new sgm::StereoSGM(mask.cols, mask.rows, max_disp_, 8, 16, + sgm::EXECUTE_INOUT_HOST2HOST, + sgm::StereoSGM::Parameters(10,120,0.95f,true)); + } + + mask_l_ = mask; + 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 a17ca1bbfdfddb82981ede823c6e878bc1d9c723..dd92efdb206aa97897cc561f45cb57f546fb76e9 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp @@ -27,7 +27,7 @@ class FixstarsSGM : public ftl::Disparity { explicit FixstarsSGM(nlohmann::json &config); void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) override; - void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, const cv::Mat &mask_l) override; + void setMask(cv::Mat &mask) override; /* Factory creator */ static inline Disparity *create(ftl::Configurable *p, const std::string &name) { diff --git a/components/rgbd-sources/src/disparity.hpp b/components/rgbd-sources/src/disparity.hpp index ebe5b9b0849df4a0831679f86600d72a7b348c0b..f5c98f399811bd50e1776f8b0f86fc851e41f9d4 100644 --- a/components/rgbd-sources/src/disparity.hpp +++ b/components/rgbd-sources/src/disparity.hpp @@ -24,14 +24,13 @@ class Disparity : public ftl::Configurable { virtual void setMinDisparity(size_t min) { min_disp_ = min; } virtual void setMaxDisparity(size_t max) { max_disp_ = max; } + virtual void setMask(cv::Mat &mask) { mask_l_ = mask; } + /** * Pure virtual function representing the actual computation of * disparity from left and right images to be implemented. */ virtual void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp)=0; - virtual void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, const cv::Mat &mask_l) { - compute(l, r, disp); - } /** * Factory registration class. @@ -56,6 +55,7 @@ class Disparity : public ftl::Configurable { //nlohmann::json &config_; size_t min_disp_; size_t max_disp_; + cv::Mat mask_l_; private: static std::map<std::string,std::function<Disparity*(ftl::Configurable *, const std::string &)>> *algorithms__; diff --git a/components/rgbd-sources/src/stereovideo_source.cpp b/components/rgbd-sources/src/stereovideo_source.cpp index d8fd08512b462bef1c65e4bb8c60931b7b483e06..b6b7a8c6afb0fc539aec115669c3698bf74f3ec4 100644 --- a/components/rgbd-sources/src/stereovideo_source.cpp +++ b/components/rgbd-sources/src/stereovideo_source.cpp @@ -76,6 +76,7 @@ StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file) disp_ = Disparity::create(this, "disparity"); if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << *get<ftl::config::json_t>("disparity"); + disp_->setMask(mask_l_); LOG(INFO) << "StereoVideo source ready..."; ready_ = true; @@ -114,7 +115,7 @@ void StereoVideoSource::grab() { calib_->rectified(left_, right_); cv::Mat disp; - disp_->compute(left_, right_, disp, mask_l_); + disp_->compute(left_, right_, disp); unique_lock<mutex> lk(mutex_); left_.copyTo(rgb_);