diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 553ac66659acffc1c16d8af21f611c685ef62f03..8954a4c62ff980c163f4c83fc9f7e77adb4b0f7e 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/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
diff --git a/components/common/cpp/include/ftl/cuda_common.hpp b/components/common/cpp/include/ftl/cuda_common.hpp
index 70a6a4ad6d4dc0def715979f9eaf348e621d103e..116e26ec74ff4b469e8d1214cafa2897264f80ad 100644
--- a/components/common/cpp/include/ftl/cuda_common.hpp
+++ b/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_;
diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index a1ef77bd357672775c6e5bf9d3899b416c2f8956..58e21f43eb785a24d4e7aa77681192b5c5d77d44 100644
--- a/components/operators/CMakeLists.txt
+++ b/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
diff --git a/components/operators/include/ftl/operators/normals.hpp b/components/operators/include/ftl/operators/normals.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5aff09e4da3b5867a77631c81750c42a6c23fdbd
--- /dev/null
+++ b/components/operators/include/ftl/operators/normals.hpp
@@ -0,0 +1,45 @@
+#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_
diff --git a/components/operators/src/normals.cpp b/components/operators/src/normals.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f8554c2986f27de3fe5bd0ab2fb03fbf2226c35
--- /dev/null
+++ b/components/operators/src/normals.cpp
@@ -0,0 +1,76 @@
+#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;
+}
diff --git a/components/operators/src/smoothing.cpp b/components/operators/src/smoothing.cpp
index 1657d483a9d20b2c62289eedab88c7003a6a41eb..38576b5cbd8b220ee25c10a64ffb8ceb16cb5c8b 100644
--- a/components/operators/src/smoothing.cpp
+++ b/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.