From db1c6641d52ba4611c20e9251b5329d9beabf5e7 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Fri, 21 Jun 2019 11:16:08 +0300
Subject: [PATCH] Threshold gpumat disparity to 0

---
 applications/reconstruct/src/scene_rep_hash_sdf.cu      | 4 ++--
 applications/registration/src/correspondances.cpp       | 8 ++++----
 components/rgbd-sources/include/ftl/rgbd/source.hpp     | 2 +-
 components/rgbd-sources/src/algorithms/fixstars_sgm.cpp | 2 ++
 4 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/applications/reconstruct/src/scene_rep_hash_sdf.cu b/applications/reconstruct/src/scene_rep_hash_sdf.cu
index ba982e7c7..cbeef0690 100644
--- a/applications/reconstruct/src/scene_rep_hash_sdf.cu
+++ b/applications/reconstruct/src/scene_rep_hash_sdf.cu
@@ -525,7 +525,7 @@ __global__ void integrateDepthMapKernel(HashData hashData, DepthCameraData camer
 		//return;
 
 		// Depth is within accepted max distance from camera
-		if (depth > 0 && depth < hashParams.m_maxIntegrationDistance) { // valid depth and color (Nick: removed colour check)
+		if (depth > 0.01f && depth < hashParams.m_maxIntegrationDistance) { // valid depth and color (Nick: removed colour check)
 			float depthZeroOne = cameraData.cameraToKinectProjZ(depth);
 
 			// Calculate SDF of this voxel wrt the depth map value
@@ -584,7 +584,7 @@ __global__ void integrateDepthMapKernel(HashData hashData, DepthCameraData camer
 		} else {
 			uint idx = entry.ptr + i;
 			float coldist = colourDistance(color, hashData.d_SDFBlocks[idx].color);
-			if (depth > 40.0f && coldist > 100.0f) {
+			if ((depth > 39.99f || depth < 0.01f) && coldist > 100.0f) {
 				//hashData.d_SDFBlocks[idx].color = make_uchar3(0,0,(uchar)(coldist));
 				hashData.d_SDFBlocks[idx].weight = hashData.d_SDFBlocks[idx].weight >> 1;
 			}
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 5292905e9..d9794cedc 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -48,7 +48,7 @@ static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
 		// Calculate mean
 		for (int i_in = 0; i_in < in.size(); ++i_in) {
 			double d = in[i_in].at<float>(i);
-			if (d < 40.0) {
+			if (ftl::rgbd::isValidDepth(d)) {
 				good_values++;
 				sum += d;
 			}
@@ -61,7 +61,7 @@ static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
 			double var = 0.0;
 			for (int i_in = 0; i_in < in.size(); ++i_in) {
 				double d = in[i_in].at<float>(i);
-				if (d < 40.0) {
+				if (ftl::rgbd::isValidDepth(d)) {
 					double delta = (d*1000.0 - sum*1000.0);
 					var += delta*delta;
 				}
@@ -74,10 +74,10 @@ static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
 			if (var < varThresh) {
 				out.at<float>(i) = (float)sum;
 			} else {
-				out.at<float>(i) = 41.0f;
+				out.at<float>(i) = 0.0f;
 			}
 		} else {
-			out.at<float>(i) = 41.0f;
+			out.at<float>(i) = 0.0f;
 		}
 	}
 }
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index fd7446607..d5b57a72c 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -19,7 +19,7 @@ class Universe;
 
 namespace rgbd {
 
-static inline bool isValidDepth(float d) { return (d > 0.0f) && (d <= 40.0f); }
+static inline bool isValidDepth(float d) { return (d > 0.01f) && (d < 39.99f); }
 
 class SnapshotReader;
 
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index a04c01836..daa1a335b 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -43,6 +43,8 @@ void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
 	GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows));
 	left_pixels.setTo(0);
 
+	cv::cuda::threshold(dispt_, dispt_, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, stream);
+
 	if (use_filter_) {
 		// parameters need benchmarking, impact of image
 		// quick tests show with parameters (max_disp_, 25, 3)
-- 
GitLab