diff --git a/applications/reconstruct/src/dibr.cu b/applications/reconstruct/src/dibr.cu
index 8c2f20c6ae77f24925555d5f89e6435794d158b9..615bdd5b08e65ae9e1f47441fb1259ba726255ca 100644
--- a/applications/reconstruct/src/dibr.cu
+++ b/applications/reconstruct/src/dibr.cu
@@ -14,7 +14,7 @@
 #define DEPTH_THRESHOLD 0.05f
 #define UPSAMPLE_MAX 60
 #define MAX_ITERATIONS 10
-#define SPATIAL_SMOOTHING 0.01f
+#define SPATIAL_SMOOTHING 0.005f
 
 using ftl::cuda::TextureObject;
 using ftl::render::SplatParams;
@@ -67,7 +67,7 @@ __device__ inline bool isStable(const float3 &previous, const float3 &estimate,
 	if (camPos.z < params.camera.m_sensorDepthWorldMin) return;
 	if (camPos.z > params.camera.m_sensorDepthWorldMax) return;
 
-	const int upsample = min(UPSAMPLE_MAX-2, int((r) * params.camera.fx / camPos.z))+2;
+	const int upsample = min(UPSAMPLE_MAX-2, int((2.0f*r) * params.camera.fx / camPos.z))+2;
 	const float interval = 1.0f / float(upsample / 2);
 
             
@@ -123,7 +123,7 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
 	if (camPos.z > params.camera.m_sensorDepthWorldMax) return;
 	const uint2 screenPos = params.camera.cameraToKinectScreen(camPos);
 
-	const int upsample = min(UPSAMPLE_MAX, int((5.0f*r) * params.camera.fx / camPos.z));
+	const int upsample = min(UPSAMPLE_MAX, int((r) * params.camera.fx / camPos.z));
 
 	// Not on screen so stop now...
 	if (screenPos.x + upsample < 0 || screenPos.y + upsample < 0 ||
@@ -345,9 +345,9 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
 	}
 }
 
-#define NEIGHBOR_RADIUS_2 10
+#define NEIGHBOR_RADIUS_2 2
 #define NEIGHBOR_WINDOW ((NEIGHBOR_RADIUS_2*2+1)*(NEIGHBOR_RADIUS_2*2+1))
