Allow transforms during render

applications/reconstruct/src/reconstruction.cpp

@@ -13,6 +13,16 @@
 using ftl::Reconstruction;
 using ftl::codecs::Channel;
 
+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;
+}
+
 Reconstruction::Reconstruction(nlohmann::json &config, const std::string name) :
 	ftl::Configurable(config), busy_(false), fs_render_(), fs_align_() {
 	group_ = new ftl::rgbd::Group;
@@ -73,5 +83,25 @@ void Reconstruction::addSource(ftl::rgbd::Source *src) {
 }
 
 void Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out) {
-	renderer_->render(vs, out);
+	// Create scene transform, intended for axis aligning the walls and floor
+	Eigen::Matrix4d transform;
+	//if (getConfig()["transform"].is_object()) {
+		//auto &c = getConfig()["transform"];
+		float rx = value("transform_pitch", 0.0f);
+		float ry = value("transform_yaw", 0.0f);
+		float rz = value("transform_roll", 0.0f);
+		float x = value("transform_x", 0.0f);
+		float y = value("transform_y", 0.0f);
+		float z = value("transform_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;
+	//} else {
+	//	transform.setIdentity();
+	//}
+
+	renderer_->render(vs, out, transform);
 }
\ No newline at end of file

components/renderers/cpp/include/ftl/render/renderer.hpp

@@ -26,7 +26,7 @@ class Renderer : public ftl::Configurable {
      * the virtual camera object passed, and writes the result into the
      * virtual camera.
      */
-    virtual bool render(ftl::rgbd::VirtualSource *, ftl::rgbd::Frame &)=0;
+    virtual bool render(ftl::rgbd::VirtualSource *, ftl::rgbd::Frame &, const Eigen::Matrix4d &)=0;
 };
 
 }

components/renderers/cpp/include/ftl/render/splat_render.hpp

@@ -22,7 +22,7 @@ class Splatter : public ftl::render::Renderer {
 	explicit Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs);
 	~Splatter();
 
-	bool render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) override;
+	bool render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, const Eigen::Matrix4d &t) override;
 	//void setOutputDevice(int);
 
 	protected:

components/renderers/cpp/include/ftl/render/tri_render.hpp

@@ -19,11 +19,11 @@ class Triangular : public ftl::render::Renderer {
 	explicit Triangular(nlohmann::json &config, ftl::rgbd::FrameSet *fs);
 	~Triangular();
 
-	bool render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) override;
+	bool render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, const Eigen::Matrix4d &t) override;
 	//void setOutputDevice(int);
 
 	protected:
-	void _renderChannel(ftl::rgbd::Frame &out, ftl::codecs::Channel channel_in, ftl::codecs::Channel channel_out, cudaStream_t stream);
+	void _renderChannel(ftl::rgbd::Frame &out, ftl::codecs::Channel channel_in, ftl::codecs::Channel channel_out, const Eigen::Matrix4d &t, cudaStream_t stream);
 
 	private:
 	int device_;
@@ -49,10 +49,10 @@ class Triangular : public ftl::render::Renderer {
 	//ftl::Filters *filters_;
 
 	template <typename T>
-	void __reprojectChannel(ftl::rgbd::Frame &, ftl::codecs::Channel in, ftl::codecs::Channel out, cudaStream_t);
-	void _reprojectChannel(ftl::rgbd::Frame &, ftl::codecs::Channel in, ftl::codecs::Channel out, cudaStream_t);
-	void _dibr(ftl::rgbd::Frame &, cudaStream_t);
-	void _mesh(ftl::rgbd::Frame &, ftl::rgbd::Source *, cudaStream_t);
+	void __reprojectChannel(ftl::rgbd::Frame &, ftl::codecs::Channel in, ftl::codecs::Channel out, const Eigen::Matrix4d &t, cudaStream_t);
+	void _reprojectChannel(ftl::rgbd::Frame &, ftl::codecs::Channel in, ftl::codecs::Channel out, const Eigen::Matrix4d &t, cudaStream_t);
+	void _dibr(ftl::rgbd::Frame &, const Eigen::Matrix4d &t, cudaStream_t);
+	void _mesh(ftl::rgbd::Frame &, ftl::rgbd::Source *, const Eigen::Matrix4d &t, cudaStream_t);
 };
 
 }

