diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp index 732c4ffb56a65a2ac8f53841b5dcb45e52ed5280..8a55fbcbbf92840869a799e69df3dc4f8980592c 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp @@ -27,28 +27,66 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) { CHECK(P1_ >= 0); CHECK(P2_ > P1_); - use_filter_ = value("use_filter", false); - if (use_filter_) { - int radius = value("filter_radius", 25); - 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"; + updateBilateralFilter(); - filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter); - } + on("use_filter", [this](const ftl::config::Event&) { + updateBilateralFilter(); + }); + + on("filter_radius", [this](const ftl::config::Event&) { + updateBilateralFilter(); + }); + + on("filter_iter", [this](const ftl::config::Event&) { + updateBilateralFilter(); + }); #ifdef HAVE_OPTFLOW - use_off_ = value("use_off", false); + updateOFDisparityFilter(); - if (use_off_) - { - int off_size = value("off_size", 9); - double off_threshold = value("off_threshold", 0.9); - off_ = ftl::rgbd::OFDisparityFilter(size_, off_size, off_threshold); - } + on("use_off", [this](const ftl::config::Event&) { + updateOFDisparityFilter(); + }); + + on("use_off", [this](const ftl::config::Event&) { + updateOFDisparityFilter(); + }); #endif init(size_); + + on("uniqueness", [this](const ftl::config::Event&) { + float uniqueness = value("uniqueness", uniqueness_); + if ((uniqueness >= 0.0) && (uniqueness <= 1.0)) { + uniqueness_ = uniqueness; + updateParameters(); + } + else { + LOG(WARNING) << "invalid uniquness: " << uniqueness; + } + }); + + on("P1", [this](const ftl::config::Event&) { + int P1 = value("P1", P1_); + if (P1 <= P2_) { + P1_ = P1; + updateParameters(); + } + else { + LOG(WARNING) << "invalid P1: " << P1 << ", (P1 <= P2), P2 is " << P2_; + } + }); + + on("P2", [this](const ftl::config::Event&) { + int P2 = value("P2", P2_); + if (P2 >= P1_) { + P2_ = P2; + updateParameters(); + } + else { + LOG(WARNING) << "invalid P2: " << P2 << ", (P1 <= P2), P1 is " << P1_; + } + }); } void FixstarsSGM::init(const cv::Size size) { @@ -64,6 +102,69 @@ void FixstarsSGM::init(const cv::Size size) { ); } +bool FixstarsSGM::updateParameters() { + if (ssgm_ == nullptr) { return false; } + return this->ssgm_->updateParameters( + sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true)); +} + +bool FixstarsSGM::updateBilateralFilter() { + bool enable = value("use_filter", false); + int radius = value("filter_radius", 25); + int iter = value("filter_iter", 1); + + if (enable) { + if (radius <= 0) { + LOG(WARNING) << "filter_radius must be greater than 0"; + enable = false; + } + if (iter <= 0) { + LOG(WARNING) << "filter_iter must be greater than 0"; + enable = false; + } + } + + if (enable) { + filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter); + use_filter_ = true; + } + else { + use_filter_ = false; + } + + return use_filter_; +} + +#ifdef HAVE_OPTFLOW +bool FixstarsSGM::updateOFDisparityFilter() { + bool enable = value("use_off", false); + int off_size = value("off_size", 9); + double off_threshold = value("off_threshold", 0.9); + + if (enable) { + if (off_size <= 0) { + LOG(WARNING) << "bad off_size: " << off_size; + enable = false; + } + + if (off_threshold <= 0.0) { + LOG(WARNING) << "bad off_threshold: " << off_threshold; + enable = false; + } + } + + if (enable) { + off_ = ftl::rgbd::OFDisparityFilter(size_, off_size, off_threshold); + use_off_ = true; + } + else { + use_off_ = false; + } + + return use_off_; +} +#endif + void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream) { /*if (!frame.hasChannel(ftl::rgbd::kChanLeftGray)) diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp index d2c0c1d2a8fd459695bb02c9f66f17e423dc9fa5..90d330079e177f6adbb41ebbd6c7830bbf87e031 100644 --- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp +++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp @@ -41,6 +41,11 @@ namespace ftl { private: void init(const cv::Size size); + bool updateParameters(); + bool updateBilateralFilter(); + #ifdef HAVE_OPTFLOW + bool updateOFDisparityFilter(); + #endif float uniqueness_; int P1_;