Use MLS normals in next iteration

components/filters/src/smoothing.cpp

@@ -74,8 +74,7 @@ void MLSSmoother::filter(ftl::rgbd::Frame &f, ftl::rgbd::Source *s, cudaStream_t
 
 	//if (!do_smooth) return;
 
-	for (int i=0; i<iters; ++i) {
-		if (i > 0 || !f.hasChannel(Channel::Normals)) {
+	if (!f.hasChannel(Channel::Normals)) {
 		ftl::cuda::normals(
 			f.createTexture<float4>(Channel::Normals, ftl::rgbd::Format<float4>(f.get<cv::cuda::GpuMat>(Channel::Depth).size())),
 			f.createTexture<float>(Channel::Depth),
@@ -83,8 +82,10 @@ void MLSSmoother::filter(ftl::rgbd::Frame &f, ftl::rgbd::Source *s, cudaStream_t
 		);
 	}
 
+	for (int i=0; i<iters; ++i) {
 		ftl::cuda::mls_smooth(
 			f.createTexture<float4>(Channel::Normals),
+			f.createTexture<float4>(Channel::Points, ftl::rgbd::Format<float4>(f.get<cv::cuda::GpuMat>(Channel::Depth).size())),
 			f.createTexture<float>(Channel::Depth),
 			f.createTexture<float>(Channel::Depth2, ftl::rgbd::Format<float>(f.get<cv::cuda::GpuMat>(Channel::Depth).size())),
 			thresh,
@@ -93,6 +94,7 @@ void MLSSmoother::filter(ftl::rgbd::Frame &f, ftl::rgbd::Source *s, cudaStream_t
 		);
 
 		f.swapChannels(Channel::Depth, Channel::Depth2);
+		f.swapChannels(Channel::Normals, Channel::Points);
 	}
 }
 

components/filters/src/smoothing.cu

@@ -13,7 +13,8 @@ using ftl::cuda::TextureObject;
  */
  template <int SEARCH_RADIUS>
  __global__ void mls_smooth_kernel(
-        TextureObject<float4> normals,
+		TextureObject<float4> normals_in,
+		TextureObject<float4> normals_out,
         TextureObject<float> depth_in,        // Virtual depth map
 		TextureObject<float> depth_out,   // Accumulated output
 		float smoothing,
@@ -41,7 +42,7 @@ using ftl::cuda::TextureObject;
 
 		// Point and normal of neighbour
 		const float3 Xi = camera.screenToCam((int)(x)+u,(int)(y)+v,d);
-		const float3 Ni = make_float3(normals.tex2D((int)(x)+u, (int)(y)+v));
+		const float3 Ni = make_float3(normals_in.tex2D((int)(x)+u, (int)(y)+v));
 
 		// Gauss approx weighting function using point distance
 		const float w = ftl::cuda::spatialWeighting(X,Xi,smoothing);
@@ -67,10 +68,12 @@ using ftl::cuda::TextureObject;
     //    depth_out(screen.x,screen.y) = X.z;
 	//}
 	depth_out(x,y) = X.z;
+	normals_out(x,y) = make_float4(nX, 0.0f);
 }
 
 void ftl::cuda::mls_smooth(
-		ftl::cuda::TextureObject<float4> &normals,
+		ftl::cuda::TextureObject<float4> &normals_in,
+		ftl::cuda::TextureObject<float4> &normals_out,
 		ftl::cuda::TextureObject<float> &depth_in,
 		ftl::cuda::TextureObject<float> &depth_out,
 		float smoothing,
@@ -80,7 +83,7 @@ void ftl::cuda::mls_smooth(
 	const dim3 gridSize((depth_out.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth_out.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
 	const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
 
-	mls_smooth_kernel<2><<<gridSize, blockSize, 0, stream>>>(normals, depth_in, depth_out, smoothing, camera);
+	mls_smooth_kernel<2><<<gridSize, blockSize, 0, stream>>>(normals_in, normals_out, depth_in, depth_out, smoothing, camera);
 	cudaSafeCall( cudaGetLastError() );
 
 

components/filters/src/smoothing_cuda.hpp

@@ -8,7 +8,8 @@ namespace ftl {
 namespace cuda {
 
 void mls_smooth(
-		ftl::cuda::TextureObject<float4> &normals,
+		ftl::cuda::TextureObject<float4> &normals_in,
+		ftl::cuda::TextureObject<float4> &normals_out,
 		ftl::cuda::TextureObject<float> &depth_in,
 		ftl::cuda::TextureObject<float> &depth_out,
 		float smoothing,