diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index 53220f174d4690776b6bb74fb97b729403387030..c4cb1e38dae435560cc4a564c4071f98cd802993 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -11,7 +11,7 @@ enable_testing()
 set(THREADS_PREFER_PTHREAD_FLAG ON)
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
 
-find_package( glog REQUIRED )
+#find_package( glog REQUIRED )
 if (WIN32)
 include_directories(${glog_DIR})
 endif (WIN32)
diff --git a/cv-node/src/algorithms/rtcensus.cu b/cv-node/src/algorithms/rtcensus.cu
index cb2010b7f4ca7945e1e7174dcf8fd7ab6c276372..0b888631d1ddc81bee13546bf82e0b34748ec6a7 100644
--- a/cv-node/src/algorithms/rtcensus.cu
+++ b/cv-node/src/algorithms/rtcensus.cu
@@ -90,6 +90,8 @@ __global__ void census_kernel(PtrStepSzb l, PtrStepSzb r, uint64_t *census) {
  */
 __global__ void disp_kernel(float *disp_l, float *disp_r, size_t width, size_t height, uint64_t *census, size_t ds) {	
 	//extern __shared__ uint64_t cache[];
+
+	const int gamma = 100;
 	
 	size_t u = (blockIdx.x * BLOCK_W) + threadIdx.x + RADIUS2;
 	size_t v_start = (blockIdx.y * ROWSperTHREAD) + RADIUS2;
@@ -115,6 +117,8 @@ __global__ void disp_kernel(float *disp_l, float *disp_r, size_t width, size_t h
 		uint16_t last_ham2 = 65535;
 		uint16_t min_disp1 = 65535;
 		uint16_t min_disp2 = 65535;
+		uint16_t min_disp1b = 65535;
+		uint16_t min_disp2b = 65535;
 		uint16_t min_before1 = 0;
 		uint16_t min_before2 = 0;
 		uint16_t min_after1 = 0;
@@ -152,6 +156,8 @@ __global__ void disp_kernel(float *disp_l, float *disp_r, size_t width, size_t h
 				min_before1 = last_ham1;
 				min_disp1 = hamming1;
 				dix1 = d;
+			} else if (hamming1 < min_disp1b) {
+				min_disp1b = hamming1;
 			}
 			if (dix1 == d) min_after1 = hamming1;
 			last_ham1 = hamming1;
@@ -160,6 +166,8 @@ __global__ void disp_kernel(float *disp_l, float *disp_r, size_t width, size_t h
 				min_before2 = last_ham2;
 				min_disp2 = hamming2;
 				dix2 = d;
+			} else if (hamming2 < min_disp2b) {
+				min_disp2b = hamming2;
 			}
 			if (dix2 == d) min_after2 = hamming2;
 			last_ham2 = hamming2;
@@ -173,9 +181,15 @@ __global__ void disp_kernel(float *disp_l, float *disp_r, size_t width, size_t h
 		float d1 = fit_parabola(dix1, min_disp1, min_before1, min_after1);
 		float d2 = fit_parabola(dix2, min_disp2, min_before2, min_after2);
 	
-	
-		disp_l[v*width+u] = d2;
-		disp_r[v*width+u] = d1;
+		// Confidence filter (25)
+		// TODO choice of gamma to depend on disparity variance
+		// Variance with next option, variance with neighbours, variance with past value
+		disp_l[v*width+u] = ((min_disp2b - min_disp2) >= gamma) ? d2 : INFINITY;
+		disp_r[v*width+u] = ((min_disp1b - min_disp1) >= gamma) ? d1 : INFINITY;
+
+		// TODO If disparity is 0.0f, perhaps
+		// Use previous value unless it conflicts with present
+		// Use neighbour values if texture matches 
 	}
 }
 
@@ -198,10 +212,8 @@ __global__ void consistency_kernel(float *d_sub_l, float *d_sub_r, PtrStepSz<flo
 		
 		auto b = d_sub_r[v*w+u-a];
 		
-		// TODO, if rejected, use semantic info to borrow neighbour
-		
-		if (abs(a-b) <= 1.0) disp(v,u) = abs((a+b)/2);
-		else disp(v,u) = 0.0f;
+		if (abs(a-b) <= 1.0) disp(v,u) = abs((a+b)/2); // was 1.0
+		else disp(v,u) = INFINITY;
 	//}
 	}
 
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index b35b6e31ec17b07d10cb641c227be798b848cac3..a5498bdc888d433121157c9bdaa8ec03d08d0589 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -158,9 +158,9 @@ int main(int argc, char **argv) {
 		l = l(rect);
 		
 		// HACK to make bad pixels invisible.
-		normalize(disparity32F, depth32F, 0, 255, NORM_MINMAX, CV_8U);
-		r = Mat(l.size(), CV_8UC3, Vec3i(255,255,255));
-		l.copyTo(r,depth32F);
+		//normalize(disparity32F, depth32F, 0, 255, NORM_MINMAX, CV_8U);
+		//r = Mat(l.size(), CV_8UC3, Vec3i(255,255,255));
+		//l.copyTo(r,depth32F);
 		
 		// TODO Send RGB+D data somewhere
 		
@@ -177,11 +177,11 @@ int main(int argc, char **argv) {
 			cv::Mat Q_32F; // = (Mat_<double>(4,4) << 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1); //(4,4,CV_32F);
 			calibrate.getQ().convertTo(Q_32F,CV_32F);
 			cv::Mat_<cv::Vec3f> XYZ(disparity32F.rows,disparity32F.cols);   // Output point cloud
-			reprojectImageTo3D(disparity32F, XYZ, Q_32F, false);
+			reprojectImageTo3D(disparity32F, XYZ, Q_32F, true);
 			
 			//cv::imshow("Points",XYZ);
 			
-			cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, r );
+			cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, l );
 			cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
 			
 			/* Rotation using rodrigues */