Implement option z restriction

applications/reconstruct/src/ilw/ilw.cpp

@@ -22,11 +22,17 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
 
     flags_ = 0;
     if (value("ignore_bad", false)) flags_ |= ftl::cuda::kILWFlag_IgnoreBad;
+    if (value("restrict_z", true)) flags_ |= ftl::cuda::kILWFlag_RestrictZ;
 
     on("ignore_bad", [this](const ftl::config::Event &e) {
         if (value("ignore_bad", false)) flags_ |= ftl::cuda::kILWFlag_IgnoreBad;
         else flags_ &= ~ftl::cuda::kILWFlag_IgnoreBad;
     });
+
+    on("restrict_z", [this](const ftl::config::Event &e) {
+        if (value("restrict_z", false)) flags_ |= ftl::cuda::kILWFlag_RestrictZ;
+        else flags_ &= ~ftl::cuda::kILWFlag_RestrictZ;
+    });
 }
 
 ILW::~ILW() {
@@ -105,7 +111,7 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
 
             auto &f1 = fs.frames[i];
             auto &f2 = fs.frames[j];
-            //auto s1 = fs.frames[i];
+            auto s1 = fs.sources[i];
             auto s2 = fs.sources[j];
 
 			// Are cameras facing similar enough direction?
@@ -114,7 +120,8 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
 			// No, so skip this combination
 			if (d1.dot(d2) <= 0.0) continue;
 
-            auto pose = MatrixConversion::toCUDA(s2->getPose().cast<float>().inverse());
+            auto pose1 = MatrixConversion::toCUDA(s1->getPose().cast<float>().inverse());
+            auto pose2 = MatrixConversion::toCUDA(s2->getPose().cast<float>().inverse());
 
             try {
             //Calculate energy vector to best correspondence
@@ -126,7 +133,8 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
                 // TODO: Add normals and other things...
                 f1.getTexture<float4>(Channel::EnergyVector),
                 f1.getTexture<float>(Channel::Energy),
-                pose,
+                pose1,
+                pose2,
                 s2->parameters(),
                 flags_,
                 stream

applications/reconstruct/src/ilw/ilw.cu

@@ -34,6 +34,7 @@ __global__ void correspondence_energy_vector_kernel(
         TextureObject<uchar4> c2,
         TextureObject<float4> vout,
         TextureObject<float> eout,
+        float4x4 pose1,  // Inverse
         float4x4 pose2,  // Inverse
         Camera cam2, uint flags) {
 
@@ -88,10 +89,17 @@ __global__ void correspondence_energy_vector_kernel(
     const float confidence = (avgcost - mincost);
 
     if (best && mincost < 1.0f) {
+        float3 tvecA = pose1 * bestpoint;
+        float3 tvecB = pose1 * world1;
+        if (flags & ftl::cuda::kILWFlag_RestrictZ) {
+            tvecA.x = tvecB.x;
+            tvecA.y = tvecB.y;
+        }
+        tvecA = (pose1.getInverse() * tvecA) - world1;
         vout(x,y) = vout.tex2D(x, y) + make_float4(
-            (bestpoint.x - world1.x),
-            (bestpoint.y - world1.y),
-            (bestpoint.z - world1.z),
+            tvecA.x, // * (1.0f - mincost) * confidence,
+            tvecA.y, // * (1.0f - mincost) * confidence,
+            tvecA.z, // * (1.0f - mincost) * confidence,
 			(1.0f - mincost) * confidence);
 			
 		//eout(x,y) = max(eout(x,y), (length(bestpoint-world1) / 0.04f) * 7.0f);
@@ -112,6 +120,7 @@ void ftl::cuda::correspondence_energy_vector(
         TextureObject<uchar4> &c2,
         TextureObject<float4> &vout,
         TextureObject<float> &eout,
+        float4x4 &pose1,
         float4x4 &pose2,
         const Camera &cam2, uint flags,
         cudaStream_t stream) {
@@ -122,7 +131,7 @@ void ftl::cuda::correspondence_energy_vector(
     //printf("COR SIZE %d,%d\n", p1.width(), p1.height());
 
     correspondence_energy_vector_kernel<<<gridSize, blockSize, 0, stream>>>(
-        p1, p2, c1, c2, vout, eout, pose2, cam2, flags
+        p1, p2, c1, c2, vout, eout, pose1, pose2, cam2, flags
     );
     cudaSafeCall( cudaGetLastError() );
 }

applications/reconstruct/src/ilw/ilw_cuda.hpp

@@ -9,6 +9,7 @@ namespace ftl {
 namespace cuda {
 
 static const uint kILWFlag_IgnoreBad = 0x0001;
+static const uint kILWFlag_RestrictZ = 0x0002;
 
 void correspondence_energy_vector(
     ftl::cuda::TextureObject<float4> &p1,
@@ -17,6 +18,7 @@ void correspondence_energy_vector(
     ftl::cuda::TextureObject<uchar4> &c2,
     ftl::cuda::TextureObject<float4> &vout,
     ftl::cuda::TextureObject<float> &eout,
+    float4x4 &pose1,
     float4x4 &pose2,
     const ftl::rgbd::Camera &cam2,
     uint flags,