diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 5983d1d940b8b420aa5fb8b6653da65f7f904b0a..e0a925616c522522342d0032b5cae7bc684698b9 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -14,6 +14,7 @@ set(RGBDSRC
# src/algorithms/opencv_sgbm.cpp
# src/algorithms/opencv_bm.cpp
src/cb_segmentation.cpp
+ src/offilter.cpp
)
if (HAVE_REALSENSE)
diff --git a/components/rgbd-sources/include/ftl/offilter.hpp b/components/rgbd-sources/include/ftl/offilter.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7775de411b952e83d431843c829d56cf8c868fce
--- /dev/null
+++ b/components/rgbd-sources/include/ftl/offilter.hpp
@@ -0,0 +1,31 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <opencv2/core/cuda.hpp>
+#include <opencv2/cudaoptflow.hpp>
+
+namespace ftl {
+namespace rgbd {
+
+class OFDisparityFilter {
+public:
+ OFDisparityFilter() : n_max_(0), threshold_(0.0), size_(0, 0) {} // TODO: invalid state
+ OFDisparityFilter(cv::Size size, int n_frames, float threshold);
+ void filter(cv::Mat &disp, const cv::Mat &rgb);
+
+private:
+ int n_;
+ int n_max_;
+ float threshold_;
+ cv::Size size_;
+
+ cv::Mat disp_;
+ cv::Mat gray_;
+
+ cv::Mat flowxy_;
+ cv::Mat flowxy_up_;
+
+ cv::Ptr<cv::cuda::NvidiaOpticalFlow_1_0> nvof_;
+};
+
+}
+}
\ No newline at end of file
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index b6a860db28fa4f9c11d4ec89eb55e0d8f87b4c2c..33bc92a177eee1370b6fd228a7f509ce38ca3b51 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -36,6 +36,15 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius, iter);
}
+
+ bool use_off_ = value("use_off", false);
+
+ 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);
+ }
init(size_);
}
@@ -95,6 +104,12 @@ void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
}
dispt_full_res_.convertTo(disp, CV_32F, 1.0f / 16.0f, stream);
+
+ if (use_off_) {
+ Mat disp_host(disp);
+ off_.filter(disp_host, Mat(lbw_));
+ disp.upload(disp_host);
+ }
}
void FixstarsSGM::setMask(Mat &mask) {
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
index 08d01a535494d10469c99e8f627c8c92571753dd..521241a70843a8e5668e223f5be7b0e7050e4311 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -12,7 +12,7 @@
#include <opencv2/cudastereo.hpp>
#include <ftl/configuration.hpp>
-#include "ftl/cb_segmentation.hpp"
+#include "ftl/offilter.hpp"
namespace ftl {
namespace algorithms {
@@ -44,6 +44,7 @@ namespace ftl {
int P2_;
cv::Size size_;
bool use_filter_;
+ bool use_off_;
cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
sgm::StereoSGM *ssgm_;
cv::cuda::GpuMat lbw_;
@@ -52,6 +53,7 @@ namespace ftl {
cv::cuda::GpuMat l_downscaled_;
cv::cuda::GpuMat dispt_full_res_;
+ ftl::rgbd::OFDisparityFilter off_;
};
};
};
diff --git a/components/rgbd-sources/src/offilter.cpp b/components/rgbd-sources/src/offilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b74ff3ce32d8e5400692f73c09b906fb048d4920
--- /dev/null
+++ b/components/rgbd-sources/src/offilter.cpp
@@ -0,0 +1,109 @@
+#include <loguru.hpp>
+
+#include "ftl/offilter.hpp"
+
+using namespace ftl::rgbd;
+
+using cv::Mat;
+using cv::Size;
+
+using std::vector;
+
+template<typename T> static bool inline isValidDisparity(T d) { return (0.0 < d) && (d < 256.0); } // TODO
+
+OFDisparityFilter::OFDisparityFilter(Size size, int n_frames, float threshold) :
+ n_(0), n_max_(n_frames), threshold_(threshold), size_(size)
+{
+ disp_ = Mat::zeros(size, CV_64FC(n_frames));
+ gray_ = Mat::zeros(size, CV_8UC1);
+
+ nvof_ = cv::cuda::NvidiaOpticalFlow_1_0::create(size.width, size.height,
+ cv::cuda::NvidiaOpticalFlow_1_0::NV_OF_PERF_LEVEL_SLOW,
+ true, false, false, 0);
+
+}
+
+void OFDisparityFilter::filter(Mat &disp, const Mat &gray)
+{
+
+ const int n = n_;
+ n_ = (n_ + 1) % n_max_;
+
+ nvof_->calc(gray, gray_, flowxy_);
+ nvof_->upSampler( flowxy_, size_.width, size_.height,
+ nvof_->getGridSize(), flowxy_up_);
+
+ CHECK(disp.type() == CV_32FC1);
+ CHECK(gray.type() == CV_8UC1);
+ CHECK(flowxy_up_.type() == CV_32FC2);
+
+ gray.copyTo(gray_);
+
+ vector<float> values(n_max_);
+
+ for (int y = 0; y < size_.height; y++)
+ {
+ float *D = disp_.ptr<float>(y);
+ float *d = disp.ptr<float>(y);
+ float *flow = flowxy_up_.ptr<float>(y);
+
+ for (int x = 0; x < size_.width; x++)
+ {
+ const float flow_l1 = abs(flow[2*x]) + abs(flow[2*x + 1]);
+
+ if (flow_l1 < threshold_)
+ {
+ values.clear();
+
+ if (isValidDisparity(d[x]))
+ {
+ bool updated = false;
+ for (int i = 0; i < n_max_; i++)
+ {
+ float &val = D[n_max_ * x + (n_max_ - i + n) % n_max_];
+ if (!isValidDisparity(val))
+ {
+ val = d[x];
+ updated = true;
+ }
+ }
+ if (!updated) { D[n_max_ * x + n] = d[x]; }
+ }
+
+ for (int i = 0; i < n_max_; i++)
+ {
+ float &val = D[n_max_ * x + i];
+ if (isValidDisparity(val)) { values.push_back(val); }
+ }
+
+ if (values.size() > 0) {
+ const auto median_it = values.begin() + values.size() / 2;
+ std::nth_element(values.begin(), median_it , values.end());
+ d[x] = *median_it;
+ }
+
+ /*
+ if (isValidDepth(d[x]) && isValidDepth(D[x]))
+ {
+ D[x] = D[x] * 0.66 + d[x] * (1.0 - 0.66);
+ }
+ if (isValidDepth(D[x]))
+ {
+ d[x] = D[x];
+ }
+ else
+ {
+ D[x] = d[x];
+ }
+ */
+ }
+ else
+ {
+ for (int i = 0; i < n_max_; i++)
+ {
+ D[n_max_ * x + i] = 0.0;
+ }
+ }
+ }
+ }
+}