Discard discontinuity points

components/renderers/cpp/src/points.cu

@@ -2,17 +2,27 @@
 
 #define T_PER_BLOCK 8
 
+template <int RADIUS>
 __global__ void point_cloud_kernel(ftl::cuda::TextureObject<float4> output, ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera params, float4x4 pose)
 {
 	const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
 	const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
 
 	if (x < params.width && y < params.height) {
+		output(x,y) = make_float4(MINF, MINF, MINF, MINF);
+
 		float d = depth.tex2D((int)x, (int)y);
 
-		output(x,y) = (d >= params.minDepth && d <= params.maxDepth) ?
-			make_float4(pose * params.screenToCam(x, y, d), 0.0f) :
-			make_float4(MINF, MINF, MINF, MINF);
+		if (d >= params.minDepth && d <= params.maxDepth) {
+			// Is there a discontinuity nearby?
+			for (int u=-RADIUS; u<=RADIUS; ++u) {
+				for (int v=-RADIUS; v<=RADIUS; ++v) {
+					if (fabs(depth.tex2D((int)x+u, (int)y+v) - d) > 0.04f) return;
+				}
+			}
+
+			output(x,y) = make_float4(pose * params.screenToCam(x, y, d), d);
+		}
 	}
 }
 
@@ -20,7 +30,7 @@ void ftl::cuda::point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda:
 	const dim3 gridSize((params.width + T_PER_BLOCK - 1)/T_PER_BLOCK, (params.height + T_PER_BLOCK - 1)/T_PER_BLOCK);
 	const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
 
-	point_cloud_kernel<<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose);
+	point_cloud_kernel<2><<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose);
 	cudaSafeCall( cudaGetLastError() );
 
 #ifdef _DEBUG