From ed36edb7e803472ee83e5d7d581879108dae0100 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Wed, 12 Jun 2019 21:44:40 +0300
Subject: [PATCH] Fixes #67 gui click exception

---
 applications/gui/src/main.cpp          | 22 ++++++++++++++++++++--
 components/rgbd-sources/src/source.cpp |  4 ++--
 2 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp
index be2ac713a..3df95ca79 100644
--- a/applications/gui/src/main.cpp
+++ b/applications/gui/src/main.cpp
@@ -161,7 +161,24 @@ class FTLApplication : public nanogui::Screen {
 		} else {
 			auto src_ = swindow_->getSource();
 			if (src_ && src_->isReady() && down) {
-				Eigen::Vector4f camPos = src_->point(p[0],p[1]);
+				Eigen::Vector2f screenSize = size().cast<float>();
+				auto mScale = (screenSize.cwiseQuotient(imageSize).minCoeff());
+				Eigen::Vector2f scaleFactor = mScale * imageSize.cwiseQuotient(screenSize);
+				Eigen::Vector2f positionInScreen(0.0f, 0.0f);
+				auto mOffset = (screenSize - (screenSize.cwiseProduct(scaleFactor))) / 2;
+				Eigen::Vector2f positionAfterOffset = positionInScreen + mOffset;
+
+				float sx = ((float)p[0] - positionAfterOffset[0]) / mScale;
+				float sy = ((float)p[1] - positionAfterOffset[1]) / mScale;
+
+				Eigen::Vector4f camPos;
+
+				try {
+					camPos = src_->point(sx,sy);
+				} catch(...) {
+					return true;
+				}
+				
 				camPos *= -1.0f;
 				Eigen::Vector4f worldPos =  src_->getPose() * camPos;
 				lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
@@ -195,7 +212,7 @@ class FTLApplication : public nanogui::Screen {
 		using namespace Eigen;
 
 		auto src_ = swindow_->getSource();
-		Vector2f imageSize(0, 0);
+		imageSize = {0, 0};
 
 		float now = (float)glfwGetTime();
 		float delta = now - ftime_;
@@ -273,6 +290,7 @@ class FTLApplication : public nanogui::Screen {
 	float lerpSpeed_;
 	bool depth_;
 	float ftime_;
+	Eigen::Vector2f imageSize;
 };
 
 int main(int argc, char **argv) {
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index bd68b0ae8..2f51b6fdc 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -128,8 +128,8 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
 
 Eigen::Vector4f Source::point(uint ux, uint uy) {
 	const auto &params = parameters();
-	const float x = ((float)ux-params.width/2) / (float)params.fx;
-	const float y = ((float)uy-params.height/2) / (float)params.fy;
+	const float x = ((float)ux-(float)params.cx) / (float)params.fx;
+	const float y = ((float)uy-(float)params.cy) / (float)params.fy;
 
 	shared_lock<shared_mutex> lk(mutex_);
 	const float depth = depth_.at<float>(uy,ux);
-- 
GitLab