Add normal filtering to render

components/renderers/cpp/include/ftl/cuda/normals.hpp

@@ -17,6 +17,11 @@ void normal_visualise(ftl::cuda::TextureObject<float4> &norm,
         const ftl::rgbd::Camera &camera, const float4x4 &pose,
         cudaStream_t stream);
 
+void normal_filter(ftl::cuda::TextureObject<float4> &norm,
+        ftl::cuda::TextureObject<float4> &points,
+        const ftl::rgbd::Camera &camera, const float4x4 &pose,
+        float thresh, cudaStream_t stream);
+
 }
 }
 

components/renderers/cpp/include/ftl/render/splat_render.hpp

@@ -43,6 +43,7 @@ class Splatter : public ftl::render::Renderer {
 	ftl::rgbd::FrameSet *scene_;
 	ftl::cuda::ClipSpace clip_;
 	bool clipping_;
+	float norm_filter_;
 };
 
 }

components/renderers/cpp/src/normals.cu

@@ -103,7 +103,12 @@ __global__ void vis_normals_kernel(ftl::cuda::TextureObject<float4> norm,
     if (l == 0) return;
     n /= l;
 
-    output(x,y) = (1.0f + dot(ray, n))*3.5f;  // FIXME: Do not hard code these value scalings
+    const float d = dot(ray, n);
+    output(x,y) = (1.0f + d)*3.5f;  // FIXME: Do not hard code these value scalings
+    
+    //if (d > 0.2f) {
+    //    output(x,y) = d * 7.0f;
+    //}
 }
 
 void ftl::cuda::normal_visualise(ftl::cuda::TextureObject<float4> &norm,
@@ -122,3 +127,47 @@ void ftl::cuda::normal_visualise(ftl::cuda::TextureObject<float4> &norm,
     //cutilCheckMsg(__FUNCTION__);
 #endif
 }
+
+//==============================================================================
+
+__global__ void filter_normals_kernel(ftl::cuda::TextureObject<float4> norm,
+        ftl::cuda::TextureObject<float4> output,
+        ftl::rgbd::Camera camera, float4x4 pose, float thresh) {
+    const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
+    const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+    if(x >= norm.width() || y >= norm.height()) return;
+
+    float3 ray = pose.getFloat3x3() * camera.screenToCam(x,y,1.0f);
+    ray = ray / length(ray);
+    float3 n = make_float3(norm.tex2D((int)x,(int)y));
+    float l = length(n);
+    if (l == 0) {
+        output(x,y) = make_float4(MINF);
+        return;
+    }
+    n /= l;
+
+    const float d = dot(ray, n);
+    if (d <= thresh) {
+        output(x,y) = make_float4(MINF);
+    }
+}
+
+void ftl::cuda::normal_filter(ftl::cuda::TextureObject<float4> &norm,
+        ftl::cuda::TextureObject<float4> &output,
+        const ftl::rgbd::Camera &camera, const float4x4 &pose,
+        float thresh,
+        cudaStream_t stream) {
+
+    const dim3 gridSize((norm.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (norm.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+    const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+    filter_normals_kernel<<<gridSize, blockSize, 0, stream>>>(norm, output, camera, pose, thresh);
+
+    cudaSafeCall( cudaGetLastError() );
+    #ifdef _DEBUG
+    cudaSafeCall(cudaDeviceSynchronize());
+    //cutilCheckMsg(__FUNCTION__);
+    #endif
+}

components/renderers/cpp/src/splat_render.cpp

@@ -49,6 +49,11 @@ Splatter::Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::rende
 	on("clipping_enabled", [this](const ftl::config::Event &e) {
 		clipping_ = value("clipping_enabled", true);
 	});
+
+	norm_filter_ = value("normal_filter", -1.0f);
+	on("normal_filter", [this](const ftl::config::Event &e) {
+		norm_filter_ = value("normal_filter", -1.0f);
+	});
 }
 
 Splatter::~Splatter() {
@@ -251,6 +256,26 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda
 
 	temp_.createTexture<int>(Channel::Depth);
 
+	// First make sure each input has normals
+	temp_.createTexture<float4>(Channel::Normals);
+	for (int i=0; i<scene_->frames.size(); ++i) {
+		auto &f = scene_->frames[i];
+		auto s = scene_->sources[i];
+
+		if (!f.hasChannel(Channel::Normals)) {
+			auto &g = f.get<GpuMat>(Channel::Colour);
+			ftl::cuda::normals(f.createTexture<float4>(Channel::Normals, Format<float4>(g.cols, g.rows)),
+				temp_.getTexture<float4>(Channel::Normals),  // FIXME: Uses assumption of vcam res same as input res
+				f.getTexture<float4>(Channel::Points), stream);
+
+			if (norm_filter_ > -0.1f) {
+				Eigen::Matrix4f matrix =  s->getPose().cast<float>();
+				auto pose = MatrixConversion::toCUDA(matrix);
+				ftl::cuda::normal_filter(f.getTexture<float4>(Channel::Normals), f.getTexture<float4>(Channel::Points), s->parameters(), pose, norm_filter_, stream);
+			}
+		}
+	}
+
 	renderChannel(params, out, Channel::Colour, stream);
 	
 	Channel chan = src->getChannel();
@@ -258,24 +283,6 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda
 	{
 		temp_.get<GpuMat>(Channel::Depth).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 1000.0f, cvstream);
 	} else if (chan == Channel::Normals) {
-		//temp_.get<GpuMat>(Channel::Depth).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 1000.0f, cvstream);
-		//ftl::cuda::point_cloud(temp_.createTexture<float4>(Channel::Points, Format<float4>(camera.width, camera.height)),
-		//	temp_.createTexture<float>(Channel::Depth), camera, params.m_viewMatrixInverse, stream);
-		//ftl::cuda::normals(temp_.getTexture<float4>(Channel::Normals), temp_.getTexture<float4>(Channel::Points), stream);
-		//ftl::cuda::normal_visualise(temp_.getTexture<float4>(Channel::Normals), temp_.getTexture<float>(Channel::Contribution), camera);
-
-		// First make sure each input has normals
-		temp_.createTexture<float4>(Channel::Normals);
-		for (auto &f : scene_->frames) {
-			if (!f.hasChannel(Channel::Normals)) {
-				auto &g = f.get<GpuMat>(Channel::Colour);
-				LOG(INFO) << "Make normals channel";
-				ftl::cuda::normals(f.createTexture<float4>(Channel::Normals, Format<float4>(g.cols, g.rows)),
-					temp_.getTexture<float4>(Channel::Normals),  // FIXME: Uses assumption of vcam res same as input res
-					f.getTexture<float4>(Channel::Points), stream);
-			}
-		}
-
 		out.create<GpuMat>(Channel::Normals, Format<float4>(camera.width, camera.height));
 
 		// Render normal attribute