Merge branch 'feature/40/guireg' into 'master'

Resolves #40 by colouring registration errors

Closes #40

See merge request nicolas.pope/ftl!57

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...";
 	}
 }

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

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

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

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