diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index a3e5504a41b11ef0f2b020710b88dae050b0d9c1..55c6e6d1c92f60512817d4ac349b37cad9f6bd30 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -246,7 +246,7 @@ static void run(ftl::Configurable *root) {
bool busy = false;
- auto *smooth = ftl::config::create<ftl::DepthSmoother>(root, "filters");
+ auto *smooth = ftl::config::create<ftl::filters::DepthSmoother>(root, "filters");
group->setLatency(4);
group->setName("ReconGroup");
diff --git a/components/filters/include/ftl/filters/smoothing.hpp b/components/filters/include/ftl/filters/smoothing.hpp
index 5e5035f58981107d94f20ad948afe80297254edb..06f8a594155c95b7d5f1f2ab40f30086c4aa8f25 100644
--- a/components/filters/include/ftl/filters/smoothing.hpp
+++ b/components/filters/include/ftl/filters/smoothing.hpp
@@ -7,6 +7,7 @@
#include <ftl/rgbd/frame.hpp>
namespace ftl {
+namespace filters {
class DepthSmoother : public ftl::Configurable {
public:
@@ -20,6 +21,17 @@ class DepthSmoother : public ftl::Configurable {
ftl::rgbd::Frame frames_[4];
};
+class MLSSmoother : public ftl::Configurable {
+ public:
+ explicit MLSSmoother(nlohmann::json &config);
+ ~MLSSmoother();
+
+ void smooth(ftl::rgbd::Frame &frame, ftl::rgbd::Source *src);
+
+ private:
+};
+
+}
}
#endif // _FTL_SMOOTHING_HPP_
diff --git a/components/filters/src/smoothing.cpp b/components/filters/src/smoothing.cpp
index edd6a072fbb51cc498b243c8530f3abdb550e592..ce0e03db369e436b89d47f42c19a0c43fa1b0bb8 100644
--- a/components/filters/src/smoothing.cpp
+++ b/components/filters/src/smoothing.cpp
@@ -1,7 +1,8 @@
#include <ftl/filters/smoothing.hpp>
#include "smoothing_cuda.hpp"
-using ftl::DepthSmoother;
+using ftl::filters::DepthSmoother;
+using ftl::filters::MLSSmoother;
using ftl::codecs::Channel;
using cv::cuda::GpuMat;
@@ -53,3 +54,33 @@ void DepthSmoother::smooth(ftl::rgbd::Frame &f, ftl::rgbd::Source *s) {
//cv::cuda::subtract(f.get<GpuMat>(Channel::Depth), f.get<GpuMat>(Channel::Smoothing), f.get<GpuMat>(Channel::Depth));
}
+
+// ===== MLS ===================================================================
+
+MLSSmoother::MLSSmoother(nlohmann::json &config) : ftl::Configurable(config) {
+
+}
+
+MLSSmoother::~MLSSmoother() {
+
+}
+
+void MLSSmoother::smooth(ftl::rgbd::Frame &f, ftl::rgbd::Source *s) {
+ if (!f.hasChannel(Channel::Normals)) {
+ LOG(ERROR) << "Missing normals for MLS smooth";
+ return;
+ }
+
+ float thresh = value("mls_threshold", 0.04f);
+ ftl::cuda::mls_smooth(
+ f.createTexture<float4>(Channel::Normals),
+ f.createTexture<float>(Channel::Depth),
+ f.createTexture<float>(Channel::Depth2, ftl::rgbd::Format<float>(f.get<cv::cuda::GpuMat>(Channel::Depth).size())),
+ thresh,
+ s->parameters(),
+ 0
+ );
+
+
+}
+
diff --git a/components/filters/src/smoothing.cu b/components/filters/src/smoothing.cu
index 43cb1f6bd0a56760aa2c6d4d3fff4c664631c404..0ba749d25c57e23496df0a59b5428dea0a2c0d27 100644
--- a/components/filters/src/smoothing.cu
+++ b/components/filters/src/smoothing.cu
@@ -6,6 +6,90 @@ using ftl::cuda::TextureObject;
#define T_PER_BLOCK 8
+// ===== MLS Smooth ============================================================
+
+/*
+ * Smooth depth map using Moving Least Squares
+ */
+ template <int SEARCH_RADIUS>
+ __global__ void mls_smooth_kernel(
+ TextureObject<float4> normals,
+ TextureObject<float> depth_in, // Virtual depth map
+ TextureObject<float> depth_out, // Accumulated output
+ float smoothing,
+ ftl::rgbd::Camera camera) {
+
+ const int x = blockIdx.x*blockDim.x + threadIdx.x;
+ const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ if (x < 0 || y < 0 || x >= depth_in.width() || y >= depth_in.height()) return;
+
+ float3 aX = make_float3(0.0f,0.0f,0.0f);
+ float3 nX = make_float3(0.0f,0.0f,0.0f);
+ float contrib = 0.0f;
+
+ float d0 = depth_in.tex2D(x, y);
+ if (d0 < camera.minDepth || d0 > camera.maxDepth) return;
+ float3 X = camera.screenToCam((int)(x),(int)(y),d0);
+
+ // Neighbourhood
+ for (int v=-SEARCH_RADIUS; v<=SEARCH_RADIUS; ++v) {
+ for (int u=-SEARCH_RADIUS; u<=SEARCH_RADIUS; ++u) {
+ const float d = depth_in.tex2D(x+u, y+v);
+ if (d < camera.minDepth || d > camera.maxDepth) continue;
+
+ // 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));
+
+ // Gauss approx weighting function using point distance
+ const float w = ftl::cuda::spatialWeighting(X,Xi,smoothing);
+
+ aX += Xi*w;
+ nX += Ni*w;
+ contrib += w;
+ }
+ }
+
+ nX /= contrib; // Weighted average normal
+ aX /= contrib; // Weighted average point (centroid)
+
+ // Signed-Distance Field function
+ float fX = nX.x * (X.x - aX.x) + nX.y * (X.y - aX.y) + nX.z * (X.z - aX.z);
+
+ // Calculate new point using SDF function to adjust depth (and position)
+ X = X - nX * fX;
+
+ //uint2 screen = camera.camToScreen<uint2>(X);
+
+ //if (screen.x < depth_out.width() && screen.y < depth_out.height()) {
+ // depth_out(screen.x,screen.y) = X.z;
+ //}
+ depth_out(x,y) = X.z;
+}
+
+void ftl::cuda::mls_smooth(
+ ftl::cuda::TextureObject<float4> &normals,
+ ftl::cuda::TextureObject<float> &depth_in,
+ ftl::cuda::TextureObject<float> &depth_out,
+ float smoothing,
+ const ftl::rgbd::Camera &camera,
+ cudaStream_t stream) {
+
+ 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<3><<<gridSize, blockSize, 0, stream>>>(normals, depth_in, depth_out, smoothing, camera);
+ cudaSafeCall( cudaGetLastError() );
+
+
+ #ifdef _DEBUG
+ cudaSafeCall(cudaDeviceSynchronize());
+ #endif
+}
+
+// ===== Colour-based Smooth ===================================================
+
template <int RADIUS>
__global__ void depth_smooth_kernel(
ftl::cuda::TextureObject<float> depth_in,
diff --git a/components/filters/src/smoothing_cuda.hpp b/components/filters/src/smoothing_cuda.hpp
index da800f883f2e39a44253929fbc7675360e3a543d..ee28b4d489c717dfa60d43c942e264df9ed830d0 100644
--- a/components/filters/src/smoothing_cuda.hpp
+++ b/components/filters/src/smoothing_cuda.hpp
@@ -7,6 +7,14 @@
namespace ftl {
namespace cuda {
+void mls_smooth(
+ ftl::cuda::TextureObject<float4> &normals,
+ ftl::cuda::TextureObject<float> &depth_in,
+ ftl::cuda::TextureObject<float> &depth_out,
+ float smoothing,
+ const ftl::rgbd::Camera &camera,
+ cudaStream_t stream);
+
void depth_smooth(
ftl::cuda::TextureObject<float> &depth_in,
ftl::cuda::TextureObject<uchar4> &colour_in,