From d7a0c7834e91daa82c43da9ede53725c33e1936d Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Fri, 11 Oct 2019 20:45:39 +0300
Subject: [PATCH] Allow clipping before align
---
applications/reconstruct/src/ilw/ilw.cpp | 45 +++++++++++++++++++
applications/reconstruct/src/ilw/ilw.hpp | 4 ++
.../renderers/cpp/include/ftl/cuda/points.hpp | 4 ++
components/renderers/cpp/src/points.cu | 26 +++++++++++
4 files changed, 79 insertions(+)
diff --git a/applications/reconstruct/src/ilw/ilw.cpp b/applications/reconstruct/src/ilw/ilw.cpp
index 3ce265b5b..7e0c29259 100644
--- a/applications/reconstruct/src/ilw/ilw.cpp
+++ b/applications/reconstruct/src/ilw/ilw.cpp
@@ -13,6 +13,17 @@ using ftl::rgbd::Channels;
using ftl::rgbd::Format;
using cv::cuda::GpuMat;
+// TODO: Put in common place
+static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
+ Eigen::Affine3d rx =
+ Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
+ Eigen::Affine3d ry =
+ Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
+ Eigen::Affine3d rz =
+ Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
+ return rz * rx * ry;
+}
+
ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
enabled_ = value("ilw_align", true);
iterations_ = value("iterations", 4);
@@ -95,6 +106,33 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
if (value("colour_confidence_only", false)) params_.flags |= ftl::cuda::kILWFlag_ColourConfidenceOnly;
else params_.flags &= ~ftl::cuda::kILWFlag_ColourConfidenceOnly;
});
+
+ if (config["clipping"].is_object()) {
+ auto &c = config["clipping"];
+ float rx = c.value("pitch", 0.0f);
+ float ry = c.value("yaw", 0.0f);
+ float rz = c.value("roll", 0.0f);
+ float x = c.value("x", 0.0f);
+ float y = c.value("y", 0.0f);
+ float z = c.value("z", 0.0f);
+ float width = c.value("width", 1.0f);
+ float height = c.value("height", 1.0f);
+ float depth = c.value("depth", 1.0f);
+
+ Eigen::Affine3f r = create_rotation_matrix(rx, ry, rz).cast<float>();
+ Eigen::Translation3f trans(Eigen::Vector3f(x,y,z));
+ Eigen::Affine3f t(trans);
+
+ clip_.origin = MatrixConversion::toCUDA(r.matrix() * t.matrix());
+ clip_.size = make_float3(width, height, depth);
+ clipping_ = value("clipping_enabled", true);
+ } else {
+ clipping_ = false;
+ }
+
+ on("clipping_enabled", [this](const ftl::config::Event &e) {
+ clipping_ = value("clipping_enabled", true);
+ });
}
ILW::~ILW() {
@@ -159,6 +197,13 @@ bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
cv::cuda::cvtColor(tmp,col, cv::COLOR_BGR2BGRA, 0, cvstream);
}
+ // Clip first?
+ if (clipping_) {
+ auto clip = clip_;
+ clip.origin = clip.origin * pose;
+ ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), s->parameters(), clip, stream);
+ }
+
f.createTexture<float>(Channel::Depth2, Format<float>(f.get<GpuMat>(Channel::Colour).size()));
f.createTexture<float>(Channel::Confidence, Format<float>(f.get<GpuMat>(Channel::Colour).size()));
f.createTexture<int>(Channel::Mask, Format<int>(f.get<GpuMat>(Channel::Colour).size()));
diff --git a/applications/reconstruct/src/ilw/ilw.hpp b/applications/reconstruct/src/ilw/ilw.hpp
index 98357ba73..5abf31207 100644
--- a/applications/reconstruct/src/ilw/ilw.hpp
+++ b/applications/reconstruct/src/ilw/ilw.hpp
@@ -8,6 +8,8 @@
#include "ilw_cuda.hpp"
+#include <ftl/cuda/points.hpp>
+
namespace ftl {
namespace detail {
@@ -71,6 +73,8 @@ class ILW : public ftl::Configurable {
bool use_lab_;
int discon_mask_;
bool fill_depth_;
+ ftl::cuda::ClipSpace clip_;
+ bool clipping_;
};
}
diff --git a/components/renderers/cpp/include/ftl/cuda/points.hpp b/components/renderers/cpp/include/ftl/cuda/points.hpp
index a705e39e2..3b31a0be1 100644
--- a/components/renderers/cpp/include/ftl/cuda/points.hpp
+++ b/components/renderers/cpp/include/ftl/cuda/points.hpp
@@ -21,6 +21,10 @@ void point_cloud(ftl::cuda::TextureObject<float4> &output,
void clipping(ftl::cuda::TextureObject<float4> &points,
const ClipSpace &clip, cudaStream_t stream);
+void clipping(ftl::cuda::TextureObject<float> &depth,
+ const ftl::rgbd::Camera &camera,
+ const ClipSpace &clip, cudaStream_t stream);
+
void point_cloud(ftl::cuda::TextureObject<float> &output, ftl::cuda::TextureObject<float4> &points, const ftl::rgbd::Camera ¶ms, const float4x4 &poseinv, cudaStream_t stream);
void world_to_cam(ftl::cuda::TextureObject<float4> &points, const float4x4 &poseinv, cudaStream_t stream);
diff --git a/components/renderers/cpp/src/points.cu b/components/renderers/cpp/src/points.cu
index a188adaef..f1f23974f 100644
--- a/components/renderers/cpp/src/points.cu
+++ b/components/renderers/cpp/src/points.cu
@@ -87,6 +87,21 @@ __global__ void clipping_kernel(ftl::cuda::TextureObject<float4> points, ftl::cu
}
}
+__global__ void clipping_kernel(ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera camera, ftl::cuda::ClipSpace clip)
+{
+ const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
+ const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+ if (x < depth.width() && y < depth.height()) {
+ float d = depth(x,y);
+ float4 p = make_float4(camera.screenToCam(x,y,d), 0.0f);
+
+ if (isClipped(p, clip)) {
+ depth(x,y) = 0.0f;
+ }
+ }
+}
+
void ftl::cuda::clipping(ftl::cuda::TextureObject<float4> &points,
const ClipSpace &clip, cudaStream_t stream) {
@@ -96,3 +111,14 @@ void ftl::cuda::clipping(ftl::cuda::TextureObject<float4> &points,
clipping_kernel<<<gridSize, blockSize, 0, stream>>>(points, clip);
cudaSafeCall( cudaGetLastError() );
}
+
+void ftl::cuda::clipping(ftl::cuda::TextureObject<float> &depth,
+ const ftl::rgbd::Camera &camera,
+ const ClipSpace &clip, cudaStream_t stream) {
+
+const dim3 gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+clipping_kernel<<<gridSize, blockSize, 0, stream>>>(depth, camera, clip);
+cudaSafeCall( cudaGetLastError() );
+}
--
GitLab