From 754a1eac93c19c6ceab96f78646c33fafc8473b8 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Sun, 29 Sep 2019 17:54:10 +0300
Subject: [PATCH] Support non-axis aligned clipping
---
applications/reconstruct/src/main.cpp | 31 ++++++++++++++-
.../renderers/cpp/include/ftl/cuda/points.hpp | 13 ++++++-
.../cpp/include/ftl/render/splat_render.hpp | 3 ++
components/renderers/cpp/src/points.cu | 32 ++++++++++++++++
components/renderers/cpp/src/splat_render.cpp | 38 ++++++++++++++++++-
5 files changed, 114 insertions(+), 3 deletions(-)
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 6c1d8a881..576f72929 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -51,6 +51,16 @@ using std::chrono::milliseconds;
using ftl::registration::loadTransformations;
using ftl::registration::saveTransformations;
+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;
+}
+
static void run(ftl::Configurable *root) {
Universe *net = ftl::create<Universe>(root, "net");
ftl::ctrl::Slave slave(net, root);
@@ -66,6 +76,24 @@ static void run(ftl::Configurable *root) {
return;
}
+ // Create scene transform, intended for axis aligning the walls and floor
+ Eigen::Matrix4d transform;
+ if (root->getConfig()["transform"].is_object()) {
+ auto &c = root->getConfig()["transform"];
+ 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);
+
+ Eigen::Affine3d r = create_rotation_matrix(rx, ry, rz);
+ Eigen::Translation3d trans(Eigen::Vector3d(x,y,z));
+ Eigen::Affine3d t(trans);
+ transform = t.matrix() * r.matrix();
+ LOG(INFO) << "Set transform: " << transform;
+ }
+
// Must find pose for each source...
if (sources.size() > 1) {
std::map<std::string, Eigen::Matrix4d> transformations;
@@ -87,9 +115,10 @@ static void run(ftl::Configurable *root) {
//sources = { sources[0] };
//sources[0]->setPose(Eigen::Matrix4d::Identity());
//break;
+ input->setPose(transform * input->getPose());
continue;
}
- input->setPose(T->second);
+ input->setPose(transform * T->second);
}
}
diff --git a/components/renderers/cpp/include/ftl/cuda/points.hpp b/components/renderers/cpp/include/ftl/cuda/points.hpp
index deffe3277..ddbac8ba9 100644
--- a/components/renderers/cpp/include/ftl/cuda/points.hpp
+++ b/components/renderers/cpp/include/ftl/cuda/points.hpp
@@ -8,7 +8,18 @@
namespace ftl {
namespace cuda {
-void point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda::TextureObject<float> &depth, const ftl::rgbd::Camera ¶ms, const float4x4 &pose, cudaStream_t stream);
+struct ClipSpace {
+ float4x4 origin;
+ float3 size;
+};
+
+void point_cloud(ftl::cuda::TextureObject<float4> &output,
+ ftl::cuda::TextureObject<float> &depth,
+ const ftl::rgbd::Camera ¶ms,
+ const float4x4 &pose, cudaStream_t stream);
+
+void clipping(ftl::cuda::TextureObject<float4> &points,
+ const ClipSpace &clip, cudaStream_t stream);
}
}
diff --git a/components/renderers/cpp/include/ftl/render/splat_render.hpp b/components/renderers/cpp/include/ftl/render/splat_render.hpp
index 2cbb82a81..ca7bfa2fb 100644
--- a/components/renderers/cpp/include/ftl/render/splat_render.hpp
+++ b/components/renderers/cpp/include/ftl/render/splat_render.hpp
@@ -4,6 +4,7 @@
#include <ftl/render/renderer.hpp>
#include <ftl/rgbd/frameset.hpp>
#include <ftl/render/splat_params.hpp>
+#include <ftl/cuda/points.hpp>
namespace ftl {
namespace render {
@@ -40,6 +41,8 @@ class Splatter : public ftl::render::Renderer {
ftl::rgbd::Frame temp_;
ftl::rgbd::FrameSet *scene_;
+ ftl::cuda::ClipSpace clip_;
+ bool clipping_;
};
}
diff --git a/components/renderers/cpp/src/points.cu b/components/renderers/cpp/src/points.cu
index 39764e4c8..1bfca42fc 100644
--- a/components/renderers/cpp/src/points.cu
+++ b/components/renderers/cpp/src/points.cu
@@ -21,8 +21,40 @@ void ftl::cuda::point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda:
const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
point_cloud_kernel<<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose);
+ cudaSafeCall( cudaGetLastError() );
#ifdef _DEBUG
cudaSafeCall(cudaDeviceSynchronize());
#endif
}
+
+//==============================================================================
+
+__device__ bool isClipped(const float4 &p, const ftl::cuda::ClipSpace &clip) {
+ const float3 tp = clip.origin * make_float3(p);
+ return fabs(tp.x) > clip.size.x || fabs(tp.y) > clip.size.y || fabs(tp.z) > clip.size.z;
+}
+
+__global__ void clipping_kernel(ftl::cuda::TextureObject<float4> points, 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 < points.width() && y < points.height()) {
+ float4 p = points(x,y);
+
+ if (isClipped(p, clip)) {
+ points(x,y) = make_float4(MINF, MINF, MINF, MINF);
+ }
+ }
+}
+
+void ftl::cuda::clipping(ftl::cuda::TextureObject<float4> &points,
+ const ClipSpace &clip, cudaStream_t stream) {
+
+ const dim3 gridSize((points.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (points.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
+ const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+ clipping_kernel<<<gridSize, blockSize, 0, stream>>>(points, clip);
+ cudaSafeCall( cudaGetLastError() );
+}
diff --git a/components/renderers/cpp/src/splat_render.cpp b/components/renderers/cpp/src/splat_render.cpp
index 1b39ccebf..aed04ad89 100644
--- a/components/renderers/cpp/src/splat_render.cpp
+++ b/components/renderers/cpp/src/splat_render.cpp
@@ -11,8 +11,39 @@ using ftl::rgbd::Channels;
using ftl::rgbd::Format;
using cv::cuda::GpuMat;
-Splatter::Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::render::Renderer(config), scene_(fs) {
+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;
+}
+Splatter::Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::render::Renderer(config), scene_(fs) {
+ 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_ = true;
+ } else {
+ clipping_ = false;
+ }
}
Splatter::~Splatter() {
@@ -52,6 +83,11 @@ void Splatter::renderChannel(
//LOG(INFO) << "POINTS Added";
}
+ // Clip first?
+ if (clipping_) {
+ ftl::cuda::clipping(f.createTexture<float4>(Channel::Points), clip_, stream);
+ }
+
ftl::cuda::dibr_merge(
f.createTexture<float4>(Channel::Points),
temp_.getTexture<int>(Channel::Depth),
--
GitLab