Merge branch 'validate-pose' into 'master'

validate poses (hmd/origin) before saving

See merge request nicolas.pope/ftl!339

applications/gui2/src/modules/camera.cpp

@@ -608,9 +608,17 @@ 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)";
+						}
 					}
 				};
 			}

components/streams/src/renderers/openvr_render.cpp

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