Fix for not swapping texture objects in frames

applications/reconstruct/src/ilw.cpp

 #include "ilw.hpp"
+#include <ftl/utility/matrix_conversion.hpp>
+#include <ftl/rgbd/source.hpp>
+#include <ftl/cuda/points.hpp>
 
 using ftl::ILW;
 using ftl::detail::ILWData;
+using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Format;
+using cv::cuda::GpuMat;
 
 ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
 
@@ -11,10 +18,8 @@ ILW::~ILW() {
 
 }
 
-bool ILW::process(ftl::rgbd::FrameSet &fs) {
-	return true;
-	
-    _phase0(fs);
+bool ILW::process(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
+    _phase0(fs, stream);
 
     for (int i=0; i<2; ++i) {
         _phase1(fs);
@@ -28,8 +33,21 @@ bool ILW::process(ftl::rgbd::FrameSet &fs) {
     return true;
 }
 
-bool ILW::_phase0(ftl::rgbd::FrameSet &fs) {
-    // Clear points channel...
+bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
+    // Make points channel...
+    for (size_t i=0; i<fs.frames.size(); ++i) {
+		auto &f = fs.frames[i];
+		auto *s = fs.sources[i];
+
+		if (f.empty(Channel::Depth + Channel::Colour)) {
+			LOG(ERROR) << "Missing required channel";
+			continue;
+		}
+			
+        auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
+        auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse());
+        ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, stream);
+    }
 
 	// Upload camera data?
     return true;

applications/reconstruct/src/ilw.hpp

@@ -40,13 +40,13 @@ class ILW : public ftl::Configurable {
     /**
      * Take a frameset and perform the iterative lattice warping.
      */
-    bool process(ftl::rgbd::FrameSet &fs);
+    bool process(ftl::rgbd::FrameSet &fs, cudaStream_t stream=0);
 
     private:
     /*
      * Initialise data.
      */
-    bool _phase0(ftl::rgbd::FrameSet &fs);
+    bool _phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream);
 
     /*
      * Find possible correspondences and a confidence value.

applications/reconstruct/src/main.cpp

@@ -121,6 +121,7 @@ static void run(ftl::Configurable *root) {
 
 	bool busy = false;
 
+	group.setLatency(4);
 	group.setName("ReconGroup");
 	group.sync([splat,virt,&busy,&slave,&scene_A,&scene_B,&align](ftl::rgbd::FrameSet &fs) -> bool {
 		//cudaSetDevice(scene->getCUDADevice());

components/rgbd-sources/src/frame.cpp

@@ -66,6 +66,10 @@ void Frame::swapTo(ftl::rgbd::Channels channels, Frame &f) {
 
 			cv::swap(m1.host, m2.host);
 			cv::cuda::swap(m1.gpu, m2.gpu);
+
+			auto temptex = std::move(m2.tex);
+			m2.tex = std::move(m1.tex);
+			m1.tex = std::move(temptex);
 		}
 	}
 }

components/rgbd-sources/src/frameset.cpp

@@ -19,12 +19,12 @@ void FrameSet::download(ftl::rgbd::Channels c, cudaStream_t stream) {
 void FrameSet::swapTo(ftl::rgbd::FrameSet &fs) {
 	UNIQUE_LOCK(fs.mtx, lk);
 
-	if (fs.frames.size() != frames.size()) {
+	//if (fs.frames.size() != frames.size()) {
 		// Assume "this" is correct and "fs" is not.
 		fs.sources.clear();
 		for (auto s : sources) fs.sources.push_back(s);
 		fs.frames.resize(frames.size());
-	}
+	//}
 
 	fs.timestamp = timestamp;
 	fs.count = static_cast<int>(count);