From bbb0dc350ce62f3441c2d854fa7cc7916e491528 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Fri, 27 Sep 2019 13:19:54 +0300
Subject: [PATCH] Display cost confidence

---
 applications/reconstruct/src/ilw/ilw.cpp           |  2 +-
 applications/reconstruct/src/ilw/ilw.cu            | 14 +++++++++++---
 .../renderers/cpp/include/ftl/cuda/points.hpp      |  6 ++++++
 3 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/applications/reconstruct/src/ilw/ilw.cpp b/applications/reconstruct/src/ilw/ilw.cpp
index e24af21f5..3b566df68 100644
--- a/applications/reconstruct/src/ilw/ilw.cpp
+++ b/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
diff --git a/applications/reconstruct/src/ilw/ilw.cu b/applications/reconstruct/src/ilw/ilw.cu
index 98b0658a5..c1222ca79 100644
--- a/applications/reconstruct/src/ilw/ilw.cu
+++ b/applications/reconstruct/src/ilw/ilw.cu
@@ -1,4 +1,5 @@
 #include "ilw_cuda.hpp"
+#include <ftl/cuda/weighting.hpp>
 
 using ftl::cuda::TextureObject;
 using ftl::rgbd::Camera;
@@ -39,6 +40,7 @@ __global__ void correspondence_energy_vector_kernel(
     const uint2 screen2 = cam2.camToScreen<uint2>(camPos2);
     
     float bestconf = 0.0f;
+    float nextbest = 0.0f;
     float3 bestpoint;
 
     // Project to p2 using cam2
@@ -53,22 +55,27 @@ __global__ void correspondence_energy_vector_kernel(
 
         // Determine degree of correspondence
         const float l = length(world1 - world2);
-        const float confidence = (l < 0.1f) ? 1.0f / l : 0.0f;
+        const float confidence = ftl::cuda::spatialWeighting(l, 0.04f);
 
         if (confidence > bestconf) {
             bestpoint = world2;
+            nextbest = bestconf;
             bestconf = confidence;
         }
     }
 
     const float maxconf = warpMax(bestconf);
-    if (maxconf == bestconf && maxconf > 0.0f) {
+    bool best = maxconf == bestconf;
+    bestconf = (best) ? 0.0f : bestconf;
+    const float conf = maxconf - warpMax(bestconf);
+
+    if (best && maxconf > 0.0f) {
         vout(x,y) = vout.tex2D(x, y) + make_float4(
             (bestpoint.x - world1.x),
             (bestpoint.y - world1.y),
             (bestpoint.z - world1.z),
             maxconf);
-        eout(x,y) = length(world1 - bestpoint)*20.0f;
+        eout(x,y) = conf * 5.0f; //maxconf * 5.0f; //(maxconf - warpMax(nextbest));
     } else if (maxconf == 0.0f && lane == 0) {
         vout(x,y) = make_float4(0.0f);
         eout(x,y) = 0.0f;
@@ -100,6 +107,7 @@ void ftl::cuda::correspondence_energy_vector(
 //==============================================================================
 
 
+
 __global__ void move_points_kernel(
     ftl::cuda::TextureObject<float4> p,
     ftl::cuda::TextureObject<float4> v,
diff --git a/components/renderers/cpp/include/ftl/cuda/points.hpp b/components/renderers/cpp/include/ftl/cuda/points.hpp
index deffe3277..667879985 100644
--- a/components/renderers/cpp/include/ftl/cuda/points.hpp
+++ b/components/renderers/cpp/include/ftl/cuda/points.hpp
@@ -10,6 +10,12 @@ namespace cuda {
 
 void point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda::TextureObject<float> &depth, const ftl::rgbd::Camera &params, const float4x4 &pose, cudaStream_t stream);
 
+void point_cloud(ftl::cuda::TextureObject<float> &output, ftl::cuda::TextureObject<float4> &points, const ftl::rgbd::Camera &params, const float4x4 &poseinv, cudaStream_t stream);
+
+void world_to_cam(ftl::cuda::TextureObject<float4> &points, const float4x4 &poseinv, cudaStream_t stream);
+
+void cam_to_world(ftl::cuda::TextureObject<float4> &points, const float4x4 &pose, cudaStream_t stream);
+
 }
 }
 
-- 
GitLab