Implements #133 point alignment

Further work to be done:

• switch to depth ray cast correspondences
• use normals etc in correspondences

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