diff --git a/components/operators/include/ftl/operators/cuda/disparity.hpp b/components/operators/include/ftl/operators/cuda/disparity.hpp
index de4648f6f4c4d2dfe8681aea6a96d7fbc40b40af..75034d9e1808e6023712988e1a66ec2d8f118a0d 100644
--- a/components/operators/include/ftl/operators/cuda/disparity.hpp
+++ b/components/operators/include/ftl/operators/cuda/disparity.hpp
@@ -19,6 +19,16 @@ void mask_occlusions(const cv::cuda::GpuMat &depth,
cv::cuda::GpuMat &mask,
const ftl::rgbd::Camera &c, cudaStream_t stream);
+void check_reprojection(const cv::cuda::GpuMat &disp,
+ const ftl::cuda::TextureObject<uchar4> &left, const ftl::cuda::TextureObject<uchar4> &right,
+ cudaStream_t stream);
+
+void show_rpe(const cv::cuda::GpuMat &disp, cv::cuda::GpuMat &left, const cv::cuda::GpuMat &right,
+ float scale, cudaStream_t stream);
+
+void show_disp_density(const cv::cuda::GpuMat &disp, cv::cuda::GpuMat &left,
+ float scale, cudaStream_t stream);
+
void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
cv::cuda::GpuMat &history, cv::cuda::GpuMat &support, int n_max, float threshold, bool fill,
diff --git a/components/operators/src/disparity/disp2depth.cu b/components/operators/src/disparity/disp2depth.cu
index c0011adf4d92ea9d30a4f56e1ad9ccbe4c06b976..bed3bc3e88dbeb83fdbcd35b78ec31abad76c019 100644
--- a/components/operators/src/disparity/disp2depth.cu
+++ b/components/operators/src/disparity/disp2depth.cu
@@ -137,3 +137,107 @@ void ftl::cuda::mask_occlusions(const cv::cuda::GpuMat &depth, const cv::cuda::G
depth, depthR, mask, c);
cudaSafeCall( cudaGetLastError() );
}
+
+
+// =============================================================================
+
+__global__ void check_reprojection_kernel(cv::cuda::PtrStepSz<short> disp,
+ ftl::cuda::TextureObject<uchar4> left,
+ ftl::cuda::TextureObject<uchar4> right)
+{
+ for (STRIDE_Y(v,disp.rows)) {
+ for (STRIDE_X(u,disp.cols)) {
+ const float d = float(disp(v,u)) / 16.0f;
+ const float4 l = left.tex2D(float(u)+0.5f, float(v)+0.5f);
+
+ if (d > 0) {
+ const float4 r = right.tex2D(float(u-d)+0.5f, float(v)+0.5f);
+ const float diff = max(fabsf(l.x-r.x),max(fabsf(l.y-r.y), fabsf(l.z-r.z)));
+ if (diff > 10.0f) disp(v,u) = 0;
+ }
+ }
+ }
+}
+
+void ftl::cuda::check_reprojection(const cv::cuda::GpuMat &disp, const ftl::cuda::TextureObject<uchar4> &left, const ftl::cuda::TextureObject<uchar4> &right, cudaStream_t stream) {
+ dim3 grid(1,1,1);
+ dim3 threads(128, 4, 1);
+ grid.x = cv::cuda::device::divUp(disp.cols, 128);
+ grid.y = cv::cuda::device::divUp(disp.rows, 4);
+
+ check_reprojection_kernel<<<grid, threads, 0, stream>>>(disp, left, right);
+
+ cudaSafeCall( cudaGetLastError() );
+}
+
+
+// =============================================================================
+
+
+__global__ void show_rpe_kernel(cv::cuda::PtrStepSz<short> disp,
+ cv::cuda::PtrStepSz<uchar4> left,
+ cv::cuda::PtrStepSz<uchar4> right,
+ float scale)
+{
+ for (STRIDE_Y(v,left.rows)) {
+ for (STRIDE_X(u,left.cols)) {
+ short d = disp(v,u) / 16;
+
+ if (d > 0 && u-d >= 0) {
+ uchar4 l = left(v,u);
+ uchar4 r = right(v,u-d);
+ float d = max(abs(int(l.x)-int(r.x)),max(abs(int(l.y)-int(r.y)), abs(int(l.z)-int(r.z))));
+
+ left(v,u) = make_uchar4(0,0,min(255.0f, (d/scale) * 255.0f),255);
+ }
+ }
+ }
+}
+
+void ftl::cuda::show_rpe(const cv::cuda::GpuMat &disp, cv::cuda::GpuMat &left, const cv::cuda::GpuMat &right,
+ float scale, cudaStream_t stream) {
+ dim3 grid(1,1,1);
+ dim3 threads(128, 4, 1);
+ grid.x = cv::cuda::device::divUp(disp.cols, 128);
+ grid.y = cv::cuda::device::divUp(disp.rows, 4);
+ show_rpe_kernel<<<grid, threads, 0, stream>>>(
+ disp, left, right, scale);
+ cudaSafeCall( cudaGetLastError() );
+}
+
+// =============================================================================
+
+
+template <int MAX_DISP>
+__global__ void show_disp_density_kernel(cv::cuda::PtrStepSz<short> disp,
+ cv::cuda::PtrStepSz<uchar4> left,
+ float scale)
+{
+ for (STRIDE_Y(v,disp.rows)) {
+ for (STRIDE_X(u,disp.cols)) {
+ short d = disp(v,u) / 16;
+ int count = 0;
+
+ for (int i=1; i<MAX_DISP; ++i) {
+ if (u+i-d < disp.cols && u+i-d >= 0) {
+ short dd = disp(v,u+i-d) / 16;
+ if (d > 0 && dd == i) ++count;
+ }
+ }
+
+ count = max(0,count-1);
+ left(v,u) = make_uchar4(0,0,min(255.0f, (float(count)/4.0f) * 255.0f),255);
+ }
+ }
+}
+
+void ftl::cuda::show_disp_density(const cv::cuda::GpuMat &disp, cv::cuda::GpuMat &left,
+ float scale, cudaStream_t stream) {
+ dim3 grid(1,1,1);
+ dim3 threads(128, 4, 1);
+ grid.x = cv::cuda::device::divUp(disp.cols, 128);
+ grid.y = cv::cuda::device::divUp(disp.rows, 4);
+ show_disp_density_kernel<256><<<grid, threads, 0, stream>>>(
+ disp, left, scale);
+ cudaSafeCall( cudaGetLastError() );
+}
diff --git a/components/operators/src/disparity/fixstars_sgm.cpp b/components/operators/src/disparity/fixstars_sgm.cpp
index a473585ec935419684df68fa66cd825ef87a03fe..b55a7af6d7f7c4b006f1e8988aa4369be7db82e9 100644
--- a/components/operators/src/disparity/fixstars_sgm.cpp
+++ b/components/operators/src/disparity/fixstars_sgm.cpp
@@ -1,6 +1,7 @@
#include <loguru.hpp>
#include "ftl/operators/disparity.hpp"
+#include <ftl/operators/cuda/disparity.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudaarithm.hpp>
@@ -146,7 +147,7 @@ bool FixstarsSGM::apply(Frame &in, Frame &out, cudaStream_t stream) {
return false;
}
- const auto &l = in.get<GpuMat>(Channel::Left);
+ auto &l = in.get<GpuMat>(Channel::Left);
const auto &r = in.get<GpuMat>(Channel::Right);
if (l.size() != size_) {
@@ -170,9 +171,21 @@ bool FixstarsSGM::apply(Frame &in, Frame &out, cudaStream_t stream) {
cv::cuda::threshold(disp_int_, disp, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, cvstream);
+ if (config()->value("check_reprojection", false)) {
+ ftl::cuda::check_reprojection(disp, in.getTexture<uchar4>(Channel::Colour),
+ in.createTexture<uchar4>(Channel::Colour2, true),
+ stream);
+ }
+
if (config()->value("show_P2_map", false)) {
cv::cuda::cvtColor(P2_map_, out.get<GpuMat>(Channel::Colour), cv::COLOR_GRAY2BGRA);
}
+ if (config()->value("show_rpe", false)) {
+ ftl::cuda::show_rpe(disp, l, r, 100.0f, stream);
+ }
+ if (config()->value("show_disp_density", false)) {
+ ftl::cuda::show_disp_density(disp, l, 100.0f, stream);
+ }
//disp_int_.convertTo(disp, CV_32F, 1.0f / 16.0f, cvstream);
return true;