From 79208e86cb21d47f1a7bc4d76485fa716f8125c1 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Mon, 30 Sep 2019 09:47:46 +0300
Subject: [PATCH] Implement option z restriction
---
applications/reconstruct/src/ilw/ilw.cpp | 14 +++++++++++---
applications/reconstruct/src/ilw/ilw.cu | 17 +++++++++++++----
applications/reconstruct/src/ilw/ilw_cuda.hpp | 2 ++
3 files changed, 26 insertions(+), 7 deletions(-)
diff --git a/applications/reconstruct/src/ilw/ilw.cpp b/applications/reconstruct/src/ilw/ilw.cpp
index 087aa100c..833f47ad3 100644
--- a/applications/reconstruct/src/ilw/ilw.cpp
+++ b/applications/reconstruct/src/ilw/ilw.cpp
@@ -22,11 +22,17 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
flags_ = 0;
if (value("ignore_bad", false)) flags_ |= ftl::cuda::kILWFlag_IgnoreBad;
+ if (value("restrict_z", true)) flags_ |= ftl::cuda::kILWFlag_RestrictZ;
on("ignore_bad", [this](const ftl::config::Event &e) {
if (value("ignore_bad", false)) flags_ |= ftl::cuda::kILWFlag_IgnoreBad;
else flags_ &= ~ftl::cuda::kILWFlag_IgnoreBad;
});
+
+ on("restrict_z", [this](const ftl::config::Event &e) {
+ if (value("restrict_z", false)) flags_ |= ftl::cuda::kILWFlag_RestrictZ;
+ else flags_ &= ~ftl::cuda::kILWFlag_RestrictZ;
+ });
}
ILW::~ILW() {
@@ -105,7 +111,7 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
auto &f1 = fs.frames[i];
auto &f2 = fs.frames[j];
- //auto s1 = fs.frames[i];
+ auto s1 = fs.sources[i];
auto s2 = fs.sources[j];
// Are cameras facing similar enough direction?
@@ -114,7 +120,8 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
// No, so skip this combination
if (d1.dot(d2) <= 0.0) continue;
- auto pose = MatrixConversion::toCUDA(s2->getPose().cast<float>().inverse());
+ auto pose1 = MatrixConversion::toCUDA(s1->getPose().cast<float>().inverse());
+ auto pose2 = MatrixConversion::toCUDA(s2->getPose().cast<float>().inverse());
try {
//Calculate energy vector to best correspondence
@@ -126,7 +133,8 @@ bool ILW::_phase1(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
// TODO: Add normals and other things...
f1.getTexture<float4>(Channel::EnergyVector),
f1.getTexture<float>(Channel::Energy),
- pose,
+ pose1,
+ pose2,
s2->parameters(),
flags_,
stream
diff --git a/applications/reconstruct/src/ilw/ilw.cu b/applications/reconstruct/src/ilw/ilw.cu
index bb300d327..1c495f3e0 100644
--- a/applications/reconstruct/src/ilw/ilw.cu
+++ b/applications/reconstruct/src/ilw/ilw.cu
@@ -34,6 +34,7 @@ __global__ void correspondence_energy_vector_kernel(
TextureObject<uchar4> c2,
TextureObject<float4> vout,
TextureObject<float> eout,
+ float4x4 pose1, // Inverse
float4x4 pose2, // Inverse
Camera cam2, uint flags) {
@@ -88,10 +89,17 @@ __global__ void correspondence_energy_vector_kernel(
const float confidence = (avgcost - mincost);
if (best && mincost < 1.0f) {
+ float3 tvecA = pose1 * bestpoint;
+ float3 tvecB = pose1 * world1;
+ if (flags & ftl::cuda::kILWFlag_RestrictZ) {
+ tvecA.x = tvecB.x;
+ tvecA.y = tvecB.y;
+ }
+ tvecA = (pose1.getInverse() * tvecA) - world1;
vout(x,y) = vout.tex2D(x, y) + make_float4(
- (bestpoint.x - world1.x),
- (bestpoint.y - world1.y),
- (bestpoint.z - world1.z),
+ tvecA.x, // * (1.0f - mincost) * confidence,
+ tvecA.y, // * (1.0f - mincost) * confidence,
+ tvecA.z, // * (1.0f - mincost) * confidence,
(1.0f - mincost) * confidence);
//eout(x,y) = max(eout(x,y), (length(bestpoint-world1) / 0.04f) * 7.0f);
@@ -112,6 +120,7 @@ void ftl::cuda::correspondence_energy_vector(
TextureObject<uchar4> &c2,
TextureObject<float4> &vout,
TextureObject<float> &eout,
+ float4x4 &pose1,
float4x4 &pose2,
const Camera &cam2, uint flags,
cudaStream_t stream) {
@@ -122,7 +131,7 @@ void ftl::cuda::correspondence_energy_vector(
//printf("COR SIZE %d,%d\n", p1.width(), p1.height());
correspondence_energy_vector_kernel<<<gridSize, blockSize, 0, stream>>>(
- p1, p2, c1, c2, vout, eout, pose2, cam2, flags
+ p1, p2, c1, c2, vout, eout, pose1, pose2, cam2, flags
);
cudaSafeCall( cudaGetLastError() );
}
diff --git a/applications/reconstruct/src/ilw/ilw_cuda.hpp b/applications/reconstruct/src/ilw/ilw_cuda.hpp
index b979ff7c0..5e3996062 100644
--- a/applications/reconstruct/src/ilw/ilw_cuda.hpp
+++ b/applications/reconstruct/src/ilw/ilw_cuda.hpp
@@ -9,6 +9,7 @@ namespace ftl {
namespace cuda {
static const uint kILWFlag_IgnoreBad = 0x0001;
+static const uint kILWFlag_RestrictZ = 0x0002;
void correspondence_energy_vector(
ftl::cuda::TextureObject<float4> &p1,
@@ -17,6 +18,7 @@ void correspondence_energy_vector(
ftl::cuda::TextureObject<uchar4> &c2,
ftl::cuda::TextureObject<float4> &vout,
ftl::cuda::TextureObject<float> &eout,
+ float4x4 &pose1,
float4x4 &pose2,
const ftl::rgbd::Camera &cam2,
uint flags,
--
GitLab