From a05c897263b37ef2bcae9a9b9b6993cfce74a5ab Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Sat, 29 Jun 2019 09:51:06 +0300
Subject: [PATCH] Initial registration indicator mode
---
.../reconstruct/include/ftl/voxel_scene.hpp | 1 +
applications/reconstruct/src/garbage.cu | 19 +++++
applications/reconstruct/src/garbage.hpp | 1 +
applications/reconstruct/src/integrators.cu | 80 +++++++++++++++++++
applications/reconstruct/src/integrators.hpp | 3 +
applications/reconstruct/src/voxel_scene.cpp | 11 ++-
6 files changed, 113 insertions(+), 2 deletions(-)
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..fc02c414f 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];
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..76f132c08 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) : 1.0f;
+
+ out.color.x = v1.color.x*redshift;
+ out.color.y = v1.color.y*redshift;
+ out.color.z = v1.color.z*(1.0f / redshift);
+
+ 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..b857c8efb 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;
@@ -200,7 +205,8 @@ void SceneRep::nextFrame() {
_destroy();
_create(_parametersFromConfig());
} else {
- ftl::cuda::starveVoxels(m_hashData, m_hashParams);
+ if (reg_mode_) ftl::cuda::clearVoxels(m_hashData, m_hashParams);
+ else ftl::cuda::starveVoxels(m_hashData, m_hashParams);
m_numIntegratedFrames = 0;
}
}
@@ -407,7 +413,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() {
--
GitLab