Resolves #229 field filling attempts

applications/reconstruct/src/main.cpp

@@ -33,6 +33,7 @@
 #include <ftl/operators/smoothing.hpp>
 #include <ftl/operators/colours.hpp>
 #include <ftl/operators/normals.hpp>
+#include <ftl/operators/filling.hpp>
 
 #include <ftl/cuda/normals.hpp>
 #include <ftl/registration.hpp>
@@ -255,7 +256,8 @@ static void run(ftl::Configurable *root) {
 	pipeline1->append<ftl::operators::HFSmoother>("hfnoise");  // Remove high-frequency noise
 	pipeline1->append<ftl::operators::Normals>("normals");  // Estimate surface normals
 	pipeline1->append<ftl::operators::SmoothChannel>("smoothing");  // Generate a smoothing channel
-	pipeline1->append<ftl::operators::AdaptiveMLS>("mls");  // Perform MLS (using smoothing channel)
+	//pipeline1->append<ftl::operators::ScanFieldFill>("filling");  // Generate a smoothing channel
+	pipeline1->append<ftl::operators::ColourMLS>("mls");  // Perform MLS (using smoothing channel)
 	// Alignment
 
 

components/common/cpp/src/timer.cpp

@@ -169,6 +169,8 @@ namespace ftl {
 	extern bool running;
 }
 
+//static MUTEX mtx;
+
 void ftl::timer::start(bool block) {
 	if (active) return;
 	active = true;

components/operators/CMakeLists.txt

@@ -6,6 +6,8 @@ add_library(ftloperators
 	src/operator.cpp
 	src/colours.cpp
 	src/normals.cpp
+	src/filling.cpp
+	src/filling.cu
 )
 
 # These cause errors in CI build and are being removed from PCL in newer versions

components/operators/include/ftl/operators/filling.hpp

+#ifndef _FTL_OPERATORS_FILLING_HPP_
+#define _FTL_OPERATORS_FILLING_HPP_
+
+#include <ftl/operators/operator.hpp>
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace operators {
+
+/**
+ * Use colour distance field to scanline correct depth edges and fill holes.
+ */
+class ScanFieldFill : public ftl::operators::Operator {
+	public:
+    explicit ScanFieldFill(ftl::Configurable*);
+    ~ScanFieldFill();
+
+	inline Operator::Type type() const override { return Operator::Type::OneToOne; }
+
+    bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
+
+};
+
+}
+}
+
+#endif  // _FTL_OPERATORS_FILLING_HPP_

components/operators/src/filling.cpp

+#include <ftl/operators/filling.hpp>
+
+#include "filling_cuda.hpp"
+
+using ftl::operators::ScanFieldFill;
+using ftl::codecs::Channel;
+
+ScanFieldFill::ScanFieldFill(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+
+}
+
+ScanFieldFill::~ScanFieldFill() {
+
+}
+
+bool ScanFieldFill::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
+	float thresh = config()->value("threshold", 0.1f);
+
+	ftl::cuda::scan_field_fill(
+		in.createTexture<float>(Channel::Depth),
+		in.createTexture<float>(Channel::Depth),
+		in.createTexture<float>(Channel::Smoothing),
+		thresh,
+		s->parameters(), 0
+	);
+
+	return true;
+}

components/operators/src/filling.cu

