Add normals mask

applications/reconstruct/src/main.cpp

@@ -32,6 +32,8 @@
 
 #include <ftl/operators/smoothing.hpp>
 #include <ftl/operators/colours.hpp>
+#include <ftl/operators/normals.hpp>
+
 #include <ftl/cuda/normals.hpp>
 #include <ftl/registration.hpp>
 
@@ -251,7 +253,7 @@ static void run(ftl::Configurable *root) {
 	auto *prefilter = ftl::config::create<ftl::operators::Graph>(root, "pre_filters");
 	prefilter->append<ftl::operators::ColourChannels>("colour");
 	prefilter->append<ftl::operators::HFSmoother>("hfnoise");
-	// Smooth normals
+	prefilter->append<ftl::operators::Normals>("normals");
 	prefilter->append<ftl::operators::SimpleMLS>("mls");
 	// Alignment
 

components/common/cpp/include/ftl/cuda_common.hpp

@@ -102,6 +102,9 @@ class TextureObject : public TextureObjectBase {
 
 	operator cv::cuda::GpuMat();
 
+	void create(const cv::Size &);
+	void create(int w, int h);
+
 	__host__ __device__ T *devicePtr() const { return (T*)(ptr_); };
 	__host__ __device__ T *devicePtr(int v) const { return &(T*)(ptr_)[v*pitch2_]; }
 
@@ -264,6 +267,20 @@ TextureObject<T>::TextureObject(size_t width, size_t height) {
 	//needsdestroy_ = true;
 }
 
+#ifndef __CUDACC__
+template <typename T>
+void TextureObject<T>::create(const cv::Size &s) {
+	create(s.width, s.height);
+}
+
+template <typename T>
+void TextureObject<T>::create(int w, int h) {
+	if (width_ != w || height_ != h) {
+		*this = std::move(TextureObject<T>(w, h));
+	}
+}
+#endif
+
 template <typename T>
 TextureObject<T>::TextureObject(const TextureObject<T> &p) {
 	texobj_ = p.texobj_;

components/operators/CMakeLists.txt

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

components/operators/include/ftl/operators/normals.hpp

+#ifndef _FTL_OPERATORS_NORMALS_HPP_
+#define _FTL_OPERATORS_NORMALS_HPP_
+
+#include <ftl/operators/operator.hpp>
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace operators {
+
+/**
+ * Calculate rough normals from local depth gradients.
+ */
+class Normals : public ftl::operators::Operator {
+	public:
+    explicit Normals(ftl::Configurable*);
+    ~Normals();
+
+	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;
+
+};
+
+/**
+ * Calculate rough normals from local depth gradients and perform a weighted
+ * smoothing over the neighbourhood.
+ */
+class SmoothNormals : public ftl::operators::Operator {
+	public:
+    explicit SmoothNormals(ftl::Configurable*);
+    ~SmoothNormals();
+
+	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;
+
+	private:
+	ftl::cuda::TextureObject<float4> temp_;
+
+};
+
+}
+}
+
+#endif  // _FTL_OPERATORS_NORMALS_HPP_

components/operators/src/normals.cpp

+#include <ftl/operators/normals.hpp>
+#include <ftl/cuda/normals.hpp>
+#include <ftl/utility/matrix_conversion.hpp>
+
+using ftl::operators::Normals;
+using ftl::operators::SmoothNormals;
+using ftl::codecs::Channel;
+using ftl::rgbd::Format;
+
+Normals::Normals(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+
+}
+
+Normals::~Normals() {
+
+}
+
+bool Normals::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
+	if (!in.hasChannel(Channel::Depth)) {
+		LOG(ERROR) << "Missing depth channel in Normals operator";
+		return false;
+	}
+
+	if (out.hasChannel(Channel::Normals)) {
+		LOG(WARNING) << "Output already has normals";
+	}
+
+	ftl::cuda::normals(
+		out.createTexture<float4>(Channel::Normals, ftl::rgbd::Format<float4>(in.get<cv::cuda::GpuMat>(Channel::Depth).size())),
+		in.createTexture<float>(Channel::Depth),
+		s->parameters(), 0
+	);
+
+	return true;
+}
+
+
+SmoothNormals::SmoothNormals(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+
+}
+
+SmoothNormals::~SmoothNormals() {
+
+}
+
+bool SmoothNormals::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
+    float smoothing = config()->value("normal_smoothing", 0.02f);
+    int radius = max(0, min(config()->value("radius",1), 5));
+
+	if (!in.hasChannel(Channel::Depth)) {
+		LOG(ERROR) << "Missing depth channel in SmoothNormals operator";
+		return false;
+	}
+
+	if (out.hasChannel(Channel::Normals)) {
+		LOG(WARNING) << "Output already has normals";
+	}
+
+	auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);
+
+	temp_.create(depth.size());
+
+    ftl::cuda::normals(
+		out.createTexture<float4>(Channel::Normals, Format<float4>(depth.size())),
+		temp_,
+		in.createTexture<float>(Channel::Depth),
+		radius, smoothing,
+		s->parameters(),
+		MatrixConversion::toCUDA(s->getPose().cast<float>().inverse()).getFloat3x3(),
+		MatrixConversion::toCUDA(s->getPose().cast<float>()).getFloat3x3(),
+		stream
+	);
+
+
+	return true;
+}

components/operators/src/smoothing.cpp

@@ -74,11 +74,13 @@ bool SimpleMLS::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::So
 	int radius = config()->value("mls_radius",2);
 
 	if (!in.hasChannel(Channel::Normals)) {
-		ftl::cuda::normals(
+		/*ftl::cuda::normals(
 			in.createTexture<float4>(Channel::Normals, ftl::rgbd::Format<float4>(in.get<cv::cuda::GpuMat>(Channel::Depth).size())),
 			in.createTexture<float>(Channel::Depth),
 			s->parameters(), 0
-		);
+		);*/
+		LOG(ERROR) << "Required normals channel missing for MLS";
+		return false;
 	}
 
 	// FIXME: Assume in and out are the same frame.