Attempt at hole fill

applications/reconstruct/src/ilw/ilw.cpp

@@ -157,8 +157,22 @@ bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
 
         f.createTexture<float>(Channel::Depth2, Format<float>(f.get<GpuMat>(Channel::Colour).size()));
         f.createTexture<float>(Channel::Confidence, Format<float>(f.get<GpuMat>(Channel::Colour).size()));
+		f.createTexture<int>(Channel::Mask, Format<int>(f.get<GpuMat>(Channel::Colour).size()));
         f.createTexture<uchar4>(Channel::Colour);
 		f.createTexture<float>(Channel::Depth);
+
+		ftl::cuda::preprocess_depth(
+			f.getTexture<float>(Channel::Depth),
+			f.getTexture<float>(Channel::Depth2),
+			f.getTexture<uchar4>(Channel::Colour),
+			f.getTexture<int>(Channel::Mask),
+			s->parameters(),
+			params_,
+			stream
+		);
+
+		//cv::cuda::swap(f.get<GpuMat>(Channel::Depth),f.get<GpuMat>(Channel::Depth2)); 
+		f.swapChannels(Channel::Depth, Channel::Depth2);
     }
 
     return true;

applications/reconstruct/src/ilw/ilw.cu

@@ -24,6 +24,67 @@ __device__ inline float warpSum(float e) {
 	return e;
 }
 
+//==============================================================================
+
+template <int RADIUS>
+__global__ void preprocess_kernel(
+    	ftl::cuda::TextureObject<float> depth_in,
+		ftl::cuda::TextureObject<float> depth_out,
+		ftl::cuda::TextureObject<uchar4> colour,
+		ftl::cuda::TextureObject<int> mask,
+		ftl::rgbd::Camera camera,
+		ftl::cuda::ILWParams params) {
+
+    const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
+    const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+	float d = depth_in.tex2D((int)x,(int)y);
+	uchar4 c = colour.tex2D((int)x,(int)y);
+
+	// Calculate discontinuity mask
+
+	// Fill missing depths
+	if (d < camera.minDepth || d > camera.maxDepth) {
+		float depth_accum = 0.0f;
+		float contrib = 0.0f;
+
+		for (int v=-RADIUS; v<=RADIUS; ++v) {
+			for (int u=-RADIUS; u<=RADIUS; ++u) {
+				uchar4 c2 = colour.tex2D((int)x+u,(int)y+v);
+				float d2 = depth_in.tex2D((int)x+u,(int)y+v);
+				if (d2 >= camera.minDepth && d2 <= camera.maxDepth) {
+					float w = ftl::cuda::colourWeighting(c, c2, params.colour_smooth);
+					depth_accum += d2*w;
+					contrib += w;
+				}
+			}
+		}
+
+		if (contrib >= 0.0f) d = depth_accum / contrib;
+	}
+
+	depth_out(x,y) = d;
+}
+
+void ftl::cuda::preprocess_depth(
+		ftl::cuda::TextureObject<float> &depth_in,
+		ftl::cuda::TextureObject<float> &depth_out,
+		ftl::cuda::TextureObject<uchar4> &colour,
+		ftl::cuda::TextureObject<int> &mask,
+		const ftl::rgbd::Camera &camera,
+		const ftl::cuda::ILWParams &params,
+		cudaStream_t stream) {
+
+	const dim3 gridSize((depth_in.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth_in.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+	const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+	preprocess_kernel<3><<<gridSize, blockSize, 0, stream>>>(depth_in, depth_out, colour, mask, camera, params);
+
+	cudaSafeCall( cudaGetLastError() );
+}
+
+//==============================================================================
+
 //#define COR_WIN_RADIUS 17
 //#define COR_WIN_SIZE (COR_WIN_RADIUS * COR_WIN_RADIUS)
 

applications/reconstruct/src/ilw/ilw_cuda.hpp

@@ -52,6 +52,16 @@ class ILWMask {
 	static const int kMask_Bad = 0x0008;
 };
 
+void preprocess_depth(
+	ftl::cuda::TextureObject<float> &depth_in,
+	ftl::cuda::TextureObject<float> &depth_out,
+	ftl::cuda::TextureObject<uchar4> &colour,
+	ftl::cuda::TextureObject<int> &mask,
+	const ftl::rgbd::Camera &camera,
+	const ILWParams &params,
+	cudaStream_t stream
+);
+
 void correspondence(
     ftl::cuda::TextureObject<float> &d1,
     ftl::cuda::TextureObject<float> &d2,

components/rgbd-sources/include/ftl/rgbd/frame.hpp

@@ -57,6 +57,8 @@ public:
 	 */
 	void swapTo(ftl::rgbd::Channels, Frame &);
 
+	void swapChannels(ftl::rgbd::Channel, ftl::rgbd::Channel);
+
 	/**
 	 * Create a channel with a given format. This will discard any existing
 	 * data associated with the channel and ensure all data structures and

components/rgbd-sources/src/frame.cpp

@@ -74,6 +74,17 @@ void Frame::swapTo(ftl::rgbd::Channels channels, Frame &f) {
 	}
 }
 
+void Frame::swapChannels(ftl::rgbd::Channel a, ftl::rgbd::Channel b) {
+	auto &m1 = _get(a);
+	auto &m2 = _get(b);
+	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);
+}
+
 template<> cv::Mat& Frame::get(ftl::rgbd::Channel channel) {
 	if (channel == Channel::None) {
 		DLOG(WARNING) << "Cannot get the None channel from a Frame";