Merge branch 'master' into feature/133/ilw

applications/reconstruct/src/main.cpp

@@ -53,6 +53,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);
@@ -68,6 +78,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;
@@ -89,9 +117,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);
 		}
 	}
 

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

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

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

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() {
@@ -53,6 +84,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),