From 5fad3d8680caa7ea5983670a36dc32df0b0172ef Mon Sep 17 00:00:00 2001 From: Yoyobuae Date: Sun, 26 Feb 2023 13:25:50 -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];