Merge "Fix pose predictor jank." into oc-dev

7 files changed, 84 insertions, 8 deletions

diff --git a/libs/vr/libposepredictor/include/polynomial_predictor.h b/libs/vr/libposepredictor/include/polynomial_predictor.h
index 762afd3b90..4b8d51bcb2 100644
--- a/libs/vr/libposepredictor/include/polynomial_predictor.h
+++ b/libs/vr/libposepredictor/include/polynomial_predictor.h
@@ -19,7 +19,7 @@ class PolynomialPosePredictor : public BufferedPredictor {
  public:
   PolynomialPosePredictor(real regularization = 1e-9)
       : BufferedPredictor(TrainingWindow), regularization_(regularization) {
-    static_assert(PolynomialDegree + 1 >= TrainingWindow,
+    static_assert(TrainingWindow >= PolynomialDegree + 1,
                   "Underconstrained polynomial regressor");
   }
 
diff --git a/libs/vr/libposepredictor/predictor.cpp b/libs/vr/libposepredictor/predictor.cpp
index 4d2eafdb2e..beba156378 100644
--- a/libs/vr/libposepredictor/predictor.cpp
+++ b/libs/vr/libposepredictor/predictor.cpp
@@ -5,7 +5,7 @@
 namespace posepredictor {
 
 vec3 Predictor::AngularVelocity(const quat& a, const quat& b, real delta_time) {
-  const auto delta_q = b.inverse() * a;
+  const auto delta_q = a.inverse() * b;
   // Check that delta_q.w() == 1, Eigen doesn't respect this convention. If
   // delta_q.w() == -1, we'll get the opposite velocity.
   return 2.0 * (delta_q.w() < 0 ? static_cast<vec3>(-delta_q.vec()) : delta_q.vec()) / delta_time;
diff --git a/libs/vr/libvrsensor/Android.bp b/libs/vr/libvrsensor/Android.bp
index 6d48f18385..d59182efe7 100644
--- a/libs/vr/libvrsensor/Android.bp
+++ b/libs/vr/libvrsensor/Android.bp
@@ -15,6 +15,7 @@
 sourceFiles = [
     "pose_client.cpp",
     "sensor_client.cpp",
+    "latency_model.cpp",
 ]
 
 includeFiles = [
diff --git a/libs/vr/libvrsensor/include/private/dvr/latency_model.h b/libs/vr/libvrsensor/include/private/dvr/latency_model.h
new file mode 100644
index 0000000000..1bb3c4fdd8
--- /dev/null
+++ b/libs/vr/libvrsensor/include/private/dvr/latency_model.h
@@ -0,0 +1,31 @@
+#ifndef ANDROID_DVR_LATENCY_MODEL_H_
+#define ANDROID_DVR_LATENCY_MODEL_H_
+
+#include <vector>
+
+namespace android {
+namespace dvr {
+
+// This class holds a rolling average of the sensor latency.
+class LatencyModel {
+ public:
+  LatencyModel(size_t window_size, double weight_mass_in_window);
+  ~LatencyModel() = default;
+
+  void AddLatency(int64_t latency_ns);
+  int64_t CurrentLatencyEstimate() const {
+    return static_cast<int64_t>(rolling_average_);
+  }
+
+ private:
+  // The rolling average of the latencies.
+  double rolling_average_ = 0;
+
+  // The alpha parameter for an exponential moving average.
+  double alpha_;
+};
+
+}  // namespace dvr
+}  // namespace android
+
+#endif  // ANDROID_DVR_LATENCY_MODEL_H_
diff --git a/libs/vr/libvrsensor/latency_model.cpp b/libs/vr/libvrsensor/latency_model.cpp
new file mode 100644
index 0000000000..8233889383
--- /dev/null
+++ b/libs/vr/libvrsensor/latency_model.cpp
@@ -0,0 +1,33 @@
+#include <private/dvr/latency_model.h>
+
+#include <cmath>
+
+namespace android {
+namespace dvr {
+
+LatencyModel::LatencyModel(size_t window_size, double weight_mass_in_window) {
+  // Compute an alpha so the weight of the last window_size measurements is
+  // weight_mass_in_window of the total weights.
+
+  // The weight in a series of k measurements:
+  // alpha + (1 + (1 - alpha) + (1 - alpha)^2 + ... (1 - alpha)^k-1)
+  // = alpha x (1 - (1 - alpha) ^ k) / alpha
+  // = 1 - (1 - alpha) ^ k
+  // weight_mass_in_window = 1 - (1 - alpha) ^ k / lim_k->inf (1 - alpha) ^ k
+  // weight_mass_in_window = 1 - (1 - alpha) ^ k / 1
+  // 1 - weight_mass_in_window = (1 - alpha) ^ k
+  // log(1 - weight_mass_in_window) = k * log(1 - alpha)
+  // 10 ^ (log(1 - weight_mass_in_window) / k) = 1 - alpha
+  // alpha = 1 - 10 ^ (log(1 - weight_mass_in_window) / k)
+  // alpha = 1 - 10 ^ (log(1 - weight_mass_in_window) / window_size)
+
+  alpha_ = 1 - std::pow(10.0, std::log10(1 - weight_mass_in_window) /
+                                  static_cast<double>(window_size));
+}
+
+void LatencyModel::AddLatency(int64_t latency_ns) {
+  rolling_average_ = latency_ns * alpha_ + rolling_average_ * (1 - alpha_);
+}
+
+}  // namespace dvr
+}  // namespace android
diff --git a/services/vr/sensord/pose_service.cpp b/services/vr/sensord/pose_service.cpp
index 3cd52973bc..7534732ec2 100644
--- a/services/vr/sensord/pose_service.cpp
+++ b/services/vr/sensord/pose_service.cpp
@@ -65,6 +65,9 @@ static constexpr char kPoseRingBufferName[] = "PoseService:RingBuffer";
 static constexpr int kDatasetIdLength = 36;
 static constexpr char kDatasetIdChars[] = "0123456789abcdef-";
 
+static constexpr int kLatencyWindowSize = 100;
+static constexpr double kLatencyWindowMass = 0.5;
+
 // These are the flags used by BufferProducer::CreatePersistentUncachedBlob,
 // plus PRIVATE_ADSP_HEAP to allow access from the DSP.
 static constexpr int kPoseRingBufferFlags =
@@ -111,7 +114,8 @@ PoseService::PoseService(SensorThread* sensor_thread)
       vsync_count_(0),
       photon_timestamp_(0),
       // Will be updated by external service, but start with a non-zero value:
-      display_period_ns_(16000000) {
+      display_period_ns_(16000000),
+      sensor_latency_(kLatencyWindowSize, kLatencyWindowMass) {
   last_known_pose_ = {
       .orientation = {1.0f, 0.0f, 0.0f, 0.0f},
       .translation = {0.0f, 0.0f, 0.0f, 0.0f},
@@ -463,10 +467,13 @@ void PoseService::UpdatePoseMode() {
           start_from_head_rotation * Vector3d(0.0, kDefaultNeckVerticalOffset,
                                               -kDefaultNeckHorizontalOffset);
 
-      // IMU driver gives timestamps on its own clock, but we need monotonic
-      // clock. Subtract 5ms to account for estimated IMU sample latency.
-      WriteAsyncPoses(position, start_from_head_rotation,
-                      pose_state.timestamp_ns + 5000000);
+      // Update the current latency model.
+      sensor_latency_.AddLatency(GetSystemClockNs() - pose_state.timestamp_ns);
+
+      // Update the timestamp with the expected latency.
+      WriteAsyncPoses(
+          position, start_from_head_rotation,
+          pose_state.timestamp_ns + sensor_latency_.CurrentLatencyEstimate());
       break;
     }
     default:
diff --git a/services/vr/sensord/pose_service.h b/services/vr/sensord/pose_service.h
index fdd29b5351..7b7adecb83 100644
--- a/services/vr/sensord/pose_service.h
+++ b/services/vr/sensord/pose_service.h
@@ -11,8 +11,9 @@
 #include <dvr/pose_client.h>
 #include <pdx/service.h>
 #include <private/dvr/buffer_hub_client.h>
-#include <private/dvr/pose_client_internal.h>
 #include <private/dvr/dvr_pose_predictor.h>
+#include <private/dvr/latency_model.h>
+#include <private/dvr/pose_client_internal.h>
 #include <private/dvr/ring_buffer.h>
 
 #include "sensor_fusion.h"
@@ -132,6 +133,9 @@ class PoseService : public pdx::ServiceBase<PoseService> {
   int64_t display_period_ns_;
   int64_t right_eye_photon_offset_ns_ = 0;
 
+  // To model the measurement - arrival latency.
+  LatencyModel sensor_latency_;
+
   // Type for controlling pose orientation calculation.
   OrientationType device_orientation_type_;