Add colour cost

applications/reconstruct/src/ilw/ilw.cu

@@ -16,7 +16,7 @@ __device__ inline float warpMin(float e) {
 	return e;
 }
 
-#define COR_WIN_RADIUS 16
+#define COR_WIN_RADIUS 17
 #define COR_WIN_SIZE (COR_WIN_RADIUS * COR_WIN_RADIUS)
 
 __global__ void correspondence_energy_vector_kernel(
@@ -34,7 +34,8 @@ __global__ void correspondence_energy_vector_kernel(
 	const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE;
     const int y = blockIdx.y*blockDim.y + threadIdx.y;
     
-	const float3 world1 = make_float3(p1.tex2D(x, y));
+    const float3 world1 = make_float3(p1.tex2D(x, y));
+    const uchar4 colour1 = c1.tex2D(x, y);
 	if (world1.x == MINF) return;
     const float3 camPos2 = pose2 * world1;
     const uint2 screen2 = cam2.camToScreen<uint2>(camPos2);
@@ -50,11 +51,14 @@ __global__ void correspondence_energy_vector_kernel(
 		const float u = (i % COR_WIN_RADIUS) - (COR_WIN_RADIUS / 2);
         const float v = (i / COR_WIN_RADIUS) - (COR_WIN_RADIUS / 2);
         
-		const float3 world2 = make_float3(p2.tex2D(screen2.x+u, screen2.y+v));
+        const float3 world2 = make_float3(p2.tex2D(screen2.x+u, screen2.y+v));
+        const uchar4 colour2 = c2.tex2D(screen2.x+u, screen2.y+v);
 		if (world2.x == MINF) continue;
 
         // Determine degree of correspondence
-        const float cost = 1.0f - ftl::cuda::spatialWeighting(world1, world2, 0.04f);
+        float cost = 1.0f - ftl::cuda::spatialWeighting(world1, world2, 0.04f);
+        cost += 1.0f - ftl::cuda::colourWeighting(colour1, colour2, 50.0f);
+        cost /= 2.0f;
 
         if (cost < bestcost) {
             bestpoint = world2;
@@ -74,7 +78,8 @@ __global__ void correspondence_energy_vector_kernel(
             (bestpoint.y - world1.y),
             (bestpoint.z - world1.z),
             mincost);
-        eout(x,y) = mincost * 5.0f; //confidence * 5.0f;
+        //eout(x,y) = (1.0f - mincost) * 7.0f; //confidence * 5.0f;
+        eout(x,y) = confidence * 7.0f;
     } else if (mincost >= 1.0f && lane == 0) {
         vout(x,y) = make_float4(0.0f);
         eout(x,y) = 0.0f;