From 44f9b1f57045e906b10d9439761d8aa72e6bad64 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 6 Aug 2020 20:15:04 +0300
Subject: [PATCH] Add some cursor checks and a reset
---
applications/gui2/src/modules/camera.cpp | 47 +++++++++++++++++++++++-
applications/gui2/src/modules/camera.hpp | 1 +
2 files changed, 47 insertions(+), 1 deletion(-)
diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index 7049f0b44..f527a4cc1 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -428,7 +428,11 @@ bool Camera::hasFrame() {
}
Eigen::Matrix4d Camera::cursor() const {
- return nanogui::lookAt(cursor_pos_, cursor_target_, cursor_normal_).cast<double>();
+ if (cursor_normal_.norm() > 0.0f) return nanogui::lookAt(cursor_pos_, cursor_target_, cursor_normal_).cast<double>();
+
+ Eigen::Matrix4d ident;
+ ident.setIdentity();
+ return ident;
}
void Camera::drawOverlay(NVGcontext *ctx, const nanogui::Vector2f &s, const nanogui::Vector2f &is, const Eigen::Vector2f &offset) {
@@ -573,6 +577,14 @@ void Camera::setCursor(int x, int y) {
void Camera::setOriginToCursor() {
using ftl::calibration::transform::inverse;
+
+ // Check for valid cursor
+ if (cursor_normal_.norm() == 0.0f) return;
+ float cursor_length = (cursor_target_ - cursor_pos_).norm();
+ float cursor_dist = cursor_pos_.norm();
+ if (cursor_length < 0.01f || cursor_length > 5.0f) return;
+ if (cursor_dist > 10.0f) return;
+
if (movable_) {
auto *rend = io->feed()->getRenderer(frame_id_);
if (rend) {
@@ -596,4 +608,37 @@ void Camera::setOriginToCursor() {
}
}
}
+
+ cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+}
+
+void Camera::resetOrigin() {
+ cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+
+ if (movable_) {
+ auto *rend = io->feed()->getRenderer(frame_id_);
+ if (rend) {
+ auto *filter = rend->filter();
+ if (filter) {
+ cv::Mat cur;
+ cv::eigen2cv(cursor(), cur);
+ auto fss = filter->getLatestFrameSets();
+ for (auto &fs : fss) {
+ if (fs->frameset() == frame_id_.frameset()) continue;
+
+ for (auto &f : fs->frames) {
+ auto response = f.response();
+ auto &rgbdf = response.cast<ftl::rgbd::Frame>();
+ auto &calib = rgbdf.setCalibration();
+ calib = f.cast<ftl::rgbd::Frame>().getCalibration();
+ calib.origin = cur;
+ }
+ };
+ }
+ }
+ }
}
diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp
index ec1ae47dc..d314f0a7a 100644
--- a/applications/gui2/src/modules/camera.hpp
+++ b/applications/gui2/src/modules/camera.hpp
@@ -81,6 +81,7 @@ public:
const Eigen::Vector3f getCursorPosition() const { return cursor_pos_; }
void setOriginToCursor();
+ void resetOrigin();
private:
int frame_idx = -1;
--
GitLab