From 61efce0afca8494356eeea148460c475c79f4c75 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Tue, 18 Aug 2020 13:20:56 +0300
Subject: [PATCH] validate poses before saving
---
applications/gui2/src/modules/camera.cpp | 20 +++++++++++-----
.../streams/src/renderers/openvr_render.cpp | 23 +++++++++++++------
2 files changed, 30 insertions(+), 13 deletions(-)
diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index 72d91779c..368ead646 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -585,7 +585,7 @@ 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();
@@ -608,26 +608,34 @@ void Camera::setOriginToCursor() {
auto response = f.response();
auto &rgbdf = response.cast<ftl::rgbd::Frame>();
auto &calib = rgbdf.setCalibration();
+
calib = f.cast<ftl::rgbd::Frame>().getCalibration();
// apply correction to existing one
- calib.origin = cur*calib.origin;
+ cv::Mat new_origin = cur*calib.origin;
+ if (ftl::calibration::validate::pose(new_origin)) {
+ calib.origin = new_origin;
+ }
+ else {
+ // TODO: add error message to gui as well
+ LOG(ERROR) << "Bad origin update (invalid pose)";
+ }
}
};
}
}
}
- cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ 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);
+ cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
cursor_ = _cursor();
}
void Camera::resetOrigin() {
- cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ 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);
- cursor_ = _cursor();
+ cursor_ = _cursor();
if (movable_) {
auto *rend = io->feed()->getRenderer(frame_id_);
diff --git a/components/streams/src/renderers/openvr_render.cpp b/components/streams/src/renderers/openvr_render.cpp
index 481f0b065..b4a190c65 100644
--- a/components/streams/src/renderers/openvr_render.cpp
+++ b/components/streams/src/renderers/openvr_render.cpp
@@ -96,7 +96,7 @@ bool OpenVRRender::initVR() {
vr::EVRInitError eError = vr::VRInitError_None;
HMD = vr::VR_Init( &eError, vr::VRApplication_Scene );
-
+
if (eError != vr::VRInitError_None)
{
HMD = nullptr;
@@ -159,9 +159,9 @@ static Eigen::Matrix3d getCameraMatrix(const double tanx1,
const double tany2,
const double size_x,
const double size_y) {
-
+
Eigen::Matrix3d C = Eigen::Matrix3d::Identity();
-
+
CHECK(tanx1 < 0 && tanx2 > 0 && tany1 < 0 && tany2 > 0);
CHECK(size_x > 0 && size_y > 0);
@@ -209,7 +209,7 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
left.baseline = baseline_;
right.baseline = baseline_;
-
+
unsigned int size_x, size_y;
HMD->GetRecommendedRenderTargetSize(&size_x, &size_y);
left.width = size_x;
@@ -330,7 +330,16 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
if (headset_origin.size() > 0) {
ftl::operators::Poser::get(headset_origin, horigin);
}
- initial_pose_ = horigin*viewPose.inverse();
+ Eigen::Matrix4d new_pose = horigin*viewPose.inverse();
+
+ // validate new values before saving
+ const Eigen::Matrix3d rot(new_pose.block<3, 3>(0, 0));
+ if ((abs(rot.determinant() - 1.0) < 0.0001) && (new_pose(3, 3) == 1.0)) {
+ initial_pose_ = new_pose;
+ }
+ else {
+ LOG(ERROR) << "Bad pose update";
+ }
if (host_->value("reset_pose", false) && ftl::timer::get_time() < pose_calibration_start_ + host_->value("calibration_time",10000)) {
pose_calibrated_.clear();
@@ -351,7 +360,7 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
texture1_.make(width, height, ftl::utility::GLTexture::Type::BGRA);
texture2_.make(width, height, ftl::utility::GLTexture::Type::BGRA);
-
+
rgbdframe.create<cv::cuda::GpuMat>(Channel::Colour) = texture1_.map(renderer_->getCUDAStream());
rgbdframe.create<cv::cuda::GpuMat>(Channel::Colour2) = texture2_.map(renderer_->getCUDAStream());
rgbdframe.create<cv::cuda::GpuMat>(Channel::Depth).create(height, width, CV_32F);
@@ -481,7 +490,7 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
renderer_->render();
//renderer2_->render();
-
+
mixer_.mix();
// Write mixed audio to frame.
--
GitLab