components/renderers/cpp/src/splatter.cpp

@@ -304,7 +304,7 @@ void Splatter::_renderChannel(
 	}
 }
 
-bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
+bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, const Eigen::Matrix4d &t) {
 	SHARED_LOCK(scene_->mtx, lk);
 	if (!src->isReady()) return false;
 

components/renderers/cpp/src/tri_render.cpp

@@ -206,7 +206,7 @@ void Triangular::__blendChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel i
 }*/
 
 template <typename T>
-void Triangular::__reprojectChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel in, ftl::codecs::Channel out, cudaStream_t stream) {
+void Triangular::__reprojectChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel in, ftl::codecs::Channel out, const Eigen::Matrix4d &t, cudaStream_t stream) {
 	cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
 	temp_.create<GpuMat>(
 		AccumSelector<T>::channel,
@@ -228,8 +228,8 @@ void Triangular::__reprojectChannel(ftl::rgbd::Frame &output, ftl::codecs::Chann
 			cv::cuda::cvtColor(tmp,col, cv::COLOR_BGR2BGRA);
 		}
 
-		auto transform = MatrixConversion::toCUDA(s->getPose().cast<float>().inverse()) * params_.m_viewMatrixInverse;
-		auto transformR = MatrixConversion::toCUDA(s->getPose().cast<float>().inverse()).getFloat3x3();
+		auto transform = MatrixConversion::toCUDA(t.cast<float>().inverse() * s->getPose().cast<float>().inverse()) * params_.m_viewMatrixInverse;
+		auto transformR = MatrixConversion::toCUDA(t.cast<float>().inverse() * s->getPose().cast<float>().inverse()).getFloat3x3();
 
 		if (mesh_) {
 			ftl::cuda::reproject(
@@ -277,18 +277,18 @@ void Triangular::__reprojectChannel(ftl::rgbd::Frame &output, ftl::codecs::Chann
 	}
 }*/
 
-void Triangular::_reprojectChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel in, ftl::codecs::Channel out, cudaStream_t stream) {
+void Triangular::_reprojectChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel in, ftl::codecs::Channel out, const Eigen::Matrix4d &t, cudaStream_t stream) {
 	int type = output.get<GpuMat>(out).type(); // == CV_32F; //ftl::rgbd::isFloatChannel(channel);
 	
 	switch (type) {
-	case CV_32F		: __reprojectChannel<float>(output, in, out, stream); break;
-	case CV_32FC4	: __reprojectChannel<float4>(output, in, out, stream); break;
-	case CV_8UC4	: __reprojectChannel<uchar4>(output, in, out, stream); break;
+	case CV_32F		: __reprojectChannel<float>(output, in, out, t, stream); break;
+	case CV_32FC4	: __reprojectChannel<float4>(output, in, out, t, stream); break;
+	case CV_8UC4	: __reprojectChannel<uchar4>(output, in, out, t, stream); break;
 	default			: LOG(ERROR) << "Invalid output channel format";
 	}
 }
 
-void Triangular::_dibr(ftl::rgbd::Frame &out, cudaStream_t stream) {
+void Triangular::_dibr(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStream_t stream) {
 	cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
 	temp_.get<GpuMat>(Channel::Depth2).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
 
@@ -301,7 +301,7 @@ void Triangular::_dibr(ftl::rgbd::Frame &out, cudaStream_t stream) {
 			continue;
 		}
 
-		auto transform = params_.m_viewMatrix * MatrixConversion::toCUDA(s->getPose().cast<float>());
+		auto transform = params_.m_viewMatrix * MatrixConversion::toCUDA(t.cast<float>() * s->getPose().cast<float>());
 
 		ftl::cuda::dibr_merge(
 			f.createTexture<float>(Channel::Depth),
@@ -316,7 +316,7 @@ void Triangular::_dibr(ftl::rgbd::Frame &out, cudaStream_t stream) {
 	temp_.get<GpuMat>(Channel::Depth2).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 100000.0f, cvstream);
 }
 
-void Triangular::_mesh(ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) {
+void Triangular::_mesh(ftl::rgbd::Frame &out, ftl::rgbd::Source *src, const Eigen::Matrix4d &t, cudaStream_t stream) {
 	cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
 
 	bool do_blend = value("mesh_blend", true);
@@ -338,7 +338,7 @@ void Triangular::_mesh(ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream
 			continue;
 		}
 
-		auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>());
+		auto pose = MatrixConversion::toCUDA(t.cast<float>() * s->getPose().cast<float>());
 
 		// Calculate and save virtual view screen position of each source pixel
 		ftl::cuda::screen_coord(
@@ -389,7 +389,7 @@ void Triangular::_mesh(ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream
 
 void Triangular::_renderChannel(
 		ftl::rgbd::Frame &out,
-		Channel channel_in, Channel channel_out, cudaStream_t stream)
+		Channel channel_in, Channel channel_out, const Eigen::Matrix4d &t, cudaStream_t stream)
 {
 	if (channel_out == Channel::None || channel_in == Channel::None) return;
 	cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
@@ -414,7 +414,7 @@ void Triangular::_renderChannel(
 		accum_.get<GpuMat>(channel_out).setTo(cv::Scalar(0,0,0,0), cvstream);
 	}
 
-	_reprojectChannel(out, channel_in, channel_out, stream);
+	_reprojectChannel(out, channel_in, channel_out, t, stream);
 }
 
 /*
@@ -464,7 +464,7 @@ static cv::Scalar HSVtoRGB(int H, double S, double V) {
 	return cv::Scalar((Bs + m) * 255, (Gs + m) * 255, (Rs + m) * 255, 0);
 }
 
-bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
+bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, const Eigen::Matrix4d &t) {
 	SHARED_LOCK(scene_->mtx, lk);
 	if (!src->isReady()) return false;
 
@@ -601,13 +601,13 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 
 	// Create and render triangles for depth
 	if (mesh_) {
-		_mesh(out, src, stream_);
+		_mesh(out, src, t, stream_);
 	} else {
-		_dibr(out, stream_);
+		_dibr(out, t, stream_);
 	}
 
 	// Reprojection of colours onto surface
-	_renderChannel(out, Channel::Colour, Channel::Colour, stream_);
+	_renderChannel(out, Channel::Colour, Channel::Colour, t, stream_);
 
 	if (value("cool_effect", false)) {
 		auto pose = params.m_viewMatrixInverse.getFloat3x3();
@@ -657,7 +657,7 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 	else if (chan == Channel::Density) {
 		out.create<GpuMat>(chan, Format<float>(camera.width, camera.height));
 		out.get<GpuMat>(chan).setTo(cv::Scalar(0.0f), cvstream);
-		_renderChannel(out, Channel::Depth, Channel::Density, stream_);
+		_renderChannel(out, Channel::Depth, Channel::Density, t, stream_);
 	}
 	else if (chan == Channel::Right)
 	{
@@ -682,11 +682,11 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 
 		// Need to re-dibr due to pose change
 		if (mesh_) {
-			_mesh(out, src, stream_);
+			_mesh(out, src, t, stream_);
 		} else {
-			_dibr(out, stream_);
+			_dibr(out, t, stream_);
 		}
-		_renderChannel(out, Channel::Left, Channel::Right, stream_);
+		_renderChannel(out, Channel::Left, Channel::Right, t, stream_);
 
 	} else if (chan != Channel::None) {
 		if (ftl::codecs::isFloatChannel(chan)) {
@@ -696,7 +696,7 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 			out.create<GpuMat>(chan, Format<uchar4>(camera.width, camera.height));
 			out.get<GpuMat>(chan).setTo(background_, cvstream);
 		}
-		_renderChannel(out, chan, chan, stream_);
+		_renderChannel(out, chan, chan, t, stream_);
 	}
 
 	cudaSafeCall(cudaStreamSynchronize(stream_));