diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index ff7d290981c1a84c5ee211219ae0651f85c58c77..3ac2e2de5f173a12bd7107d8d40b5d2eca3b4b5d 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/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"
diff --git a/components/rgbd-sources/include/ftl/offilter.hpp b/components/rgbd-sources/include/ftl/offilter.hpp
index 4c4fbbb9837ef39e80f9aad68c62ae5d82d4671e..c63b4ca5486d7b6698d6a9933d762093999d344a 100644
--- a/components/rgbd-sources/include/ftl/offilter.hpp
+++ b/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_;
diff --git a/components/rgbd-sources/src/algorithms/offilter.cu b/components/rgbd-sources/src/algorithms/offilter.cu
new file mode 100644
index 0000000000000000000000000000000000000000..ff5ecbe9a62cab842aa035a3736df99c79cffdd6
--- /dev/null
+++ b/components/rgbd-sources/src/algorithms/offilter.cu
@@ -0,0 +1,62 @@
+#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
diff --git a/components/rgbd-sources/src/cuda_algorithms.hpp b/components/rgbd-sources/src/cuda_algorithms.hpp
index 0aa7399c0a24035bdbbb0442b3c429ddd03d145c..439c16cfc21fef08086bb69b7e84b1c8b49fec74 100644
--- a/components/rgbd-sources/src/cuda_algorithms.hpp
+++ b/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);
}
}
diff --git a/components/rgbd-sources/src/offilter.cpp b/components/rgbd-sources/src/offilter.cpp
index 03db4807a7f9fa045708d3cf17bf32556b7bf5b5..c49e24c7a1f828a78d64bdec581031f985fcc453 100644
--- a/components/rgbd-sources/src/offilter.cpp
+++ b/components/rgbd-sources/src/offilter.cpp
@@ -1,4 +1,6 @@
#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++)