Use neighbour motion vectors

applications/reconstruct/src/ilw/ilw.cpp

@@ -27,7 +27,7 @@ bool ILW::process(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
     //for (int i=0; i<2; ++i) {
         _phase1(fs, stream);
         //for (int j=0; j<3; ++j) {
-            //_phase2(fs, 0.5f, stream);
+            _phase2(fs, 0.5f, stream);
         //}
 
 		// TODO: Break if no time left

applications/reconstruct/src/ilw/ilw.cu

@@ -92,13 +92,13 @@ __global__ void correspondence_energy_vector_kernel(
             (bestpoint.x - world1.x),
             (bestpoint.y - world1.y),
             (bestpoint.z - world1.z),
-			mincost);
+			(1.0f - mincost) * confidence);
 			
 		//eout(x,y) = max(eout(x,y), (length(bestpoint-world1) / 0.04f) * 7.0f);
 		//eout(x,y) = max(eout(x,y), (1.0f - mincost) * 7.0f);
 		//eout(x,y) = max(eout(x, y), (1.0f - mincost) * confidence * (length(bestpoint-world1) / 0.04f) * 12.0f);
-		//eout(x,y) = max(eout(x, y), (1.0f - mincost) * confidence * 12.0f);
-		eout(x,y) = max(eout(x, y), confidence * 12.0f);
+		eout(x,y) = max(eout(x, y), (1.0f - mincost) * confidence * 12.0f);
+		//eout(x,y) = max(eout(x, y), confidence * 12.0f);
     } else if (mincost >= 1.0f && lane == 0) {
         //vout(x,y) = make_float4(0.0f);
         //eout(x,y) = 0.0f;
@@ -129,11 +129,11 @@ void ftl::cuda::correspondence_energy_vector(
 
 //==============================================================================
 
-
+#define MOTION_RADIUS 3
 
 __global__ void move_points_kernel(
     ftl::cuda::TextureObject<float4> p,
-    ftl::cuda::TextureObject<float4> v,
+    ftl::cuda::TextureObject<float4> ev,
     ftl::rgbd::Camera camera,
     float rate) {
 
@@ -142,12 +142,20 @@ __global__ void move_points_kernel(
     
     if (x < p.width() && y < p.height()) {
         const float4 world = p(x,y);
-        const float4 vec = v.tex2D((int)x,(int)y);
-
-        // Calculate screen space distortion with neighbours
+		float4 vec = ev.tex2D((int)x,(int)y);
+		float contrib = vec.w;
+
+		// Calculate screen space distortion with neighbours
+		for (int v=-MOTION_RADIUS; v<=MOTION_RADIUS; ++v) {
+			for (int u=-MOTION_RADIUS; u<=MOTION_RADIUS; ++u) {
+				const float4 vecn = ev.tex2D((int)x+u,(int)y+v);
+				contrib += vecn.w;
+				vec += vecn.w * vecn;
+			}
+		}
 
         if (vec.w > 0.0f) {
-            p(x,y) = world + rate * vec;
+            p(x,y) = world + rate * (vec / contrib);
         }
     }
 }