From 8a604901a2eb0f7bba4895245fc8820208a7b93b Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Sat, 29 Jun 2019 13:44:25 +0300
Subject: [PATCH] Resolves #40 by colouring registration errors
---
applications/gui/src/screen.cpp | 65 +++++++++++----
.../reconstruct/include/ftl/voxel_scene.hpp | 1 +
applications/reconstruct/src/garbage.cu | 21 ++++-
applications/reconstruct/src/garbage.hpp | 1 +
applications/reconstruct/src/integrators.cu | 80 +++++++++++++++++++
applications/reconstruct/src/integrators.hpp | 3 +
applications/reconstruct/src/voxel_scene.cpp | 26 ++++--
.../rgbd-sources/src/snapshot_source.cpp | 19 +++++
8 files changed, 193 insertions(+), 23 deletions(-)
diff --git a/applications/gui/src/screen.cpp b/applications/gui/src/screen.cpp
index 63965a3fc..00d46ee58 100644
--- a/applications/gui/src/screen.cpp
+++ b/applications/gui/src/screen.cpp
@@ -9,6 +9,7 @@
#include <nanogui/combobox.h>
#include <nanogui/label.h>
#include <nanogui/toolbutton.h>
+#include <nanogui/popupbutton.h>
#include <opencv2/opencv.hpp>
@@ -121,6 +122,7 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
button->setFixedSize(Vector2i(40,40));
button->setCallback([this]() {
//swindow_->setVisible(true);
+ setActiveCamera(nullptr);
});
/*button = new ToolButton(innertool, ENTYPO_ICON_PLUS);
@@ -132,22 +134,54 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
//swindow_->setVisible(true);
});*/
- button = new ToolButton(innertool, ENTYPO_ICON_PLUS);
- button->setIconExtraScale(1.5f);
- button->setTheme(toolbuttheme);
- button->setTooltip("Camera Sources");
- button->setFixedSize(Vector2i(40,40));
- button->setCallback([this]() {
+ auto popbutton = new PopupButton(innertool, "", ENTYPO_ICON_PLUS);
+ popbutton->setIconExtraScale(1.5f);
+ popbutton->setTheme(toolbuttheme);
+ popbutton->setTooltip("Add");
+ popbutton->setFixedSize(Vector2i(40,40));
+ popbutton->setSide(Popup::Side::Right);
+ popbutton->setChevronIcon(0);
+ Popup *popup = popbutton->popup();
+ popup->setLayout(new GroupLayout());
+ popup->setTheme(toolbuttheme);
+ //popup->setAnchorHeight(100);
+
+ auto itembutton = new Button(popup, "Add Camera", ENTYPO_ICON_CAMERA);
+ itembutton->setCallback([this,popup]() {
swindow_->setVisible(true);
+ popup->setVisible(false);
});
- button = new ToolButton(innertool, ENTYPO_ICON_TOOLS);
- button->setIconExtraScale(1.5f);
- button->setTheme(toolbuttheme);
- button->setTooltip("Connections");
- button->setFixedSize(Vector2i(40,40));
- button->setCallback([this]() {
+ itembutton = new Button(popup, "Add Node", ENTYPO_ICON_LAPTOP);
+ itembutton->setCallback([this,popup]() {
+ cwindow_->setVisible(true);
+ popup->setVisible(false);
+ });
+
+ popbutton = new PopupButton(innertool, "", ENTYPO_ICON_TOOLS);
+ popbutton->setIconExtraScale(1.5f);
+ popbutton->setTheme(toolbuttheme);
+ popbutton->setTooltip("Tools");
+ popbutton->setFixedSize(Vector2i(40,40));
+ popbutton->setSide(Popup::Side::Right);
+ popbutton->setChevronIcon(0);
+ popup = popbutton->popup();
+ popup->setLayout(new GroupLayout());
+ popup->setTheme(toolbuttheme);
+ //popbutton->setCallback([this]() {
+ // cwindow_->setVisible(true);
+ //});
+
+ itembutton = new Button(popup, "Connections");
+ itembutton->setCallback([this,popup]() {
cwindow_->setVisible(true);
+ popup->setVisible(false);
+ });
+
+ itembutton = new Button(popup, "Manual Registration");
+ itembutton->setCallback([this,popup]() {
+ // Show pose win...
+ popup->setVisible(false);
});
button = new ToolButton(toolbar, ENTYPO_ICON_COG);
@@ -164,9 +198,10 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
mwindow_->setVisible(false);
cwindow_->setPosition(Eigen::Vector2i(80, 20));
- swindow_->setPosition(Eigen::Vector2i(80, 400));
+ //swindow_->setPosition(Eigen::Vector2i(80, 400));
cwindow_->setVisible(false);
- swindow_->setVisible(false);
+ swindow_->setVisible(true);
+ swindow_->center();
cwindow_->setTheme(windowtheme);
swindow_->setTheme(windowtheme);
@@ -201,8 +236,10 @@ void ftl::gui::Screen::setActiveCamera(ftl::gui::Camera *cam) {
if (cam) {
status_ = cam->source()->getURI();
mwindow_->setVisible(true);
+ swindow_->setVisible(false);
} else {
mwindow_->setVisible(false);
+ swindow_->setVisible(true);
status_ = "No camera...";
}
}
diff --git a/applications/reconstruct/include/ftl/voxel_scene.hpp b/applications/reconstruct/include/ftl/voxel_scene.hpp
index 44cdf9750..1babf0145 100644
--- a/applications/reconstruct/include/ftl/voxel_scene.hpp
+++ b/applications/reconstruct/include/ftl/voxel_scene.hpp
@@ -101,6 +101,7 @@ class SceneRep : public ftl::Configurable {
bool do_reset_;
std::vector<Cameras> cameras_;
cudaStream_t integ_stream_;
+ bool reg_mode_;
};
}; // namespace voxhash
diff --git a/applications/reconstruct/src/garbage.cu b/applications/reconstruct/src/garbage.cu
index f0ba686e4..beb7027b9 100644
--- a/applications/reconstruct/src/garbage.cu
+++ b/applications/reconstruct/src/garbage.cu
@@ -35,6 +35,25 @@ void ftl::cuda::starveVoxels(HashData& hashData, const HashParams& hashParams) {
#endif
}
+__global__ void clearVoxelsKernel(HashData hashData) {
+
+ // Stride over all allocated blocks
+ for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
+
+ const HashEntry& entry = hashData.d_hashCompactified[bi];
+ hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight = 0;
+
+ }
+}
+
+void ftl::cuda::clearVoxels(HashData& hashData, const HashParams& hashParams) {
+ const unsigned int threadsPerBlock = SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE;
+ const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
+ const dim3 blockSize(threadsPerBlock, 1);
+
+ clearVoxelsKernel << <gridSize, blockSize >> >(hashData);
+}
+
__shared__ float shared_MinSDF[SDF_BLOCK_SIZE * SDF_BLOCK_SIZE * SDF_BLOCK_SIZE / 2];
__shared__ uint shared_MaxWeight[SDF_BLOCK_SIZE * SDF_BLOCK_SIZE * SDF_BLOCK_SIZE / 2];
@@ -118,7 +137,7 @@ void ftl::cuda::garbageCollectIdentify(HashData& hashData, const HashParams& has
__global__ void garbageCollectFreeKernel(HashData hashData) {
// Stride over all allocated blocks
- for (int bi=blockIdx.x*blockDim.x + threadIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
+ for (int bi=blockIdx.x*blockDim.x + threadIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS*blockDim.x) {
if (hashData.d_hashDecision[bi] != 0) { //decision to delete the hash entry
diff --git a/applications/reconstruct/src/garbage.hpp b/applications/reconstruct/src/garbage.hpp
index a7bdbc245..477db1ae8 100644
--- a/applications/reconstruct/src/garbage.hpp
+++ b/applications/reconstruct/src/garbage.hpp
@@ -4,6 +4,7 @@
namespace ftl {
namespace cuda {
+void clearVoxels(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
void starveVoxels(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams);
void garbageCollectIdentify(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t stream);
void garbageCollectFree(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, cudaStream_t stream);
diff --git a/applications/reconstruct/src/integrators.cu b/applications/reconstruct/src/integrators.cu
index 07f0834d4..d34c02125 100644
--- a/applications/reconstruct/src/integrators.cu
+++ b/applications/reconstruct/src/integrators.cu
@@ -189,3 +189,83 @@ void ftl::cuda::integrateDepthMap(HashData& hashData, const HashParams& hashPara
//cutilCheckMsg(__FUNCTION__);
#endif
}
+
+
+__global__ void integrateRegistrationKernel(HashData hashData, HashParams hashParams, DepthCameraParams cameraParams, cudaTextureObject_t depthT, cudaTextureObject_t colourT) {
+
+ // Stride over all allocated blocks
+ for (int bi=blockIdx.x; bi<*hashData.d_hashCompactifiedCounter; bi+=NUM_CUDA_BLOCKS) {
+
+ //TODO check if we should load this in shared memory
+ HashEntry& entry = hashData.d_hashCompactified[bi];
+
+
+ int3 pi_base = hashData.SDFBlockToVirtualVoxelPos(entry.pos);
+
+ uint i = threadIdx.x; //inside of an SDF block
+ int3 pi = pi_base + make_int3(hashData.delinearizeVoxelIndex(i));
+ float3 pf = hashData.virtualVoxelPosToWorld(pi);
+
+ pf = hashParams.m_rigidTransformInverse * pf;
+ uint2 screenPos = make_uint2(cameraParams.cameraToKinectScreenInt(pf));
+
+ // For this voxel in hash, get its screen position and check it is on screen
+ if (screenPos.x < cameraParams.m_imageWidth && screenPos.y < cameraParams.m_imageHeight) { //on screen
+
+ //float depth = g_InputDepth[screenPos];
+ float depth = tex2D<float>(depthT, screenPos.x, screenPos.y);
+ //if (depth > 20.0f) return;
+
+ uchar4 color = make_uchar4(0, 0, 0, 0);
+ color = tex2D<uchar4>(colourT, screenPos.x, screenPos.y);
+
+ // Depth is within accepted max distance from camera
+ if (depth > 0.01f && depth < hashParams.m_maxIntegrationDistance) { // valid depth and color (Nick: removed colour check)
+ float depthZeroOne = cameraParams.cameraToKinectProjZ(depth);
+
+ // Calculate SDF of this voxel wrt the depth map value
+ float sdf = depth - pf.z;
+ float truncation = hashData.getTruncation(depth);
+
+ if (sdf > -truncation) {
+ float weightUpdate = max(hashParams.m_integrationWeightSample * 1.5f * (1.0f-depthZeroOne), 1.0f);
+
+ Voxel curr; //construct current voxel
+ curr.sdf = sdf;
+ curr.weight = weightUpdate;
+ curr.color = make_uchar3(color.x, color.y, color.z);
+
+ uint idx = entry.ptr + i;
+
+ Voxel out;
+ const Voxel &v1 = curr;
+ const Voxel &v0 = hashData.d_SDFBlocks[idx];
+
+ float redshift = (v0.weight > 0) ? 1.0f - ((v1.sdf - v0.sdf) / hashParams.m_truncation)*0.5f : 1.0f;
+
+ out.color.x = min(max(v1.color.x*redshift,0.0f),255.0f);
+ out.color.y = min(max(v1.color.y*redshift,0.0f),255.0f);
+ out.color.z = min(max(v1.color.z*(1.0f / redshift),0.0f),255.0f);
+
+ out.sdf = (v0.sdf * (float)v0.weight + v1.sdf * (float)v1.weight) / ((float)v0.weight + (float)v1.weight);
+ out.weight = min(c_hashParams.m_integrationWeightMax, (unsigned int)v0.weight + (unsigned int)v1.weight);
+
+ hashData.d_SDFBlocks[idx] = out;
+
+ }
+ }
+ }
+
+ } // Stride loop
+}
+
+
+void ftl::cuda::integrateRegistration(HashData& hashData, const HashParams& hashParams,
+ const DepthCameraData& depthCameraData, const DepthCameraParams& depthCameraParams, cudaStream_t stream) {
+ const unsigned int threadsPerBlock = SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE;
+ const dim3 gridSize(NUM_CUDA_BLOCKS, 1);
+ const dim3 blockSize(threadsPerBlock, 1);
+
+ integrateRegistrationKernel << <gridSize, blockSize, 0, stream >> >(hashData, hashParams, depthCameraParams, depthCameraData.depth_obj_, depthCameraData.colour_obj_);
+
+}
\ No newline at end of file
diff --git a/applications/reconstruct/src/integrators.hpp b/applications/reconstruct/src/integrators.hpp
index df9f425c9..756ff2ea4 100644
--- a/applications/reconstruct/src/integrators.hpp
+++ b/applications/reconstruct/src/integrators.hpp
@@ -10,6 +10,9 @@ namespace cuda {
void integrateDepthMap(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams,
const DepthCameraData& depthCameraData, const DepthCameraParams& depthCameraParams, cudaStream_t stream);
+void integrateRegistration(ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams,
+ const DepthCameraData& depthCameraData, const DepthCameraParams& depthCameraParams, cudaStream_t stream);
+
}
}
diff --git a/applications/reconstruct/src/voxel_scene.cpp b/applications/reconstruct/src/voxel_scene.cpp
index eb4628858..216325ded 100644
--- a/applications/reconstruct/src/voxel_scene.cpp
+++ b/applications/reconstruct/src/voxel_scene.cpp
@@ -47,6 +47,11 @@ SceneRep::SceneRep(nlohmann::json &config) : Configurable(config), do_reset_(fal
on("SDFMaxIntegrationDistance", [this](const ftl::config::Event &e) {
m_hashParams.m_maxIntegrationDistance = value("SDFMaxIntegrationDistance", 10.0f);
});
+ on("showRegistration", [this](const ftl::config::Event &e) {
+ reg_mode_ = value("showRegistration", false);
+ });
+
+ reg_mode_ = value("showRegistration", false);
cudaSafeCall(cudaStreamCreate(&integ_stream_));
//integ_stream_ = 0;
@@ -79,12 +84,14 @@ int SceneRep::upload() {
continue;
} else {
auto in = cam.source;
+
+ cam.params.fx = in->parameters().fx;
+ cam.params.fy = in->parameters().fy;
+ cam.params.mx = -in->parameters().cx;
+ cam.params.my = -in->parameters().cy;
+
// Only now do we have camera parameters for allocations...
if (cam.params.m_imageWidth == 0) {
- cam.params.fx = in->parameters().fx;
- cam.params.fy = in->parameters().fy;
- cam.params.mx = -in->parameters().cx;
- cam.params.my = -in->parameters().cy;
cam.params.m_imageWidth = in->parameters().width;
cam.params.m_imageHeight = in->parameters().height;
cam.params.m_sensorDepthWorldMax = in->parameters().maxDepth;
@@ -144,7 +151,7 @@ void SceneRep::integrate() {
//volumetrically integrate the depth data into the depth SDFBlocks
_integrateDepthMap(cam.gpu, cam.params);
- //_garbageCollect(cam.gpu);
+ //_garbageCollect();
m_numIntegratedFrames++;
}
@@ -180,7 +187,7 @@ void SceneRep::garbage() {
void SceneRep::setLastRigidTransform(const Eigen::Matrix4f& lastRigidTransform) {
m_hashParams.m_rigidTransform = MatrixConversion::toCUDA(lastRigidTransform);
- m_hashParams.m_rigidTransformInverse = m_hashParams.m_rigidTransform.getInverse();
+ m_hashParams.m_rigidTransformInverse = MatrixConversion::toCUDA(lastRigidTransform.inverse()); //m_hashParams.m_rigidTransform.getInverse();
}
/*void SceneRep::setLastRigidTransformAndCompactify(const Eigen::Matrix4f& lastRigidTransform, const DepthCameraData& depthCameraData) {
@@ -200,7 +207,9 @@ void SceneRep::nextFrame() {
_destroy();
_create(_parametersFromConfig());
} else {
- ftl::cuda::starveVoxels(m_hashData, m_hashParams);
+ //ftl::cuda::compactifyAllocated(m_hashData, m_hashParams, integ_stream_);
+ if (reg_mode_) ftl::cuda::clearVoxels(m_hashData, m_hashParams);
+ else ftl::cuda::starveVoxels(m_hashData, m_hashParams);
m_numIntegratedFrames = 0;
}
}
@@ -407,7 +416,8 @@ void SceneRep::_compactifyAllocated() {
}
void SceneRep::_integrateDepthMap(const DepthCameraData& depthCameraData, const DepthCameraParams& depthCameraParams) {
- ftl::cuda::integrateDepthMap(m_hashData, m_hashParams, depthCameraData, depthCameraParams, integ_stream_);
+ if (!reg_mode_) ftl::cuda::integrateDepthMap(m_hashData, m_hashParams, depthCameraData, depthCameraParams, integ_stream_);
+ else ftl::cuda::integrateRegistration(m_hashData, m_hashParams, depthCameraData, depthCameraParams, integ_stream_);
}
void SceneRep::_garbageCollect() {
diff --git a/components/rgbd-sources/src/snapshot_source.cpp b/components/rgbd-sources/src/snapshot_source.cpp
index 0e8dec08b..d3e70524e 100644
--- a/components/rgbd-sources/src/snapshot_source.cpp
+++ b/components/rgbd-sources/src/snapshot_source.cpp
@@ -23,5 +23,24 @@ SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader,
ftl::rgbd::colourCorrection(rgb_, host->value("gamma", 1.0f), host->value("temperature", 6500));
});
+ // Add calibration to config object
+ host_->getConfig()["focal"] = params_.fx;
+ host_->getConfig()["centre_x"] = params_.cx;
+ host_->getConfig()["centre_y"] = params_.cy;
+ host_->getConfig()["baseline"] = params_.baseline;
+
+ host_->on("focal", [this](const ftl::config::Event &e) {
+ params_.fx = host_->value("focal", params_.fx);
+ params_.fy = params_.fx;
+ });
+
+ host_->on("centre_x", [this](const ftl::config::Event &e) {
+ params_.cx = host_->value("centre_x", params_.cx);
+ });
+
+ host_->on("centre_y", [this](const ftl::config::Event &e) {
+ params_.cy = host_->value("centre_y", params_.cy);
+ });
+
setPose(pose);
}
--
GitLab