From 358c1a45eef1b206194cbaa0ed97953edbfcf6d8 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Thu, 5 Dec 2019 14:16:00 +0200
Subject: [PATCH] Implements #261 recon transforms
---
.../reconstruct/src/reconstruction.cpp | 32 +++++++++++++-
.../cpp/include/ftl/render/renderer.hpp | 2 +-
.../cpp/include/ftl/render/splat_render.hpp | 2 +-
.../cpp/include/ftl/render/tri_render.hpp | 12 ++---
components/renderers/cpp/src/splatter.cpp | 2 +-
components/renderers/cpp/src/tri_render.cpp | 44 +++++++++----------
6 files changed, 62 insertions(+), 32 deletions(-)
diff --git a/applications/reconstruct/src/reconstruction.cpp b/applications/reconstruct/src/reconstruction.cpp
index bbaf7f8b5..a2aa11093 100644
--- a/applications/reconstruct/src/reconstruction.cpp
+++ b/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
diff --git a/components/renderers/cpp/include/ftl/render/renderer.hpp b/components/renderers/cpp/include/ftl/render/renderer.hpp
index 432be6839..605fa27d1 100644
--- a/components/renderers/cpp/include/ftl/render/renderer.hpp
+++ b/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;
};
}
diff --git a/components/renderers/cpp/include/ftl/render/splat_render.hpp b/components/renderers/cpp/include/ftl/render/splat_render.hpp
index 3b36e8ec9..8e51aadf1 100644
--- a/components/renderers/cpp/include/ftl/render/splat_render.hpp
+++ b/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:
diff --git a/components/renderers/cpp/include/ftl/render/tri_render.hpp b/components/renderers/cpp/include/ftl/render/tri_render.hpp
index 3d9183643..dbe797797 100644
--- a/components/renderers/cpp/include/ftl/render/tri_render.hpp
+++ b/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);
};
}
diff --git a/components/renderers/cpp/src/splatter.cpp b/components/renderers/cpp/src/splatter.cpp
index 30c47dc11..fad38ba82 100644
--- a/components/renderers/cpp/src/splatter.cpp
+++ b/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;
diff --git a/components/renderers/cpp/src/tri_render.cpp b/components/renderers/cpp/src/tri_render.cpp
index 05edcb2ef..d1cf0c8a1 100644
--- a/components/renderers/cpp/src/tri_render.cpp
+++ b/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_));
--
GitLab