Merge "Fix pose predictor jank." into oc-dev
diff --git a/libs/vr/libposepredictor/include/polynomial_predictor.h b/libs/vr/libposepredictor/include/polynomial_predictor.h
index 762afd3..4b8d51b 100644
--- a/libs/vr/libposepredictor/include/polynomial_predictor.h
+++ b/libs/vr/libposepredictor/include/polynomial_predictor.h
@@ -19,7 +19,7 @@
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 4d2eafd..beba156 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 6d48f18..d59182e 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 0000000..1bb3c4f
--- /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 0000000..8233889
--- /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 3cd5297..7534732 100644
--- a/services/vr/sensord/pose_service.cpp
+++ b/services/vr/sensord/pose_service.cpp
@@ -65,6 +65,9 @@
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 @@
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 @@
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 fdd29b5..7b7adec 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 @@
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_;