Unified tracker: bad handling of IMU data

[0x1100]

The unified tracker offer a better positional tracking than the old plugin but it suffers from a terrible amount of judder.

The pose extrapolation offered by Steam when using the SteamVR-OSVR driver makes things obviously better, yet it also shows without a doubt that the angular velocity given by the unified tracker is also buggy. We get a good extrapolation when we're looking forward with a neutral pose but a wrong one when facing another direction or when our head is tilted. Overriding the orientation and angular velocity from the plugin with the data coming from the IMU gives incredibly better results, although translations still aren't really smooth.

This proves there is something completely wrong with the way the plugin handles IMU data. The good news is this can be used as a workaround until it gets fixed.

diff --git a/plugins/unifiedvideoinertialtracker/org_osvr_unifiedvideoinertial.cpp b/plugins/unifiedvideoinertialtracker/org_osvr_unifiedvideoinertial.cpp
index d5cb931..1a8d086 100644
--- a/plugins/unifiedvideoinertialtracker/org_osvr_unifiedvideoinertial.cpp
+++ b/plugins/unifiedvideoinertialtracker/org_osvr_unifiedvideoinertial.cpp
@@ -27,6 +27,7 @@
 #include "ConfigurationParser.h"
 #include "HDKData.h"
 #include "MakeHDKTrackingSystem.h"
+#include "TrackedBodyIMU.h"
 #include "TrackerThread.h"

 // ImageSources mini-library
@@ -105,6 +106,14 @@ class UnifiedVideoInertialTracker : boost::noncopyable {
     TrackedBody *m_mainBody = nullptr;
     /// @todo kind-of assumes there's only one body.
     TrackedBodyIMU *m_imu = nullptr;
+    /// @todo kind-of assumes there's only one body.
+    osvr::vbtracker::BodyReport m_saved_report;
+    /// @todo kind-of assumes there's only one body.
+    bool m_new_imu_data;
+    /// @todo kind-of assumes there's only one body.
+    OSVR_OrientationReport m_imu_orientation;
+    /// @todo kind-of assumes there's only one body.
+    OSVR_AngularVelocityReport m_imu_angvelocity;
     const double m_additionalPrediction;
     const std::int32_t m_camUsecOffset = 0;
     const std::int32_t m_oriUsecOffset = 0;
@@ -200,6 +209,8 @@ class UnifiedVideoInertialTracker : boost::noncopyable {
             osvrTimeValueSum(&tv, &offset);
         }
         self.handleData(tv, *report);
+        self.m_imu_orientation = *report;
+        self.m_new_imu_data = true;
     }

     static void angVelCallback(void *userdata, const OSVR_TimeValue *timestamp,
@@ -212,6 +223,7 @@ class UnifiedVideoInertialTracker : boost::noncopyable {
             osvrTimeValueSum(&tv, &offset);
         }
         self.handleData(tv, *report);
+        self.m_imu_angvelocity = *report;
     }

     ~UnifiedVideoInertialTracker() { stopTrackerThread(); }
@@ -354,18 +366,40 @@ inline OSVR_ReturnCode UnifiedVideoInertialTracker::update() {
         osvr::vbtracker::BodyReport report;
         auto gotReport =
             m_bodyReportingVector[i]->getReport(m_additionalPrediction, report);
-        if (!gotReport) {
-            /// couldn't get a report for this sensor for one reason or another.
-            // std::cout << "Couldn't get report for " << i << std::endl;
+        // check that we have a valid report and fallback to the previous one
+        if (gotReport) {
+            if (m_imu && Eigen::Vector3d(report.pose.translation.data)
+                == m_imu->getBody().getSystem().getCameraPose().translation()) {
+                // When sending position too often, we sometimes get a
+                gotReport = false; // report with the camera position (sic)
+            } else {
+                m_saved_report = report;
+            }
+        }
+        report = m_saved_report;
+
+        // don't send twice the same data
+        if (!gotReport && !m_new_imu_data) {
             continue;
         }
+        m_new_imu_data = false;
+
+        const Eigen::Quaterniond yawCorrection = (m_imu != NULL)
+            ? m_imu->getIMUToRoom() : Eigen::Quaterniond::Identity();
+        // overwrite orientation
+        const auto imurot(osvr::util::fromQuat(m_imu_orientation.rotation));
+        const auto angvel(osvr::util::fromQuat(m_imu_angvelocity.state.incrementalRotation));
+        osvr::util::toQuat(yawCorrection*imurot, report.pose.rotation);
+        // overwrite angular velocity  // plgrot(osvr::util::fromQuat(report.pose.rotation));
+        osvr::util::toQuat( // plgrot * (imurot.inverse()*angvel*imurot) * plgrot.inverse(),
+            yawCorrection * angvel * yawCorrection.inverse(),
+            report.vel.angularVelocity.incrementalRotation);
+        report.vel.angularVelocity.dt = m_imu_angvelocity.state.dt;
+
         osvrDeviceTrackerSendPoseTimestamped(m_dev, m_tracker, &report.pose, i,
                                              &report.timestamp);
-        if (OSVR_TRUE == report.vel.angularVelocityValid ||
-            OSVR_TRUE == report.vel.linearVelocityValid) {
-            osvrDeviceTrackerSendVelocityTimestamped(
+        osvrDeviceTrackerSendVelocityTimestamped(
                 m_dev, m_tracker, &report.vel, i, &report.timestamp);
-        }
     }
     if (m_debugData) {
         osvr::vbtracker::DebugArray arr;

[JeroMiya]

Thanks for the diff, but could you provide this in the form of a pull request? It makes it easier to review and merge, and also the CI server will run a test build.

[0x1100]

This patch is a workaround, not a fix, it wasn't really meant to be merged. We get a much better result by only using the plugin's output position, sure, but it doesn't actually address any of the following issues.

The plugin updates the position at a rate that is noticeably too low. It should provide an extrapolated position for each IMU report to limit latency and provide a smooth experience. Right now it effectively throws away most of the IMU reports. It also outputs a bogus angular velocity which, in addition to the pose prediction, is probably used by the Kalman filter. And finally, the BodyReporting shouldn't ever give you a report with the camera position instead of an actual position estimation.