diff --git a/cv-node/src/algorithms/rtcensus.cu b/cv-node/src/algorithms/rtcensus.cu
index d9a92df23435f3bd09750f44254b1284ebed24e8..a041e7b29acab1d714ad39663f2d984457cca782 100644
--- a/cv-node/src/algorithms/rtcensus.cu
+++ b/cv-node/src/algorithms/rtcensus.cu
@@ -329,6 +329,8 @@ __global__ void filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
 				neigh = tex2D<unsigned char>(nTex, u+n, v+m);
 			}
 
+			if (m+n == 0) continue;
+
 			if (ndisp > 1.0f && !isnan(ndisp) && (abs(neigh-pixel) <= FILTER_SIM_THRESH)) { // && (isnan(disp) || abs(ndisp-disp) < FILTER_DISP_THRESH)) {
 				est += ndisp;
 				nn++;
@@ -339,7 +341,8 @@ __global__ void filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
 	// Texture map filtering
 	int tm = (neigh_sq / (15*15)) - ((neigh_sum*neigh_sum) / (15*15));
 	if (tm >= -9000000 && (abs(ppixel-pixel) > FILTER_SIM_THRESH || abs(pdisp - disp) <= FILTER_DISP_THRESH) ) {
-		f(v,u) = disp; // = (nn==0) ? NAN : est / nn;
+		if (nn > 2) f(v,u) = (nn==0) ? NAN : (est+disp) / (nn+1);
+		else f(v,u) = NAN;
 	} else {
 		f(v,u) = NAN;
 	}