diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 83dd19f84dabd37d345c0ac1294970fa8fd8c4cf..73445da8c9afded93b510954212b9100fb5ee5b4 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -63,15 +63,57 @@ using ftl::registration::loadTransformations; using ftl::registration::saveTransformations; static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) { - Eigen::Affine3d rx = - Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0))); - Eigen::Affine3d ry = - Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0))); - Eigen::Affine3d rz = - Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1))); - return rz * rx * ry; + Eigen::Affine3d rx = + Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0))); + Eigen::Affine3d ry = + Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0))); + Eigen::Affine3d rz = + Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1))); + return rz * rx * ry; } +// TODO: Remove this class (requires more general solution). Also does not +// process disconnections/reconnections/types etc. correctly. +class ConfigProxy { + private: + vector<ftl::UUID> peers_; + vector<std::string> uris_; + ftl::net::Universe *net_; + + public: + ConfigProxy(ftl::net::Universe *net) { + net_ = net; + + auto response = net_->findAll<std::string>("node_details"); + for (auto &r : response) { + auto r_json = json_t::parse(r); + peers_.push_back(ftl::UUID(r_json["id"].get<std::string>())); + uris_.push_back(r_json["title"].get<std::string>()); + } + } + + void add(ftl::Configurable *root, const std::string &uri, const std::string &name) { + auto config = json_t::parse(net_->call<string>(peers_[0], "get_cfg", uris_[0] + "/" + uri)); + auto *proxy = ftl::create<ftl::Configurable>(root, name); + + for (auto &itm : config.get<json::object_t>()) { + auto key = itm.first; + auto value = itm.second; + if (*key.begin() == '$') { continue; } + + proxy->set(key, value); + proxy->on(key, [this, uri, key, value, proxy](const ftl::config::Event&) { + for (size_t i = 0; i < uris_.size(); i++) { + // TODO: check that config exists? + auto peer = peers_[i]; + std::string name = uris_[i] + "/" + uri + "/" + key; + net_->send(peer, "update_cfg", name, proxy->getConfig()[key].dump()); + } + }); + } + } +}; + static void run(ftl::Configurable *root) { Universe *net = ftl::create<Universe>(root, "net"); ftl::ctrl::Slave slave(net, root); @@ -122,6 +164,9 @@ static void run(ftl::Configurable *root) { return; } + auto configproxy = ConfigProxy(net); + configproxy.add(root, "source/disparity", "disparity"); + // Create scene transform, intended for axis aligning the walls and floor Eigen::Matrix4d transform; if (root->getConfig()["transform"].is_object()) { diff --git a/components/rgbd-sources/include/ftl/offilter.hpp b/components/rgbd-sources/include/ftl/offilter.hpp index 6aece39aab241dfc7605dfb208d7d30ba6135509..0e273d80d0382dd1a805bb50f658e8180ded567e 100644 --- a/components/rgbd-sources/include/ftl/offilter.hpp +++ b/components/rgbd-sources/include/ftl/offilter.hpp @@ -17,6 +17,7 @@ public: OFDisparityFilter() : n_max_(0), threshold_(0.0) {} OFDisparityFilter(cv::Size size, int n_frames, float threshold); void filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream); + void filter(cv::cuda::GpuMat &disp, cv::cuda::GpuMat &optflow, cv::cuda::Stream &stream); private: int n_max_; diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp index 732c4ffb56a65a2ac8f53841b5dcb45e52ed5280..f96ebcd81ed7336ab34ba0e24322587a63fd6c3b 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,22 +102,72 @@ void FixstarsSGM::init(const cv::Size size) { ); } -void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream) -{ - /*if (!frame.hasChannel(ftl::rgbd::kChanLeftGray)) - { - auto &rgb = frame.getChannel<GpuMat>(ftl::rgbd::kChanLeft, stream); - auto &gray = frame.setChannel<GpuMat>(ftl::rgbd::kChanLeftGray); - cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream); +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; } - if (!frame.hasChannel(ftl::rgbd::kChanRightGray)) - { - auto &rgb = frame.getChannel<GpuMat>(ftl::rgbd::kChanRight, stream); - auto &gray = frame.setChannel<GpuMat>(ftl::rgbd::kChanRightGray); - cv::cuda::cvtColor(rgb, gray, cv::COLOR_BGR2GRAY, 0, stream); - }*/ + 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) { + LOG(INFO) << "Optical flow filter, size: " << off_size << ", threshold: " << off_threshold; + 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) +{ const auto &l = frame.get<GpuMat>(Channel::Left); const auto &r = frame.get<GpuMat>(Channel::Right); auto &disp = frame.create<GpuMat>(Channel::Disparity, Format<float>(l.size())); @@ -100,21 +188,10 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream) stream.waitForCompletion(); ssgm_->execute(lbw_.data, rbw_.data, dispt_.data); - GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows)); - left_pixels.setTo(0); + // 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); - // TODO: filter could be applied after upscaling (to the upscaled disparity image) - if (use_filter_) - { - filter_->apply( - dispt_, - (l.size() == size_) ? l : l_scaled, - dispt_, - stream - ); - } - GpuMat dispt_scaled; if (l.size() != size_) { @@ -125,10 +202,26 @@ void FixstarsSGM::compute(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream) dispt_scaled = dispt_; } + // TODO: filter could be applied after upscaling (to the upscaled disparity image) + if (use_filter_) + { + filter_->apply( + dispt_, + l, + dispt_, + stream + ); + } + dispt_scaled.convertTo(disp, CV_32F, 1.0f / 16.0f, stream); #ifdef HAVE_OPTFLOW - if (use_off_) { off_.filter(frame, stream); } + // TODO: Optical flow filter expects CV_32F + if (use_off_) { + frame.upload(Channel::Flow, stream); + stream.waitForCompletion(); + off_.filter(disp, frame.get<GpuMat>(Channel::Flow), stream); + } #endif } 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_; diff --git a/components/rgbd-sources/src/offilter.cpp b/components/rgbd-sources/src/offilter.cpp index 34c01c1feb73b8171b0890f8d94405ee22a0433b..8bb3ef601d18156e1b2a1d7600b621ceefa8c2e0 100644 --- a/components/rgbd-sources/src/offilter.cpp +++ b/components/rgbd-sources/src/offilter.cpp @@ -39,4 +39,14 @@ void OFDisparityFilter::filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream); } +void OFDisparityFilter::filter(cv::cuda::GpuMat &disp, cv::cuda::GpuMat &optflow, cv::cuda::Stream &stream) +{ + if (disp.type() != CV_32FC1) { + LOG(ERROR) << "Optical flow filter expects CV_32FC1 (TODO)"; + return; + } + + ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream); +} + #endif // HAVE_OPTFLOW