Feature/of smoothing

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)

components/rgbd-sources/include/ftl/offilter.hpp

+#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

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) {

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_;
 		};
 	};
 };

components/rgbd-sources/src/offilter.cpp

+#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;
+				}
+			}
+		}
+	}
+}