Initial registration indicator mode

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

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];

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);

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

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);
+
 }
 }
 

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() {