WIP corrections to point mover

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.1f, stream);
+            _phase2(fs, 0.5f, stream);
         //}
 
 		// TODO: Break if no time left
@@ -125,10 +125,13 @@ bool ILW::_phase2(ftl::rgbd::FrameSet &fs, float rate, cudaStream_t stream) {
 	// strongly disagreeing vectors should cancel out
 	// A weak vector is overriden by a stronger one.
 
-    for (auto &f : fs.frames) {
+    for (size_t i=0; i<fs.frames.size(); ++i) {
+        auto &f = fs.frames[i];
+
         ftl::cuda::move_points(
              f.getTexture<float4>(Channel::Points),
              f.getTexture<float4>(Channel::EnergyVector),
+             fs.sources[i]->parameters(),
              rate,
              stream
         );

applications/reconstruct/src/ilw/ilw.cu

@@ -52,7 +52,8 @@ __global__ void correspondence_energy_vector_kernel(
 		if (world2.x == MINF) continue;
 
         // Determine degree of correspondence
-        const float confidence = 1.0f / length(world1 - world2);
+        const float l = length(world1 - world2);
+        const float confidence = (l < 0.1f) ? 1.0f / l : 0.0f;
 
         if (confidence > bestconf) {
             bestpoint = world2;
@@ -63,11 +64,14 @@ __global__ void correspondence_energy_vector_kernel(
     const float maxconf = warpMax(bestconf);
     if (maxconf == bestconf && maxconf > 0.0f) {
         vout(x,y) = vout.tex2D(x, y) + make_float4(
-            (world1.x - bestpoint.x) * maxconf,
-            (world1.y - bestpoint.y) * maxconf,
-            (world1.z - bestpoint.z) * maxconf,
+            (bestpoint.x - world1.x),
+            (bestpoint.y - world1.y),
+            (bestpoint.z - world1.z),
             maxconf);
-        eout(x,y) = eout.tex2D(x,y) + length(world1 - bestpoint)*maxconf;
+        eout(x,y) = length(world1 - bestpoint)*20.0f;
+    } else if (maxconf == 0.0f && lane == 0) {
+        vout(x,y) = make_float4(0.0f);
+        eout(x,y) = 0.0f;
     }
 }
 
@@ -99,13 +103,21 @@ void ftl::cuda::correspondence_energy_vector(
 __global__ void move_points_kernel(
     ftl::cuda::TextureObject<float4> p,
     ftl::cuda::TextureObject<float4> v,
+    ftl::rgbd::Camera camera,
     float rate) {
 
     const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
     const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
     
     if (x < p.width() && y < p.height()) {
-        p(x,y) = p(x,y) + rate * v.tex2D((int)x,(int)y);
+        const float4 world = p(x,y);
+        const float4 vec = v.tex2D((int)x,(int)y);
+
+        // Calculate screen space distortion with neighbours
+
+        if (vec.w > 0.0f) {
+            p(x,y) = world + rate * vec;
+        }
     }
 }
 
@@ -113,13 +125,14 @@ __global__ void move_points_kernel(
 void ftl::cuda::move_points(
         ftl::cuda::TextureObject<float4> &p,
         ftl::cuda::TextureObject<float4> &v,
+        const ftl::rgbd::Camera &camera,
         float rate,
         cudaStream_t stream) {
 
     const dim3 gridSize((p.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (p.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
     const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
 
-    move_points_kernel<<<gridSize, blockSize, 0, stream>>>(p,v,rate);
+    move_points_kernel<<<gridSize, blockSize, 0, stream>>>(p,v,camera,rate);
 
     cudaSafeCall( cudaGetLastError() );
 }

applications/reconstruct/src/ilw/ilw_cuda.hpp

@@ -23,6 +23,7 @@ void correspondence_energy_vector(
 void move_points(
     ftl::cuda::TextureObject<float4> &p,
     ftl::cuda::TextureObject<float4> &v,
+    const ftl::rgbd::Camera &camera,
     float rate,
     cudaStream_t stream
 );