-#define MAX_NEIGHBORS_2 20
+#define MAX_NEIGHBORS_2 10
 
 
 /*
@@ -382,15 +382,12 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
 	if (camPos.z > params.camera.m_sensorDepthWorldMax) return;
 	const uint2 screenPos = params.camera.cameraToKinectScreen(camPos);
 
-	const int upsample = 10; //min(UPSAMPLE_MAX, int((4.0f*r) * params.camera.fx / camPos.z));
+	const int upsample = 16; //min(UPSAMPLE_MAX, int((4.0f*r) * params.camera.fx / camPos.z));
 
 	// Not on screen so stop now...
 	//if (screenPos.x + upsample < 0 || screenPos.y + upsample < 0 ||
     //        screenPos.x - upsample >= depth.width() || screenPos.y - upsample >= depth.height()) return;
-            
-	// TODO:(Nick) Check depth buffer and don't do anything if already hidden?
 	
-	// TODO:(Nick) Preload point neighbors and transform to eye
 	const int lane = threadIdx.x % WARP_SIZE;
 	if (lane == 0) {
 		minimum[warp] = 100000000;
@@ -400,12 +397,17 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
 
 	__syncwarp();
 
+	// Preload valid neighbour points from within a window
+	// TODO: Should be nearest neighbours, which it currently isn't if there are
+	// more than MAX_NEIGHBOUR_2 points.
 	for (int i=lane; i<NEIGHBOR_WINDOW; i+=WARP_SIZE) {
 		const int u = (i % (2*NEIGHBOR_RADIUS_2+1)) - NEIGHBOR_RADIUS_2;
 		const int v = (i / (2*NEIGHBOR_RADIUS_2+1)) - NEIGHBOR_RADIUS_2;
 		const float3 point = params.camera.kinectDepthToSkeleton(x+u, y+v, float(point_in.tex2D(x+u, y+v)) / 1000.0f);
 
+		// If it is close enough...
 		if (length(point - camPos) <= SPATIAL_SMOOTHING) {
+			// Append to neighbour list
 			unsigned int idx = atomicInc(&nidx[warp], MAX_NEIGHBORS_2-1);
 			neighborhood_cache[warp][idx] = point;
 			atomicMin(&minimum[warp], point.z*1000.0f);
@@ -418,62 +420,44 @@ __global__ void OLD_dibr_visibility_kernel(TextureObject<int> depth, int cam, Sp
 	const float interval = (float(maximum[warp])/1000.0f - float(minimum[warp]) / 1000.0f) / float(MAX_ITERATIONS);
 	//if (y == 200) printf("interval: %f\n", interval);
 
-	// TODO:(Nick) Find min and max depths of neighbors to estimate z bounds
-
 	// Each thread in warp takes an upsample point and updates corresponding depth buffer.
+	// TODO: Don't do this step, simply update the current pixel to either fill or replace existing value
+	// use warp threads to do the iteration samples ... 32 samples per pixel.
+	// could iterate each thread to perform more checks within likely range.
 	for (int i=lane; i<upsample*upsample; i+=WARP_SIZE) {
 		const float u = (i % upsample) - (upsample / 2);
 		const float v = (i / upsample) - (upsample / 2);
 
         // Make an initial estimate of the points location
-		// Use centroid depth as estimate...?
-        //float3 nearest = ftl::cuda::screen_centroid<1>(camera.points, make_float2(screenPos.x+u, screenPos.y+v), make_int2(x,y), params, upsample);
-
-		// Use current points z as estimate
-		// TODO: Use min point as estimate
+		// Use minimum z as first estimate
 		float3 nearest = params.camera.kinectDepthToSkeleton(screenPos.x+u,screenPos.y+v,float(minimum[warp])/1000.0f);
-		
-		// Or calculate upper and lower bounds for depth and do gradient
-		// descent until the gradient change is too small or max iter is reached
-		// and depth remains within the bounds.
-		// How to find min and max depths?
-
-		// TODO: (Nick) Estimate depth using points plane, but needs better normals.
-		//float t;
-		//if (ftl::cuda::intersectPlane(normal, worldPos, rayOrigin, rayDir, t)) {
-			// Plane based estimate of surface at this pixel
-			//const float3 nearest = rayOrigin + rayDir * camPos.z;
-
-            // Use MLS of camera neighbor points to get more exact estimate
-            // Iterate until pixel is stable on the surface.
-            for (int k=0; k<MAX_ITERATIONS; ++k) {
+		float lastenergy = 0.0f;
+		float lastdepth = nearest.z;
 
-                // TODO:(Nick) Should perhaps use points from all cameras?
-                // Instead of doing each camera separately...
-                // If the depth already is close then it has already been done and can skip this point
-				const float energy = ftl::cuda::mls_point_energy<MAX_NEIGHBORS_2>(neighborhood_cache[warp], nearest, nidx[warp], SPATIAL_SMOOTHING);
-				
-				if (energy <= 0.0f) break;
-            
-				//ftl::cuda::render_depth(depth, params, output);
+		// Search for best or threshold energy
+		for (int k=0; k<MAX_ITERATIONS; ++k) {
 
-                // This is essentially the SDF function f(x), only the normal should be estimated also from the weights
-                //const float d = nearest.z + (normal.x*output.x + normal.y*output.y + normal.z*output.z);
-
-				const float d = nearest.z;
-				nearest = params.camera.kinectDepthToSkeleton(screenPos.x+u,screenPos.y+v,d+interval);
-				
-				if (energy >= 0.1f) {
-					const unsigned int cx = screenPos.x+u;
-                    const unsigned int cy = screenPos.y+v;
-					if (d > params.camera.m_sensorDepthWorldMin && d < params.camera.m_sensorDepthWorldMax && cx < depth.width() && cy < depth.height()) {
-                        // Transform estimated point to virtual cam space and output z
-                        atomicMin(&depth(cx,cy), d * 1000.0f);
-					}
-					break;
+			const float energy = ftl::cuda::mls_point_energy<MAX_NEIGHBORS_2>(neighborhood_cache[warp], nearest, nidx[warp], SPATIAL_SMOOTHING);
+			
+			//if (energy <= 0.0f) break;
+		
+			const float d = nearest.z;
+			nearest = params.camera.kinectDepthToSkeleton(screenPos.x+u,screenPos.y+v,d+interval);
+			
+			// Search for first energy maximum above a threshold
+			if (lastenergy >= 0.01f && energy < lastenergy) {
+				const unsigned int cx = screenPos.x+u;
+				const unsigned int cy = screenPos.y+v;
+				if (lastdepth > params.camera.m_sensorDepthWorldMin && lastdepth < params.camera.m_sensorDepthWorldMax && cx < depth.width() && cy < depth.height()) {
+					// Transform estimated point to virtual cam space and output z
+					atomicMin(&depth(cx,cy), lastdepth * 1000.0f);
 				}
+				break;
 			}
-		//}
+
+			lastenergy = energy;
+			lastdepth = d;
+		}
 	}
 }