From 693c0a1af19e8d09f074d33241b3638ca53b86da Mon Sep 17 00:00:00 2001 From: Gerald Young Date: Sun, 26 Feb 2023 13:15:25 -0500 Subject: [PATCH] Normalize quats in predicted poses The predicted poses obtained from the linear regression model will not necessarily return normalized quaternions --- driver_files/src/Driver/TrackerDevice.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/driver_files/src/Driver/TrackerDevice.cpp b/driver_files/src/Driver/TrackerDevice.cpp index 1545c441..a04ed795 100644 --- a/driver_files/src/Driver/TrackerDevice.cpp +++ b/driver_files/src/Driver/TrackerDevice.cpp @@ -1,5 +1,7 @@ #include "TrackerDevice.hpp" +#define EPSILON 1.e-6 + void normalizeQuat(double pose[]) { @@ -9,10 +11,13 @@ void normalizeQuat(double pose[]) pose[5] * pose[5] + pose[6] * pose[6]); - pose[3] /= mag; - pose[4] /= mag; - pose[5] /= mag; - pose[6] /= mag; + if (mag > EPSILON) + { + pose[3] /= mag; + pose[4] /= mag; + pose[5] /= mag; + pose[6] /= mag; + } } ExampleDriver::TrackerDevice::TrackerDevice(std::string serial, std::string role): @@ -267,6 +272,7 @@ int ExampleDriver::TrackerDevice::get_next_pose(double time_offset, double pred[ } + normalizeQuat(pred); //printf("::: %f\n", pred[0]); return statuscode; //return pred[0], pred[1], pred[2], pred[3], pred[4], pred[5], pred[6];