Optflow temporal filter CUDA kernel

components/rgbd-sources/CMakeLists.txt

@@ -37,6 +37,7 @@ endif (LIBSGM_FOUND)
 if (CUDA_FOUND)
 	list(APPEND RGBDSRC
 		src/algorithms/disp2depth.cu
+		src/algorithms/offilter.cu
 #		"src/algorithms/opencv_cuda_bm.cpp"
 #		"src/algorithms/opencv_cuda_bp.cpp"
 #		"src/algorithms/rtcensus.cu"

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

@@ -15,6 +15,7 @@ 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);
+	void filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow);
 
 private:
 	int n_;
@@ -23,6 +24,7 @@ private:
 	cv::Size size_;
 
 	cv::Mat disp_;
+	cv::cuda::GpuMat disp_old_;
 	cv::Mat gray_;
 
 	cv::Mat flowxy_;

components/rgbd-sources/src/algorithms/offilter.cu

+#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <opencv2/core/cuda_stream_accessor.hpp>
+
+template<typename T>
+__device__  static bool inline isValidDisparity(T d) { return (0.0 < d) && (d < 256.0); } // TODO
+
+__global__ void temporal_median_filter_kernel(
+	cv::cuda::PtrStepSz<float> disp,
+	cv::cuda::PtrStepSz<int> optflow,
+	cv::cuda::PtrStepSz<float> history,
+	int n_max, float threshold)
+{
+	for (STRIDE_Y(y, disp.rows)) {
+	for (STRIDE_X(x, disp.cols)) {
+
+		int flowx = optflow(y, 2 * x);
+		int flowy = optflow(y, 2 * x + 1);
+
+		if ((abs(flowx) + abs(flowy)) > 32) // TODO float <-> fixed 10.5
+		{
+			history(y, x * n_max) = 0.0;
+			return;
+		}
+
+		int n = history(y, x * n_max);
+
+		if (isValidDisparity(disp(y, x)))
+		{
+			history(y, x * n_max) += 1.0;
+			history(y, x * n_max + n % n_max) = disp(y, x);
+		}
+
+		int n_end = n;
+		if (n_end >= n_max)	{ n_end = n_max - 1; }
+
+		if (n_end != 0)
+		{
+			// calculate median
+		}
+	}}
+}
+
+namespace ftl {
+namespace cuda {
+	
+void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
+					cv::cuda::GpuMat &history, int n, float threshold,
+					cv::cuda::Stream &stream)
+{
+	dim3 grid(1, 1, 1);
+	dim3 threads(128, 1, 1);
+	grid.x = cv::cuda::device::divUp(disp.cols, 128);
+	grid.y = cv::cuda::device::divUp(disp.rows, 1);
+	temporal_median_filter_kernel<<<grid, threads, 0, cv::cuda::StreamAccessor::getStream(stream)>>>
+		(disp, optflow, history, n, threshold);
+
+	cudaSafeCall(cudaGetLastError());
+}
+
+}
+}
\ No newline at end of file

components/rgbd-sources/src/cuda_algorithms.hpp

@@ -41,6 +41,9 @@ namespace cuda {
 	void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
 				const ftl::rgbd::Camera &c, cv::cuda::Stream &stream);
 
+	void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
+						cv::cuda::GpuMat &history, int n_max, float threshold,
+						cv::cuda::Stream &stream);
 
 }
 }

components/rgbd-sources/src/offilter.cpp

 #include "ftl/offilter.hpp"
+#include "cuda_algorithms.hpp"
+
 
 #ifdef HAVE_OPTFLOW
 
@@ -18,20 +20,26 @@ OFDisparityFilter::OFDisparityFilter(Size size, int n_frames, float threshold) :
 {
 	
 	disp_ = Mat::zeros(cv::Size(size.width * n_frames, size.height), CV_64FC1);
+	disp_old_ = cv::cuda::GpuMat(cv::Size(size.width * n_frames, size.height), CV_32FC1);
 	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(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow)
+{
+	ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, cv::cuda::Stream::Null());
+	cv::cuda::Stream::Null().waitForCompletion();
+}
+
 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_);
@@ -42,6 +50,12 @@ void OFDisparityFilter::filter(Mat &disp, const Mat &gray)
 
 	gray.copyTo(gray_);
 
+	using cv::cuda::GpuMat;
+	GpuMat disp_gpu(disp);
+	filter(disp_gpu, GpuMat(flowxy_up_));
+	disp_gpu.download(disp);
+	return;
+
 	vector<float> values(n_max_);
 
 	for (int y = 0; y < size_.height; y++)