Allow clipping before align

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()));

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_;
 };
 
 }

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 &params, const float4x4 &poseinv, cudaStream_t stream);
 
 void world_to_cam(ftl::cuda::TextureObject<float4> &points, const float4x4 &poseinv, cudaStream_t stream);

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() );
+}