+#include "filling_cuda.hpp"
+
+#define WARP_SIZE 32
+#define MAX_EDGES 128
+
+using ftl::cuda::TextureObject;
+
+__device__ inline bool isValid(const ftl::rgbd::Camera &camera, float d) {
+	return d > camera.minDepth && d < camera.maxDepth; 
+}
+
+__global__ void scan_field_fill_kernel(
+		TextureObject<float> depth_in,        // Virtual depth map
+		TextureObject<float> depth_out,   // Accumulated output
+		TextureObject<float> smoothing,
+		float thresh,
+		ftl::rgbd::Camera camera) {
+
+	__shared__ int counter;
+	__shared__ int offsets[MAX_EDGES];
+		
+	//const int x = blockIdx.x*blockDim.x + threadIdx.x;
+	const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+	if (y >= depth_in.height()) return;
+	if (threadIdx.x == 0) counter = 0;
+	__syncthreads();
+
+	for (STRIDE_X(x,depth_in.width()-1)) {
+
+		// Use entire block to find depth edges and make a list of them
+		float d0 = depth_in.tex2D(x,y);
+		float d1 = depth_in.tex2D(x+1,y);
+
+		if (isValid(camera, d0) != isValid(camera, d1)) {
+			int ix = atomicAdd(&counter, 1);
+			if (ix >= MAX_EDGES) break;
+			offsets[ix] = x;
+		}
+	}
+
+	__syncthreads();
+
+	// For each identified pixel, a thread takes on a filling job
+	for (int i=threadIdx.x; i<counter; i += blockDim.x) {
+		// Get smoothing gradient to decide action
+		int x = offsets[i];
+
+		float s0 = smoothing.tex2D(x,y);
+		float d0 = depth_in.tex2D(x,y);
+		float d1 = depth_in.tex2D(x+1,y);
+
+		// More than 8 pixels from edge
+		if (s0 < thresh) continue;
+		//if (s0 < 2.0f / 100.0f) continue;
+
+		/*bool increase = false;
+		for (int u=1; u<20; ++u) {
+			float s1 = smoothing.tex2D(x+u,y);
+			if (s1 != s0) {
+				increase = s1 > s0;
+				break;
+			}
+		}*/
+
+		// TODO: Smoothing channel has discrete steps meaning gradient can't be determined? CHECK
+		// The above would be why some slices are not filled
+
+		//bool fill_right = (isValid(camera, d0) && !increase);
+		//bool clear_right = (isValid(camera, d1) && !increase);
+		//bool fill_left = (isValid(camera, d1) && increase);
+		//bool clear_left = (isValid(camera, d0) && increase);
+		//bool clear = clear_left || clear_right;
+
+		bool fill_right = isValid(camera, d0);
+
+		//if (s1 == s0 || max(s1,s0) > 0.1f) continue;
+
+		// Set up fill value and direction
+		//float value = ((clear_left || clear_right) && s0 < thresh) ? 1000.0f : (isValid(camera, d0) ? d0 : d1);
+		//int dir = (fill_right || clear_right) ? 1 : -1;
+
+		float value = (isValid(camera, d0)) ? d0 : d1;
+		int dir = (fill_right) ? 1 : -1;
+
+		// TODO: The minimum needs to be search for and not crossed into, the filling
+		// must stop before the minimum. Requires lookahead.
+
+		/*int end_x = 0;
+		for (int u=1; u<1000; ++u) {
+			float s = smoothing(x+u*dir, y);
+			if (s < thresh) break;
+			end_x = u;
+		}
+
+		float gradient = */
+
+		// Fill
+		int end_x = 0;
+		float end_d;
+		for (int u=1; u<1000; ++u) {
+			end_d = depth_in(x+u*dir, y);
+			float s = smoothing(x+u*dir, y);
+			if ((s < thresh) || (isValid(camera, end_d))) break;
+			end_x = u;
+		}
+
+		float gradient = (end_d - value) / (float)end_x;
+
+		for (int u=1; u<end_x; ++u) {
+			depth_out(x+u*dir,y) = value;
+			value += gradient;
+		}
+	}
+}
+
+void ftl::cuda::scan_field_fill(
+		TextureObject<float> &depth_in,        // Virtual depth map
+		TextureObject<float> &depth_out,   // Accumulated output
+		TextureObject<float> &smoothing,
+		float thresh,
+		const ftl::rgbd::Camera &camera,
+		cudaStream_t stream) {
+
+	const dim3 gridSize(1, smoothing.height());
+	const dim3 blockSize(128, 1);
+
+	scan_field_fill_kernel<<<gridSize, blockSize, 0, stream>>>(depth_in, depth_out, smoothing, thresh, camera);
+	cudaSafeCall( cudaGetLastError() );
+}

components/operators/src/filling_cuda.hpp

+#ifndef _FTL_CUDA_FILLING_HPP_
+#define _FTL_CUDA_FILLING_HPP_
+
+#include <ftl/rgbd/camera.hpp>
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace cuda {
+
+void scan_field_fill(
+		ftl::cuda::TextureObject<float> &depth_in,
+		ftl::cuda::TextureObject<float> &depth_out,
+		ftl::cuda::TextureObject<float> &smoothing,
+		float thresh,
+		const ftl::rgbd::Camera &camera,
+		cudaStream_t stream);
+
+}
+}
+
+#endif
\ No newline at end of file

components/operators/src/mls.cu

@@ -131,6 +131,7 @@ void ftl::cuda::mls_smooth(
 
 	float d0 = depth_in.tex2D(x, y);
 	depth_out(x,y) = d0;
+	normals_out(x,y) = normals_in(x,y);
 	if (d0 < camera.minDepth || d0 > camera.maxDepth) return;
 	float3 X = camera.screenToCam((int)(x),(int)(y),d0);
 
@@ -146,6 +147,8 @@ void ftl::cuda::mls_smooth(
 		const float3 Xi = camera.screenToCam((int)(x)+u,(int)(y)+v,d);
 		const float3 Ni = make_float3(normals_in.tex2D((int)(x)+u, (int)(y)+v));
 
+		if (Ni.x+Ni.y+Ni.z == 0.0f) continue;
+
 		const uchar4 c = colour_in.tex2D(x+u, y+v);
 		const float cw = ftl::cuda::colourWeighting(c0,c,colour_smoothing);
 
@@ -157,6 +160,8 @@ void ftl::cuda::mls_smooth(
 		contrib += w;
     }
 	}
+
+	if (contrib == 0.0f) return;
 	
 	nX /= contrib;  // Weighted average normal
 	aX /= contrib;  // Weighted average point (centroid)

components/operators/src/smoothchan.cu

@@ -32,6 +32,7 @@ __global__ void smooth_chan_kernel(
 
 		//const float3 pos = camera.screenToCam(ix, iy, d0);
 
+		//int v=0;
 		for (int v=-RADIUS; v<=RADIUS; ++v) {
 			#pragma unroll
 			for (int u=-RADIUS; u<=RADIUS; ++u) {

components/operators/src/smoothing.cpp

@@ -181,8 +181,8 @@ ColourMLS::~ColourMLS() {
 bool ColourMLS::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
 	float thresh = config()->value("mls_threshold", 0.04f);
 	float col_smooth = config()->value("mls_colour_smoothing", 30.0f);
-	int iters = config()->value("mls_iterations", 1);
-	int radius = config()->value("mls_radius",2);
+	int iters = config()->value("mls_iterations", 10);
+	int radius = config()->value("mls_radius",3);
 
 	if (!in.hasChannel(Channel::Normals)) {
 		LOG(ERROR) << "Required normals channel missing for MLS";