Merge "vr_wm: Import cloned buffer in VR HWC"
diff --git a/libs/vr/libposepredictor/Android.mk b/libs/vr/libposepredictor/Android.mk
index 761fe06..2217819 100755
--- a/libs/vr/libposepredictor/Android.mk
+++ b/libs/vr/libposepredictor/Android.mk
@@ -15,18 +15,18 @@
LOCAL_PATH := $(call my-dir)
sourceFiles := \
- pose_predictor.cpp \
+ predictor.cpp \
buffered_predictor.cpp \
- linear_pose_predictor.cpp \
- polynomial_pose_predictor.cpp \
+ linear_predictor.cpp \
+ polynomial_predictor.cpp \
+ dvr_pose_predictor.cpp \
includeFiles := \
- $(LOCAL_PATH)/include
+ $(LOCAL_PATH)/include \
+ external/eigen \
staticLibraries := \
- libdvrcommon \
libsensor \
- libpdx_default_transport \
sharedLibraries := \
@@ -42,13 +42,12 @@
LOCAL_MODULE := libposepredictor
include $(BUILD_STATIC_LIBRARY)
-
include $(CLEAR_VARS)
LOCAL_MODULE_TAGS := optional
LOCAL_SRC_FILES := \
- pose_predictor_tests.cpp \
- linear_pose_predictor_tests.cpp \
- polynomial_pose_predictor_tests.cpp \
+ predictor_tests.cpp \
+ linear_predictor_tests.cpp \
+ polynomial_predictor_tests.cpp \
LOCAL_STATIC_LIBRARIES := libposepredictor $(staticLibraries)
LOCAL_SHARED_LIBRARIES := $(sharedLibraries)
diff --git a/libs/vr/libposepredictor/buffered_predictor.cpp b/libs/vr/libposepredictor/buffered_predictor.cpp
index 08fd524..f3b41dc 100644
--- a/libs/vr/libposepredictor/buffered_predictor.cpp
+++ b/libs/vr/libposepredictor/buffered_predictor.cpp
@@ -1,13 +1,12 @@
-#include <private/dvr/buffered_predictor.h>
+#include <buffered_predictor.h>
-namespace android {
-namespace dvr {
+namespace posepredictor {
BufferedPredictor::BufferedPredictor(size_t buffer_size) {
buffer_.resize(buffer_size);
}
-void BufferedPredictor::BufferSample(const Sample& sample) {
+void BufferedPredictor::BufferSample(const Pose& sample) {
const auto& prev_sample = buffer_[current_pose_index_];
// If we are updating a sample (the same time stamp), do not advance the
@@ -22,19 +21,18 @@
if (PrevSample(1).orientation.coeffs().dot(sample.orientation.coeffs()) < 0) {
// Flip the quaternion to be closest to the previous sample.
buffer_[current_pose_index_].orientation =
- quatd(-sample.orientation.w(), -sample.orientation.x(),
- -sample.orientation.y(), -sample.orientation.z());
+ quat(-sample.orientation.w(), -sample.orientation.x(),
+ -sample.orientation.y(), -sample.orientation.z());
}
++num_poses_added_;
}
-const PosePredictor::Sample& BufferedPredictor::PrevSample(size_t index) const {
+const Pose& BufferedPredictor::PrevSample(size_t index) const {
// We must not request a pose too far in the past.
assert(index < buffer_.size());
return buffer_[(current_pose_index_ - index + buffer_.size()) %
buffer_.size()];
}
-} // namespace dvr
-} // namespace android
+} // namespace posepredictor
diff --git a/libs/vr/libposepredictor/dvr_pose_predictor.cpp b/libs/vr/libposepredictor/dvr_pose_predictor.cpp
new file mode 100644
index 0000000..7f2ecc0
--- /dev/null
+++ b/libs/vr/libposepredictor/dvr_pose_predictor.cpp
@@ -0,0 +1,70 @@
+#include <private/dvr/dvr_pose_predictor.h>
+
+namespace android {
+namespace dvr {
+
+namespace {
+template <typename Vec3Type>
+float32x4_t FromVec3(const Vec3Type& from) {
+ return {static_cast<float>(from.x()), static_cast<float>(from.y()),
+ static_cast<float>(from.z()), 0};
+}
+
+template <typename QuatType>
+float32x4_t FromQuat(const QuatType& from) {
+ return {static_cast<float>(from.x()), static_cast<float>(from.y()),
+ static_cast<float>(from.z()), static_cast<float>(from.w())};
+}
+
+} // namespace
+
+void AddPredictorPose(posepredictor::Predictor* predictor,
+ const posepredictor::vec3& start_t_head,
+ const posepredictor::quat& start_q_head,
+ int64_t pose_timestamp, DvrPoseAsync* out) {
+ // Feed the predictor.
+ predictor->Add(
+ posepredictor::Pose{pose_timestamp, start_t_head, start_q_head});
+
+ // Fill the output.
+ out->timestamp_ns = pose_timestamp;
+
+ out->translation = FromVec3(start_t_head);
+ out->orientation = FromQuat(start_q_head);
+
+ out->right_translation = out->translation;
+ out->right_orientation = out->orientation;
+
+ const auto velocity = predictor->PredictVelocity(pose_timestamp);
+
+ out->velocity = FromVec3(velocity.linear);
+ out->angular_velocity = FromVec3(velocity.angular);
+
+ out->flags = DVR_POSE_FLAG_HEAD | DVR_POSE_FLAG_VALID;
+ memset(out->pad, 0, sizeof(out->pad));
+}
+
+void PredictPose(const posepredictor::Predictor* predictor, int64_t left_ns,
+ int64_t right_ns, DvrPoseAsync* out) {
+ const auto left_pose = predictor->Predict(left_ns);
+ const auto right_pose = predictor->Predict(right_ns);
+ const auto velocity = predictor->PredictVelocity((left_ns + right_ns) / 2);
+
+ // Fill the output.
+ out->timestamp_ns = left_ns;
+
+ out->translation = FromVec3(left_pose.position);
+ out->orientation = FromQuat(left_pose.orientation);
+
+ out->right_translation = FromVec3(right_pose.position);
+ out->right_orientation = FromQuat(right_pose.orientation);
+
+ out->velocity = FromVec3(velocity.linear);
+ out->angular_velocity = FromVec3(velocity.angular);
+
+ out->flags = DVR_POSE_FLAG_HEAD | DVR_POSE_FLAG_VALID;
+ memset(out->pad, 0, sizeof(out->pad));
+}
+
+} // dvr
+} // android
diff --git a/libs/vr/libposepredictor/include/private/dvr/buffered_predictor.h b/libs/vr/libposepredictor/include/buffered_predictor.h
similarity index 60%
rename from libs/vr/libposepredictor/include/private/dvr/buffered_predictor.h
rename to libs/vr/libposepredictor/include/buffered_predictor.h
index 89d89e1..eab0150 100644
--- a/libs/vr/libposepredictor/include/private/dvr/buffered_predictor.h
+++ b/libs/vr/libposepredictor/include/buffered_predictor.h
@@ -1,33 +1,32 @@
-#ifndef ANDROID_DVR_BUFFERED_PREDICTOR_H_
-#define ANDROID_DVR_BUFFERED_PREDICTOR_H_
+#ifndef POSEPREDICTOR_BUFFERED_PREDICTOR_H_
+#define POSEPREDICTOR_BUFFERED_PREDICTOR_H_
#include <vector>
-#include "pose_predictor.h"
+#include "predictor.h"
-namespace android {
-namespace dvr {
+namespace posepredictor {
// Keeps the previous n poses around in a ring buffer.
// The orientations are also unrolled so that a . b > 0 for two subsequent
// quaternions a and b.
-class BufferedPredictor : public PosePredictor {
+class BufferedPredictor : public Predictor {
public:
BufferedPredictor(size_t buffer_size);
~BufferedPredictor() = default;
protected:
// Add a pose sample into the buffer.
- void BufferSample(const Sample& sample);
+ void BufferSample(const Pose& sample);
// Grab a previous sample.
// index = 0: last sample
// index = 1: the one before that
// ...
- const Sample& PrevSample(size_t index) const;
+ const Pose& PrevSample(size_t index) const;
// Where we keep the last n poses.
- std::vector<Sample> buffer_;
+ std::vector<Pose> buffer_;
// Where the last valid pose is in the buffer.
size_t current_pose_index_ = 0;
@@ -36,7 +35,6 @@
size_t num_poses_added_ = 0;
};
-} // namespace dvr
-} // namespace android
+} // namespace posepredictor
-#endif // ANDROID_DVR_BUFFERED_PREDICTOR_H_
+#endif // POSEPREDICTOR_BUFFERED_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/linear_predictor.h b/libs/vr/libposepredictor/include/linear_predictor.h
new file mode 100644
index 0000000..0d17ec5
--- /dev/null
+++ b/libs/vr/libposepredictor/include/linear_predictor.h
@@ -0,0 +1,43 @@
+#ifndef POSEPREDICTOR_LINEAR_POSE_PREDICTOR_H_
+#define POSEPREDICTOR_LINEAR_POSE_PREDICTOR_H_
+
+#include "predictor.h"
+
+namespace posepredictor {
+
+// This class makes a linear prediction using the last two samples we received.
+class LinearPosePredictor : public Predictor {
+ public:
+ LinearPosePredictor() = default;
+
+ // Add a new sample.
+ void Add(const Pose& sample) override;
+
+ // Predict using the last two samples.
+ Pose Predict(int64_t time_ns) const override;
+
+ // Just copy the velocity over.
+ Velocity PredictVelocity(int64_t time_ns) const override;
+
+ private:
+ // The index of the last sample we received.
+ size_t current_index_ = 0;
+
+ // The previous two samples.
+ Pose samples_[2];
+
+ // Experimental
+ bool forward_predict_angular_speed_ = false;
+
+ // Transient variables updated when a sample is added.
+ vec3 velocity_ = vec3::Zero();
+ vec3 rotational_velocity_ = vec3::Zero();
+ vec3 rotational_axis_ = vec3::Zero();
+ real last_angular_speed_ = 0;
+ real angular_speed_ = 0;
+ real angular_accel_ = 0;
+};
+
+} // namespace posepredictor
+
+#endif // POSEPREDICTOR_LINEAR_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/polynomial_predictor.h b/libs/vr/libposepredictor/include/polynomial_predictor.h
new file mode 100644
index 0000000..762afd3
--- /dev/null
+++ b/libs/vr/libposepredictor/include/polynomial_predictor.h
@@ -0,0 +1,168 @@
+#ifndef POSEPREDICTOR_POLYNOMIAL_POSE_PREDICTOR_H_
+#define POSEPREDICTOR_POLYNOMIAL_POSE_PREDICTOR_H_
+
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "buffered_predictor.h"
+
+namespace posepredictor {
+
+// Make a polynomial prediction of the form
+// y = coefficients_[0] + coefficients_[1] * t + coefficients_[2] * t^2 + ...
+// where t is time and y is the position and orientation.
+// We recompute the coefficients whenever we add a new sample using
+// training_window previous samples.
+template <size_t PolynomialDegree, size_t TrainingWindow>
+class PolynomialPosePredictor : public BufferedPredictor {
+ public:
+ PolynomialPosePredictor(real regularization = 1e-9)
+ : BufferedPredictor(TrainingWindow), regularization_(regularization) {
+ static_assert(PolynomialDegree + 1 >= TrainingWindow,
+ "Underconstrained polynomial regressor");
+ }
+
+ ~PolynomialPosePredictor() = default;
+
+ // We convert pose samples into a vector for matrix arithmetic using this
+ // mapping.
+ enum Components {
+ kPositionX = 0,
+ kPositionY,
+ kPositionZ,
+ kOrientationX,
+ kOrientationY,
+ kOrientationZ,
+ kOrientationW,
+ kNumComponents
+ };
+
+ // Add a new sample.
+ void Add(const Pose& sample) override {
+ // Add the sample to the ring buffer.
+ BufferedPredictor::BufferSample(sample);
+
+ Eigen::Matrix<real, TrainingWindow, kNumComponents> values;
+
+ // Get the pose samples into matrices for fitting.
+ real t_vector[TrainingWindow];
+ for (size_t i = 0; i < TrainingWindow; ++i) {
+ const auto& prev_sample = PrevSample(i);
+
+ t_vector[i] = NsToT(prev_sample.time_ns);
+
+ // Save the values we will be fitting to at each sample time.
+ values(i, kPositionX) = prev_sample.position.x();
+ values(i, kPositionY) = prev_sample.position.y();
+ values(i, kPositionZ) = prev_sample.position.z();
+ values(i, kOrientationX) = prev_sample.orientation.x();
+ values(i, kOrientationY) = prev_sample.orientation.y();
+ values(i, kOrientationZ) = prev_sample.orientation.z();
+ values(i, kOrientationW) = prev_sample.orientation.w();
+ }
+
+ // Some transient matrices for solving for coefficient matrix.
+ Eigen::Matrix<real, PolynomialDegree + 1, PolynomialDegree + 1> M;
+ Eigen::Matrix<real, PolynomialDegree + 1, 1> d;
+ Eigen::Matrix<real, PolynomialDegree + 1, 1> p;
+
+ // Create a polynomial fit for each component.
+ for (size_t component = 0; component < kNumComponents; ++component) {
+ // A = [ 1 t t^2 ... ]'
+ // x = [ coefficients[0] coefficients[1] .... ]'
+ // b = [ position.x ]'
+ // We would like to solve A' x + regularization * I = b'
+ // given the samples we have in our training window.
+ //
+ // The loop below will compute:
+ // M = A' * A
+ // d = A' * b
+ // so we can solve M * coefficients + regularization * I = b
+
+ M.setIdentity();
+ d.setZero();
+ p[0] = 1;
+
+ // M = regularization * I
+ M = M * regularization_;
+
+ // Accumulate the poses in the training window.
+ for (size_t i = 0; i < TrainingWindow; ++i) {
+ // Compute the polynomial at this sample.
+ for (size_t j = 1; j <= PolynomialDegree; ++j) {
+ p[j] = p[j - 1] * t_vector[i];
+ }
+
+ // Accumulate the left and right hand sides.
+ M = M + p * p.transpose();
+ d = d + p * values(i, component);
+ }
+
+ // M is symmetric, positive semi-definite.
+ // Note: This is not the most accurate solver out there but is fast.
+ coefficients_.row(component) = Eigen::LLT<Eigen::MatrixXd>(M).solve(d);
+ }
+ }
+
+ // Predict using the polynomial coefficients.
+ Pose Predict(int64_t time_ns) const override {
+ // Predict the left side.
+ const auto components = SamplePolynomial(time_ns);
+
+ return {time_ns,
+ vec3(components[kPositionX], components[kPositionY],
+ components[kPositionZ]),
+ quat(components[kOrientationW], components[kOrientationX],
+ components[kOrientationY], components[kOrientationZ])
+ .normalized()};
+ }
+
+ private:
+ // Evaluate the polynomial at a particular time.
+ Eigen::Matrix<real, kNumComponents, 1> SamplePolynomial(
+ int64_t time_ns) const {
+ const auto t = NsToT(time_ns);
+ Eigen::Matrix<real, PolynomialDegree + 1, 1> polynomial;
+ real current_polynomial = t;
+
+ // Compute polynomial = [ 1 t t^2 ... ]
+ polynomial[0] = 1;
+ for (size_t degree = 1; degree <= PolynomialDegree;
+ ++degree, current_polynomial *= t) {
+ polynomial[degree] = polynomial[degree - 1] * t;
+ }
+
+ // The coefficients_ = [ numComponents x (polynomial degree + 1) ].
+ return coefficients_ * polynomial;
+ }
+
+ // Convert a time in nanoseconds to t.
+ // We could use the seconds as t but this would create make it more difficult
+ // to tweak the regularization amount. So we subtract the last sample time so
+ // the scale of the regularization constant doesn't change as a function of
+ // time.
+ real NsToT(int64_t time_ns) const {
+ return NsToSeconds(time_ns - buffer_[current_pose_index_].time_ns);
+ }
+
+ // The ridge regularization constant.
+ real regularization_;
+
+ // This is where we store the polynomial coefficients.
+ Eigen::Matrix<real, kNumComponents, PolynomialDegree + 1> coefficients_;
+};
+
+// Some common polynomial types.
+extern template class PolynomialPosePredictor<1, 2>;
+extern template class PolynomialPosePredictor<2, 3>;
+extern template class PolynomialPosePredictor<3, 4>;
+extern template class PolynomialPosePredictor<4, 5>;
+
+using QuadricPosePredictor = PolynomialPosePredictor<2, 3>;
+using CubicPosePredictor = PolynomialPosePredictor<3, 4>;
+using QuarticPosePredictor = PolynomialPosePredictor<4, 5>;
+
+} // namespace posepredictor
+
+#endif // POSEPREDICTOR_POLYNOMIAL_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/predictor.h b/libs/vr/libposepredictor/include/predictor.h
new file mode 100644
index 0000000..78db272
--- /dev/null
+++ b/libs/vr/libposepredictor/include/predictor.h
@@ -0,0 +1,73 @@
+#ifndef POSEPREDICTOR_POSE_PREDICTOR_H_
+#define POSEPREDICTOR_POSE_PREDICTOR_H_
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// This is the only file you need to include for pose prediction.
+
+namespace posepredictor {
+
+// The precision for the predictor.
+// TODO(okana): double precision is probably not necessary.
+typedef double real;
+
+using vec3 = Eigen::Matrix<real, 3, 1>;
+using quat = Eigen::Quaternion<real>;
+
+// Encapsulates a pose sample.
+struct Pose {
+ int64_t time_ns = 0;
+ vec3 position = vec3::Zero();
+ quat orientation = quat::Identity();
+};
+
+// Encapsulates the derivative at a time.
+struct Velocity {
+ vec3 linear = vec3::Zero();
+ vec3 angular = vec3::Zero();
+};
+
+// The preset types we support.
+enum class PredictorType { Linear, Quadric, Cubic };
+
+// This is an abstract base class for prediction 6dof pose given
+// a set of samples.
+class Predictor {
+ public:
+ Predictor() = default;
+ virtual ~Predictor() = default;
+
+ // The nanoseconds to use for finite differencing.
+ static constexpr int64_t kFiniteDifferenceNs = 100;
+
+ // Instantiate a new pose predictor for a type.
+ static std::unique_ptr<Predictor> Create(PredictorType type);
+
+ // Compute the angular velocity from orientation start_orientation to
+ // end_orientation in delta_time.
+ static vec3 AngularVelocity(const quat& start_orientation,
+ const quat& end_orientation, real delta_time);
+
+ // Add a pose sample coming from the sensors.
+ virtual void Add(const Pose& sample) = 0;
+
+ // Make a pose prediction for at specific time.
+ virtual Pose Predict(int64_t time_ns) const = 0;
+
+ // Evaluate velocity at a particular time.
+ // The default implementation uses finite differencing.
+ virtual Velocity PredictVelocity(int64_t time_ns) const;
+
+ // Helpers
+ static real NsToSeconds(int64_t time_ns) {
+ return static_cast<real>(time_ns / 1e9);
+ }
+ static int64_t SecondsToNs(real seconds) {
+ return static_cast<int64_t>(seconds * 1e9);
+ }
+};
+
+} // namespace posepredictor
+
+#endif // POSEPREDICTOR_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/private/dvr/dvr_pose_predictor.h b/libs/vr/libposepredictor/include/private/dvr/dvr_pose_predictor.h
new file mode 100644
index 0000000..bd2dcbc
--- /dev/null
+++ b/libs/vr/libposepredictor/include/private/dvr/dvr_pose_predictor.h
@@ -0,0 +1,25 @@
+#ifndef ANDROID_DVR_POSE_PREDICTOR_H_
+#define ANDROID_DVR_POSE_PREDICTOR_H_
+
+#include <dvr/pose_client.h>
+#include <predictor.h>
+
+// Some shim functions for connecting dvr to pose predictor.
+
+namespace android {
+namespace dvr {
+
+// Feed a pose to the predictor.
+void AddPredictorPose(posepredictor::Predictor* predictor,
+ const posepredictor::vec3& start_t_head,
+ const posepredictor::quat& start_q_head,
+ int64_t pose_timestamp, DvrPoseAsync* out);
+
+// Make a prediction for left and right eyes.
+void PredictPose(const posepredictor::Predictor* predictor, int64_t left_ns,
+ int64_t right_ns, DvrPoseAsync* out);
+
+} // namespace dvr
+} // namespace android
+
+#endif // ANDROID_DVR_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/private/dvr/linear_pose_predictor.h b/libs/vr/libposepredictor/include/private/dvr/linear_pose_predictor.h
deleted file mode 100644
index 1efe938..0000000
--- a/libs/vr/libposepredictor/include/private/dvr/linear_pose_predictor.h
+++ /dev/null
@@ -1,43 +0,0 @@
-#ifndef ANDROID_DVR_POSE_PREDICTOR_H_
-#define ANDROID_DVR_POSE_PREDICTOR_H_
-
-#include <private/dvr/pose_predictor.h>
-
-namespace android {
-namespace dvr {
-
-// This class makes a linear prediction using the last two samples we received.
-class LinearPosePredictor : public PosePredictor {
- public:
- LinearPosePredictor() = default;
-
- // Add a new sample.
- void Add(const Sample& sample, DvrPoseAsync* out_pose) override;
-
- // Predict using the last two samples.
- void Predict(int64_t left_time_ns, int64_t right_time_ns,
- DvrPoseAsync* out_pose) const override;
-
- private:
- // The index of the last sample we received.
- size_t current_index_ = 0;
-
- // The previous two samples.
- Sample samples_[2];
-
- // Experimental
- bool forward_predict_angular_speed_ = false;
-
- // Transient variables updated when a sample is added.
- vec3d velocity_ = vec3d::Zero();
- vec3d rotational_velocity_ = vec3d::Zero();
- vec3d rotational_axis_ = vec3d::Zero();
- double last_angular_speed_ = 0;
- double angular_speed_ = 0;
- double angular_accel_ = 0;
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/private/dvr/polynomial_pose_predictor.h b/libs/vr/libposepredictor/include/private/dvr/polynomial_pose_predictor.h
deleted file mode 100644
index 7abb486..0000000
--- a/libs/vr/libposepredictor/include/private/dvr/polynomial_pose_predictor.h
+++ /dev/null
@@ -1,219 +0,0 @@
-#ifndef ANDROID_DVR_POLYNOMIAL_POSE_PREDICTOR_H_
-#define ANDROID_DVR_POLYNOMIAL_POSE_PREDICTOR_H_
-
-#include <vector>
-
-#include <Eigen/Dense>
-
-#include "buffered_predictor.h"
-
-namespace android {
-namespace dvr {
-
-// Make a polynomial prediction of the form
-// y = coefficients_[0] + coefficients_[1] * t + coefficients_[2] * t^2 + ...
-// where t is time and y is the position and orientation.
-// We recompute the coefficients whenever we add a new sample using
-// training_window previous samples.
-template <size_t PolynomialDegree, size_t TrainingWindow>
-class PolynomialPosePredictor : public BufferedPredictor {
- public:
- PolynomialPosePredictor(double regularization = 1e-9)
- : BufferedPredictor(TrainingWindow), regularization_(regularization) {
- static_assert(PolynomialDegree + 1 >= TrainingWindow,
- "Underconstrained polynomial regressor");
- }
-
- ~PolynomialPosePredictor() = default;
-
- // We convert pose samples into a vector for matrix arithmetic using this
- // mapping.
- enum Components {
- kPositionX = 0,
- kPositionY,
- kPositionZ,
- kOrientationX,
- kOrientationY,
- kOrientationZ,
- kOrientationW,
- kNumComponents
- };
-
- // Add a new sample.
- void Add(const Sample& sample, DvrPoseAsync* out_pose) override {
- // Add the sample to the ring buffer.
- BufferedPredictor::BufferSample(sample);
-
- Eigen::Matrix<double, TrainingWindow, kNumComponents> values;
-
- // Get the pose samples into matrices for fitting.
- double t_vector[TrainingWindow];
- for (size_t i = 0; i < TrainingWindow; ++i) {
- const auto& prev_sample = PrevSample(i);
-
- t_vector[i] = NsToT(prev_sample.time_ns);
-
- // Save the values we will be fitting to at each sample time.
- values(i, kPositionX) = prev_sample.position.x();
- values(i, kPositionY) = prev_sample.position.y();
- values(i, kPositionZ) = prev_sample.position.z();
- values(i, kOrientationX) = prev_sample.orientation.x();
- values(i, kOrientationY) = prev_sample.orientation.y();
- values(i, kOrientationZ) = prev_sample.orientation.z();
- values(i, kOrientationW) = prev_sample.orientation.w();
- }
-
- // Some transient matrices for solving for coefficient matrix.
- Eigen::Matrix<double, PolynomialDegree + 1, PolynomialDegree + 1> M;
- Eigen::Vector<double, PolynomialDegree + 1> d;
- Eigen::Vector<double, PolynomialDegree + 1> p;
-
- // Create a polynomial fit for each component.
- for (size_t component = 0; component < kNumComponents; ++component) {
- // A = [ 1 t t^2 ... ]'
- // x = [ coefficients[0] coefficients[1] .... ]'
- // b = [ position.x ]'
- // We would like to solve A' x + regularization * I = b'
- // given the samples we have in our training window.
- //
- // The loop below will compute:
- // M = A' * A
- // d = A' * b
- // so we can solve M * coefficients + regularization * I = b
-
- M.setIdentity();
- d.setZero();
- p[0] = 1;
-
- // M = regularization * I
- M = M * regularization_;
-
- // Accumulate the poses in the training window.
- for (size_t i = 0; i < TrainingWindow; ++i) {
- // Compute the polynomial at this sample.
- for (size_t j = 1; j <= PolynomialDegree; ++j) {
- p[j] = p[j - 1] * t_vector[i];
- }
-
- // Accumulate the left and right hand sides.
- M = M + p * p.transpose();
- d = d + p * values(i, component);
- }
-
- // M is symmetric, positive semi-definite.
- // Note: This is not the most accurate solver out there but is fast.
- coefficients_.row(component) = Eigen::LLT<Eigen::MatrixXd>(M).solve(d);
- }
-
- // Fill out the out_pose at this sample.
- Predict(sample.time_ns, sample.time_ns, out_pose);
- }
-
- // Predict using the polynomial coefficients.
- void Predict(int64_t left_time_ns, int64_t right_time_ns,
- DvrPoseAsync* out_pose) const override {
- // Predict the left side.
- const auto left = SamplePolynomial(left_time_ns);
- out_pose->translation = {static_cast<float>(left[kPositionX]),
- static_cast<float>(left[kPositionY]),
- static_cast<float>(left[kPositionZ])};
- out_pose->orientation = normalize(left[kOrientationX], left[kOrientationY],
- left[kOrientationZ], left[kOrientationW]);
-
- // Predict the right side.
- const auto right = SamplePolynomial(right_time_ns);
- out_pose->right_translation = {static_cast<float>(right[kPositionX]),
- static_cast<float>(right[kPositionY]),
- static_cast<float>(right[kPositionZ])};
- out_pose->right_orientation =
- normalize(right[kOrientationX], right[kOrientationY],
- right[kOrientationZ], right[kOrientationW]);
-
- // Finite differencing to estimate the velocities.
- const auto a = SamplePolynomial(
- (left_time_ns + right_time_ns - kFiniteDifferenceNs) / 2);
- const auto b = SamplePolynomial(
- (left_time_ns + right_time_ns + kFiniteDifferenceNs) / 2);
-
- out_pose->velocity = {static_cast<float>((b[kPositionX] - a[kPositionX]) /
- NsToSeconds(kFiniteDifferenceNs)),
- static_cast<float>((b[kPositionY] - a[kPositionY]) /
- NsToSeconds(kFiniteDifferenceNs)),
- static_cast<float>((b[kPositionZ] - a[kPositionZ]) /
- NsToSeconds(kFiniteDifferenceNs)),
- 0.0f};
-
- // Get the predicted orientations into quaternions, which are probably not
- // quite unit.
- const quatd a_orientation(a[kOrientationW], a[kOrientationX],
- a[kOrientationY], a[kOrientationZ]);
- const quatd b_orientation(b[kOrientationW], b[kOrientationX],
- b[kOrientationY], b[kOrientationZ]);
- const auto angular_velocity =
- AngularVelocity(a_orientation.normalized(), b_orientation.normalized(),
- NsToSeconds(kFiniteDifferenceNs));
-
- out_pose->angular_velocity = {static_cast<float>(angular_velocity[0]),
- static_cast<float>(angular_velocity[1]),
- static_cast<float>(angular_velocity[2]),
- 0.0f};
- out_pose->timestamp_ns = left_time_ns;
- out_pose->flags = DVR_POSE_FLAG_HEAD | DVR_POSE_FLAG_VALID;
- memset(out_pose->pad, 0, sizeof(out_pose->pad));
- }
-
- private:
- // Take a quaternion and return a normalized version in a float32x4_t.
- static float32x4_t normalize(double x, double y, double z, double w) {
- const auto l = std::sqrt(x * x + y * y + z * z + w * w);
- return {static_cast<float>(x / l), static_cast<float>(y / l),
- static_cast<float>(z / l), static_cast<float>(w / l)};
- }
-
- // Evaluate the polynomial at a particular time.
- Eigen::Vector<double, kNumComponents> SamplePolynomial(
- int64_t time_ns) const {
- const auto t = NsToT(time_ns);
- Eigen::Vector<double, PolynomialDegree + 1> polynomial;
- double current_polynomial = t;
-
- // Compute polynomial = [ 1 t t^2 ... ]
- polynomial[0] = 1;
- for (size_t degree = 1; degree <= PolynomialDegree;
- ++degree, current_polynomial *= t) {
- polynomial[degree] = polynomial[degree - 1] * t;
- }
-
- // The coefficients_ = [ numComponents x (polynomial degree + 1) ].
- return coefficients_ * polynomial;
- }
-
- // Convert a time in nanoseconds to t.
- // We could use the seconds as t but this would create make it more difficult
- // to tweak the regularization amount. So we subtract the last sample time so
- // the scale of the regularization constant doesn't change as a function of
- // time.
- double NsToT(int64_t time_ns) const {
- return NsToSeconds(time_ns - buffer_[current_pose_index_].time_ns);
- }
-
- // The ridge regularization constant.
- double regularization_;
-
- // This is where we store the polynomial coefficients.
- Eigen::Matrix<double, kNumComponents, PolynomialDegree + 1> coefficients_;
-};
-
-// Some common polynomial types.
-extern template class PolynomialPosePredictor<1, 2>;
-extern template class PolynomialPosePredictor<2, 3>;
-extern template class PolynomialPosePredictor<3, 4>;
-extern template class PolynomialPosePredictor<4, 5>;
-
-using QuadricPosePredictor = PolynomialPosePredictor<2, 3>;
-using CubicPosePredictor = PolynomialPosePredictor<3, 4>;
-using QuarticPosePredictor = PolynomialPosePredictor<4, 5>;
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/include/private/dvr/pose_predictor.h b/libs/vr/libposepredictor/include/private/dvr/pose_predictor.h
deleted file mode 100644
index d774500..0000000
--- a/libs/vr/libposepredictor/include/private/dvr/pose_predictor.h
+++ /dev/null
@@ -1,60 +0,0 @@
-#ifndef ANDROID_DVR_LINEAR_POSE_PREDICTOR_H_
-#define ANDROID_DVR_LINEAR_POSE_PREDICTOR_H_
-
-#include <private/dvr/pose_client_internal.h>
-#include <private/dvr/types.h>
-
-namespace android {
-namespace dvr {
-
-// This is an abstract base class for prediction 6dof pose given
-// a set of samples.
-//
-// TODO(okana): Create a framework for testing different subclasses for
-// performance and accuracy.
-class PosePredictor {
- public:
- PosePredictor() = default;
- virtual ~PosePredictor() = default;
-
- // The nanoseconds to use for finite differencing.
- static constexpr int64_t kFiniteDifferenceNs = 100;
-
- // Encapsulates a pose sample.
- struct Sample {
- vec3d position = vec3d::Zero();
- quatd orientation = quatd::Identity();
- int64_t time_ns = 0;
- };
-
- // Compute the angular velocity from orientation start_orientation to
- // end_orientation in delta_time.
- static vec3d AngularVelocity(const quatd& start_orientation,
- const quatd& end_orientation, double delta_time);
-
- // Initialize the out pose from a sample.
- static void InitializeFromSample(const Sample& sample, DvrPoseAsync* out_pose,
- const vec3d& velocity,
- const vec3d& angular_velocity);
-
- // Add a pose sample coming from the sensors.
- // Returns this sample as a dvr pose.
- //
- // We will use the returned pose if prediction is not enabled.
- virtual void Add(const Sample& sample, DvrPoseAsync* out_pose) = 0;
-
- // Make a pose prediction for the left and right eyes at specific times.
- virtual void Predict(int64_t left_time_ns, int64_t right_time_ns,
- DvrPoseAsync* out_pose) const = 0;
-
- // Helpers
- static double NsToSeconds(int64_t time_ns) { return time_ns / 1e9; }
- static int64_t SecondsToNs(double seconds) {
- return static_cast<int64_t>(seconds * 1e9);
- }
-};
-
-} // namespace dvr
-} // namespace android
-
-#endif // ANDROID_DVR_LINEAR_POSE_PREDICTOR_H_
diff --git a/libs/vr/libposepredictor/linear_pose_predictor.cpp b/libs/vr/libposepredictor/linear_pose_predictor.cpp
deleted file mode 100644
index 11db735..0000000
--- a/libs/vr/libposepredictor/linear_pose_predictor.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-#include <private/dvr/linear_pose_predictor.h>
-
-namespace android {
-namespace dvr {
-
-using AngleAxisd = Eigen::AngleAxis<double>;
-
-void LinearPosePredictor::Add(const Sample& sample, DvrPoseAsync* out_pose) {
- // If we are receiving a new sample, move the index to the next item.
- // If the time stamp is the same as the last frame, we will just overwrite
- // it with the new data.
- if (sample.time_ns != samples_[current_index_].time_ns) {
- current_index_ ^= 1;
- }
-
- // Save the sample.
- samples_[current_index_] = sample;
-
- // The previous sample we received.
- const auto& previous_sample = samples_[current_index_ ^ 1];
-
- // Ready to compute velocities.
- const auto pose_delta_time =
- NsToSeconds(sample.time_ns - previous_sample.time_ns);
-
- if (pose_delta_time > 0.0) {
- velocity_ = (sample.position - previous_sample.position) / pose_delta_time;
- rotational_velocity_ = PosePredictor::AngularVelocity(
- previous_sample.orientation, sample.orientation, pose_delta_time);
- } else {
- velocity_ = vec3d::Zero();
- rotational_velocity_ = vec3d::Zero();
- }
-
- // Temporary experiment with acceleration estimate.
- angular_speed_ = rotational_velocity_.norm();
- angular_accel_ = 0.0;
- if (forward_predict_angular_speed_) {
- angular_accel_ =
- pose_delta_time > 0.0
- ? (angular_speed_ - last_angular_speed_) / pose_delta_time
- : 0.0;
- }
- last_angular_speed_ = angular_speed_;
-
- rotational_axis_ = vec3d(0.0, 1.0, 0.0);
- if (angular_speed_ > 0.0) {
- rotational_axis_ = rotational_velocity_ / angular_speed_;
- }
-
- InitializeFromSample(sample, out_pose, velocity_, rotational_velocity_);
-}
-
-void LinearPosePredictor::Predict(int64_t left_time_ns, int64_t right_time_ns,
- DvrPoseAsync* out_pose) const {
- const auto& sample = samples_[current_index_];
-
- double dt = NsToSeconds(left_time_ns - sample.time_ns);
- double r_dt = NsToSeconds(right_time_ns - sample.time_ns);
-
- // Temporary forward prediction code.
- auto start_t_head_future = sample.position + velocity_ * dt;
- auto r_start_t_head_future = sample.position + velocity_ * r_dt;
- double angle = angular_speed_ * dt;
- double r_angle = angular_speed_ * r_dt;
- if (__builtin_expect(forward_predict_angular_speed_, 0)) {
- angle += 0.5 * angular_accel_ * dt * dt;
- r_angle += 0.5 * angular_accel_ * r_dt * r_dt;
- }
- auto start_q_head_future =
- sample.orientation * quatd(AngleAxisd(angle, rotational_axis_));
- auto r_start_q_head_future =
- sample.orientation * quatd(AngleAxisd(r_angle, rotational_axis_));
-
- out_pose->orientation = {static_cast<float>(start_q_head_future.x()),
- static_cast<float>(start_q_head_future.y()),
- static_cast<float>(start_q_head_future.z()),
- static_cast<float>(start_q_head_future.w())};
-
- out_pose->translation = {static_cast<float>(start_t_head_future.x()),
- static_cast<float>(start_t_head_future.y()),
- static_cast<float>(start_t_head_future.z()), 0.0f};
-
- out_pose->right_orientation = {static_cast<float>(r_start_q_head_future.x()),
- static_cast<float>(r_start_q_head_future.y()),
- static_cast<float>(r_start_q_head_future.z()),
- static_cast<float>(r_start_q_head_future.w())};
-
- out_pose->right_translation = {static_cast<float>(r_start_t_head_future.x()),
- static_cast<float>(r_start_t_head_future.y()),
- static_cast<float>(r_start_t_head_future.z()),
- 0.0f};
-
- out_pose->angular_velocity = {static_cast<float>(rotational_velocity_.x()),
- static_cast<float>(rotational_velocity_.y()),
- static_cast<float>(rotational_velocity_.z()),
- 0.0f};
-
- out_pose->velocity = {static_cast<float>(velocity_.x()),
- static_cast<float>(velocity_.y()),
- static_cast<float>(velocity_.z()), 0.0f};
-
- out_pose->timestamp_ns = left_time_ns;
- out_pose->flags = DVR_POSE_FLAG_HEAD | DVR_POSE_FLAG_VALID;
- memset(out_pose->pad, 0, sizeof(out_pose->pad));
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/libs/vr/libposepredictor/linear_pose_predictor_tests.cpp b/libs/vr/libposepredictor/linear_pose_predictor_tests.cpp
deleted file mode 100644
index 1f38041a..0000000
--- a/libs/vr/libposepredictor/linear_pose_predictor_tests.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-#include <gtest/gtest.h>
-
-#include <private/dvr/linear_pose_predictor.h>
-
-namespace android {
-namespace dvr {
-
-namespace {
-
-// For comparing expected and actual.
-constexpr double kAbsErrorTolerance = 1e-5;
-
-// The default rotation axis we will be using.
-const vec3d kRotationAxis = vec3d(1, 4, 3).normalized();
-
-// Linearly interpolate between a and b.
-vec3d lerp(const vec3d& a, const vec3d& b, double t) { return (b - a) * t + a; }
-
-// Linearly interpolate between two angles and return the resulting rotation as
-// a quaternion (around the kRotationAxis).
-quatd qlerp(double angle1, double angle2, double t) {
- return quatd(
- Eigen::AngleAxis<double>((angle2 - angle1) * t + angle1, kRotationAxis));
-}
-
-// Compare two positions.
-void TestPosition(const vec3d& expected, const float32x4_t& actual) {
- for (int i = 0; i < 3; ++i) {
- EXPECT_NEAR(expected[i], static_cast<double>(actual[i]),
- kAbsErrorTolerance);
- }
-}
-
-// Compare two orientations.
-void TestOrientation(const quatd& expected, const float32x4_t& actual) {
- // abs(expected.dot(actual)) > 1-eps
- EXPECT_GE(std::abs(vec4d(actual[0], actual[1], actual[2], actual[3])
- .dot(expected.coeffs())),
- 0.99);
-}
-}
-
-// Test the extrapolation from two samples.
-TEST(LinearPosePredictorTest, Extrapolation) {
- LinearPosePredictor predictor;
-
- // We wil extrapolate linearly from [position|orientation] 1 -> 2.
- const vec3d position1(0, 0, 0);
- const vec3d position2(1, 2, 3);
- const double angle1 = M_PI * 0.3;
- const double angle2 = M_PI * 0.5;
- const quatd orientation1(Eigen::AngleAxis<double>(angle1, kRotationAxis));
- const quatd orientation2(Eigen::AngleAxis<double>(angle2, kRotationAxis));
- const int64_t t1_ns = 0; //< First sample time stamp
- const int64_t t2_ns = 10; //< The second sample time stamp
- const int64_t eval_left_ns = 23; //< The eval time for left
- const int64_t eval_right_ns = 31; //< The eval time for right
- DvrPoseAsync start_pose, end_pose, extrapolated_pose;
-
- predictor.Add(
- PosePredictor::Sample{
- .position = position1, .orientation = orientation1, .time_ns = t1_ns},
- &start_pose);
-
- // The start pose is passthough.
- TestPosition(position1, start_pose.translation);
- TestPosition(position1, start_pose.right_translation);
- TestOrientation(orientation1, start_pose.orientation);
- TestOrientation(orientation1, start_pose.right_orientation);
- EXPECT_EQ(t1_ns, start_pose.timestamp_ns);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = position2, .orientation = orientation2, .time_ns = t2_ns},
- &end_pose);
-
- TestPosition(position2, end_pose.translation);
- TestPosition(position2, end_pose.right_translation);
- TestOrientation(orientation2, end_pose.orientation);
- TestOrientation(orientation2, end_pose.right_orientation);
- EXPECT_EQ(t2_ns, end_pose.timestamp_ns);
-
- // Extrapolate from t1 - t2 to eval_[left/right].
- predictor.Predict(eval_left_ns, eval_right_ns, &extrapolated_pose);
-
- // The interpolation factors for left and right.
- const auto left_t =
- (eval_left_ns - t1_ns) / static_cast<double>(t2_ns - t1_ns);
- EXPECT_EQ(2.3, left_t);
-
- const auto right_t =
- (eval_right_ns - t1_ns) / static_cast<double>(t2_ns - t1_ns);
- EXPECT_EQ(3.1, right_t);
-
- TestPosition(lerp(position1, position2, left_t),
- extrapolated_pose.translation);
- TestPosition(lerp(position1, position2, right_t),
- extrapolated_pose.right_translation);
- TestOrientation(qlerp(angle1, angle2, left_t), extrapolated_pose.orientation);
- TestOrientation(qlerp(angle1, angle2, right_t),
- extrapolated_pose.right_orientation);
-}
-
-// Test three samples, where the last two samples have the same timestamp.
-TEST(LinearPosePredictorTest, DuplicateSamples) {
- LinearPosePredictor predictor;
-
- const vec3d position1(0, 0, 0);
- const vec3d position2(1, 2, 3);
- const vec3d position3(2, 2, 3);
- const double angle1 = M_PI * 0.3;
- const double angle2 = M_PI * 0.5;
- const double angle3 = M_PI * 0.65;
- const quatd orientation1(Eigen::AngleAxis<double>(angle1, kRotationAxis));
- const quatd orientation2(Eigen::AngleAxis<double>(angle2, kRotationAxis));
- const quatd orientation3(Eigen::AngleAxis<double>(angle3, kRotationAxis));
- const int64_t t1_ns = 0;
- const int64_t t2_ns = 10;
- const int64_t eval_left_ns = 27;
- const int64_t eval_right_ns = 31;
- DvrPoseAsync start_pose, end_pose, extrapolated_pose;
-
- predictor.Add(
- PosePredictor::Sample{
- .position = position1, .orientation = orientation1, .time_ns = t1_ns},
- &start_pose);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = position2, .orientation = orientation2, .time_ns = t2_ns},
- &end_pose);
-
- {
- // Extrapolate from t1 - t2 to eval_[left/right].
- predictor.Predict(eval_left_ns, eval_right_ns, &extrapolated_pose);
-
- // The interpolation factors for left and right.
- const auto left_t =
- (eval_left_ns - t1_ns) / static_cast<double>(t2_ns - t1_ns);
- const auto right_t =
- (eval_right_ns - t1_ns) / static_cast<double>(t2_ns - t1_ns);
-
- // Test the result.
- TestPosition(lerp(position1, position2, left_t),
- extrapolated_pose.translation);
- TestPosition(lerp(position1, position2, right_t),
- extrapolated_pose.right_translation);
- TestOrientation(qlerp(angle1, angle2, left_t),
- extrapolated_pose.orientation);
- TestOrientation(qlerp(angle1, angle2, right_t),
- extrapolated_pose.right_orientation);
- }
-
- // Sending a duplicate sample here.
- predictor.Add(
- PosePredictor::Sample{
- .position = position3, .orientation = orientation3, .time_ns = t2_ns},
- &end_pose);
-
- {
- // Extrapolate from t1 - t2 to eval_[left/right].
- predictor.Predict(eval_left_ns, eval_right_ns, &extrapolated_pose);
-
- // The interpolation factors for left and right.
- const auto left_t =
- (eval_left_ns - t1_ns) / static_cast<double>(t2_ns - t1_ns);
- const auto right_t =
- (eval_right_ns - t1_ns) / static_cast<double>(t2_ns - t1_ns);
-
- // Test the result.
- TestPosition(lerp(position1, position3, left_t),
- extrapolated_pose.translation);
- TestPosition(lerp(position1, position3, right_t),
- extrapolated_pose.right_translation);
- TestOrientation(qlerp(angle1, angle3, left_t),
- extrapolated_pose.orientation);
- TestOrientation(qlerp(angle1, angle3, right_t),
- extrapolated_pose.right_orientation);
- }
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/libs/vr/libposepredictor/linear_predictor.cpp b/libs/vr/libposepredictor/linear_predictor.cpp
new file mode 100644
index 0000000..6f924dc
--- /dev/null
+++ b/libs/vr/libposepredictor/linear_predictor.cpp
@@ -0,0 +1,70 @@
+#include <linear_predictor.h>
+
+namespace posepredictor {
+
+using AngleAxis = Eigen::AngleAxis<real>;
+
+void LinearPosePredictor::Add(const Pose& sample) {
+ // If we are receiving a new sample, move the index to the next item.
+ // If the time stamp is the same as the last frame, we will just overwrite
+ // it with the new data.
+ if (sample.time_ns != samples_[current_index_].time_ns) {
+ current_index_ ^= 1;
+ }
+
+ // Save the sample.
+ samples_[current_index_] = sample;
+
+ // The previous sample we received.
+ const auto& previous_sample = samples_[current_index_ ^ 1];
+
+ // Ready to compute velocities.
+ const auto pose_delta_time =
+ NsToSeconds(sample.time_ns - previous_sample.time_ns);
+
+ if (pose_delta_time > 0.0) {
+ velocity_ = (sample.position - previous_sample.position) / pose_delta_time;
+ rotational_velocity_ = Predictor::AngularVelocity(
+ previous_sample.orientation, sample.orientation, pose_delta_time);
+ } else {
+ velocity_ = vec3::Zero();
+ rotational_velocity_ = vec3::Zero();
+ }
+
+ // Temporary experiment with acceleration estimate.
+ angular_speed_ = rotational_velocity_.norm();
+ angular_accel_ = 0.0;
+ if (forward_predict_angular_speed_) {
+ angular_accel_ =
+ pose_delta_time > 0.0
+ ? (angular_speed_ - last_angular_speed_) / pose_delta_time
+ : 0.0;
+ }
+ last_angular_speed_ = angular_speed_;
+
+ rotational_axis_ = vec3(0.0, 1.0, 0.0);
+ if (angular_speed_ > 0.0) {
+ rotational_axis_ = rotational_velocity_ / angular_speed_;
+ }
+}
+
+Pose LinearPosePredictor::Predict(int64_t time_ns) const {
+ const auto& sample = samples_[current_index_];
+
+ const auto dt = NsToSeconds(time_ns - sample.time_ns);
+
+ // Temporary forward prediction code.
+ auto angle = angular_speed_ * dt;
+ if (__builtin_expect(forward_predict_angular_speed_, 0)) {
+ angle += 0.5 * angular_accel_ * dt * dt;
+ }
+
+ return {time_ns, sample.position + velocity_ * dt,
+ sample.orientation * quat(AngleAxis(angle, rotational_axis_))};
+}
+
+Velocity LinearPosePredictor::PredictVelocity(int64_t /* time_ns */) const {
+ return {velocity_, rotational_velocity_};
+}
+
+} // namespace posepredictor
diff --git a/libs/vr/libposepredictor/linear_predictor_tests.cpp b/libs/vr/libposepredictor/linear_predictor_tests.cpp
new file mode 100644
index 0000000..d94aa2d
--- /dev/null
+++ b/libs/vr/libposepredictor/linear_predictor_tests.cpp
@@ -0,0 +1,170 @@
+#include <gtest/gtest.h>
+
+#include <linear_predictor.h>
+
+namespace posepredictor {
+
+namespace {
+
+// For comparing expected and actual.
+constexpr real kAbsErrorTolerance = 1e-5;
+
+// The default rotation axis we will be using.
+const vec3 kRotationAxis = vec3(1, 4, 3).normalized();
+
+// Linearly interpolate between a and b.
+vec3 lerp(const vec3& a, const vec3& b, real t) { return (b - a) * t + a; }
+
+// Linearly interpolate between two angles and return the resulting rotation as
+// a quaternion (around the kRotationAxis).
+quat qlerp(real angle1, real angle2, real t) {
+ return quat(
+ Eigen::AngleAxis<real>((angle2 - angle1) * t + angle1, kRotationAxis));
+}
+
+// Compare two positions.
+void TestPosition(const vec3& expected, const vec3& actual) {
+ for (int i = 0; i < 3; ++i) {
+ EXPECT_NEAR(expected[i], actual[i], kAbsErrorTolerance);
+ }
+}
+
+// Compare two orientations.
+void TestOrientation(const quat& expected, const quat& actual) {
+ // abs(expected.dot(actual)) > 1-eps
+ EXPECT_GE(std::abs(actual.coeffs().dot(expected.coeffs())), 0.99);
+}
+}
+
+// Test the extrapolation from two samples.
+TEST(LinearPosePredictorTest, Extrapolation) {
+ LinearPosePredictor predictor;
+
+ // We wil extrapolate linearly from [position|orientation] 1 -> 2.
+ const vec3 position1(0, 0, 0);
+ const vec3 position2(1, 2, 3);
+ const real angle1 = M_PI * 0.3;
+ const real angle2 = M_PI * 0.5;
+ const quat orientation1(Eigen::AngleAxis<real>(angle1, kRotationAxis));
+ const quat orientation2(Eigen::AngleAxis<real>(angle2, kRotationAxis));
+ const int64_t t1_ns = 0; //< First sample time stamp
+ const int64_t t2_ns = 10; //< The second sample time stamp
+ const int64_t eval_left_ns = 23; //< The eval time for left
+ const int64_t eval_right_ns = 31; //< The eval time for right
+ Pose start_pose, end_pose, extrapolated_pose;
+
+ predictor.Add(Pose{
+ .position = position1, .orientation = orientation1, .time_ns = t1_ns});
+
+ predictor.Add(Pose{
+ .position = position2, .orientation = orientation2, .time_ns = t2_ns});
+
+ // Extrapolate from t1 - t2 to eval_[left/right].
+ extrapolated_pose = predictor.Predict(eval_left_ns);
+
+ // The interpolation factors for left and right.
+ const auto left_t = (eval_left_ns - t1_ns) / static_cast<real>(t2_ns - t1_ns);
+ EXPECT_EQ(2.3, left_t);
+
+ TestPosition(lerp(position1, position2, left_t), extrapolated_pose.position);
+
+ TestOrientation(qlerp(angle1, angle2, left_t), extrapolated_pose.orientation);
+
+ extrapolated_pose = predictor.Predict(eval_right_ns);
+
+ const auto right_t =
+ (eval_right_ns - t1_ns) / static_cast<real>(t2_ns - t1_ns);
+ EXPECT_EQ(3.1, right_t);
+
+ TestPosition(lerp(position1, position2, right_t), extrapolated_pose.position);
+
+ TestOrientation(qlerp(angle1, angle2, right_t),
+ extrapolated_pose.orientation);
+}
+
+// Test three samples, where the last two samples have the same timestamp.
+TEST(LinearPosePredictorTest, DuplicateSamples) {
+ LinearPosePredictor predictor;
+
+ const vec3 position1(0, 0, 0);
+ const vec3 position2(1, 2, 3);
+ const vec3 position3(2, 2, 3);
+ const real angle1 = M_PI * 0.3;
+ const real angle2 = M_PI * 0.5;
+ const real angle3 = M_PI * 0.65;
+ const quat orientation1(Eigen::AngleAxis<real>(angle1, kRotationAxis));
+ const quat orientation2(Eigen::AngleAxis<real>(angle2, kRotationAxis));
+ const quat orientation3(Eigen::AngleAxis<real>(angle3, kRotationAxis));
+ const int64_t t1_ns = 0;
+ const int64_t t2_ns = 10;
+ const int64_t eval_left_ns = 27;
+ const int64_t eval_right_ns = 31;
+ Pose extrapolated_pose;
+
+ predictor.Add(Pose{
+ .position = position1, .orientation = orientation1, .time_ns = t1_ns});
+
+ predictor.Add(Pose{
+ .position = position2, .orientation = orientation2, .time_ns = t2_ns});
+
+ {
+ // Extrapolate from t1 - t2 to eval_[left/right].
+ extrapolated_pose = predictor.Predict(eval_left_ns);
+
+ // The interpolation factors for left and right.
+ const auto left_t =
+ (eval_left_ns - t1_ns) / static_cast<real>(t2_ns - t1_ns);
+
+ // Test the result.
+ TestPosition(lerp(position1, position2, left_t),
+ extrapolated_pose.position);
+
+ TestOrientation(qlerp(angle1, angle2, left_t),
+ extrapolated_pose.orientation);
+
+ extrapolated_pose = predictor.Predict(eval_right_ns);
+
+ const auto right_t =
+ (eval_right_ns - t1_ns) / static_cast<real>(t2_ns - t1_ns);
+
+ TestPosition(lerp(position1, position2, right_t),
+ extrapolated_pose.position);
+
+ TestOrientation(qlerp(angle1, angle2, right_t),
+ extrapolated_pose.orientation);
+ }
+
+ // Sending a duplicate sample here.
+ predictor.Add(Pose{
+ .position = position3, .orientation = orientation3, .time_ns = t2_ns});
+
+ {
+ // Extrapolate from t1 - t2 to eval_[left/right].
+ extrapolated_pose = predictor.Predict(eval_left_ns);
+
+ // The interpolation factors for left and right.
+ const auto left_t =
+ (eval_left_ns - t1_ns) / static_cast<real>(t2_ns - t1_ns);
+
+ TestPosition(lerp(position1, position3, left_t),
+ extrapolated_pose.position);
+
+ TestOrientation(qlerp(angle1, angle3, left_t),
+ extrapolated_pose.orientation);
+
+ extrapolated_pose = predictor.Predict(eval_right_ns);
+
+ const auto right_t =
+ (eval_right_ns - t1_ns) / static_cast<real>(t2_ns - t1_ns);
+
+ // Test the result.
+
+ TestPosition(lerp(position1, position3, right_t),
+ extrapolated_pose.position);
+
+ TestOrientation(qlerp(angle1, angle3, right_t),
+ extrapolated_pose.orientation);
+ }
+}
+
+} // namespace posepredictor
diff --git a/libs/vr/libposepredictor/polynomial_pose_predictor_tests.cpp b/libs/vr/libposepredictor/polynomial_pose_predictor_tests.cpp
deleted file mode 100644
index 9722182..0000000
--- a/libs/vr/libposepredictor/polynomial_pose_predictor_tests.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-#include <gtest/gtest.h>
-
-#include <private/dvr/polynomial_pose_predictor.h>
-
-namespace android {
-namespace dvr {
-
-namespace {
-
-// For comparing expected and actual.
-constexpr double kAbsErrorTolerance = 1e-5;
-
-// Test the linear extrapolation from two samples.
-TEST(PolynomialPosePredictor, Linear) {
- DvrPoseAsync dummy;
-
- // Degree = 1, simple line, passing through two points.
- // Note the regularization is 0 so we expect the exact fit.
- PolynomialPosePredictor<1, 2> predictor(0);
-
- // Add two samples.
- predictor.Add(
- PosePredictor::Sample{
- .position = {0, 0, 0}, .orientation = {0, 0, 0, 1}, .time_ns = 0},
- &dummy);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 10},
- &dummy);
-
- DvrPoseAsync predicted_pose;
-
- predictor.Predict(20, 30, &predicted_pose);
-
- // Check the x,y,z components for the expected translation.
- EXPECT_NEAR(predicted_pose.translation[0], 2, kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.translation[1], 4, kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.translation[2], 6, kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.right_translation[0], 3, kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.right_translation[1], 6, kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.right_translation[2], 9, kAbsErrorTolerance);
-}
-
-// Test the degree two polynomial fit.
-TEST(PolynomialPosePredictor, Quadric) {
- DvrPoseAsync dummy;
-
- // Degree = 2, need three samples to fit a polynomial.
- // Note the regularization is 0 so we expect the exact fit.
- PolynomialPosePredictor<2, 3> predictor(0);
-
- // Add three samples.
- predictor.Add(
- PosePredictor::Sample{
- .position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 0},
- &dummy);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = {0, 0, 0}, .orientation = {0, 0, 0, 1}, .time_ns = 10},
- &dummy);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 20},
- &dummy);
-
- // The expected polynomials for x/y/z.
-
- // x: 0.01 * t^2 - 0.2 * t + 1
- const auto x = [](auto t) { return 0.01 * t * t - 0.2 * t + 1; };
-
- // y: 0.02 * t^2 - 0.4 * t + 2
- const auto y = [](auto t) { return 0.02 * t * t - 0.4 * t + 2; };
-
- // z: 0.03 * t^2 - 0.6 * t + 3
- const auto z = [](auto t) { return 0.03 * t * t - 0.6 * t + 3; };
-
- DvrPoseAsync predicted_pose;
-
- predictor.Predict(40, 50, &predicted_pose);
-
- // Check the x,y,z components for the expected translation.
- EXPECT_NEAR(predicted_pose.translation[0], x(40), kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.translation[1], y(40), kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.translation[2], z(40), kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.right_translation[0], x(50), kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.right_translation[1], y(50), kAbsErrorTolerance);
- EXPECT_NEAR(predicted_pose.right_translation[2], z(50), kAbsErrorTolerance);
-}
-
-// Test the degree two polynomial fit with degenerate input.
-//
-// The input samples all lie in a line which would normally make our system
-// degenerate. We will rely on the regularization term to recover the linear
-// solution in a quadric predictor.
-TEST(PolynomialPosePredictor, QuadricDegenate) {
- DvrPoseAsync dummy;
-
- // Degree = 2, need three samples to fit a polynomial.
- // Note that we are using the default regularization term here.
- // We cannot use 0 regularizer since the input is degenerate.
- PolynomialPosePredictor<2, 3> predictor(1e-20);
-
- // Add three samples.
- predictor.Add(
- PosePredictor::Sample{
- .position = {0, 0, 0}, .orientation = {0, 0, 0, 1}, .time_ns = 0},
- &dummy);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 10},
- &dummy);
-
- predictor.Add(
- PosePredictor::Sample{
- .position = {2, 4, 6}, .orientation = {0, 0, 0, 1}, .time_ns = 20},
- &dummy);
-
- DvrPoseAsync predicted_pose;
-
- predictor.Predict(30, 40, &predicted_pose);
-
- // Check the x,y,z components for the expected translation.
- // We are using a higher error threshold since this is now approximate.
- EXPECT_NEAR(predicted_pose.translation[0], 3, 0.001);
- EXPECT_NEAR(predicted_pose.translation[1], 6, 0.001);
- EXPECT_NEAR(predicted_pose.translation[2], 9, 0.001);
- EXPECT_NEAR(predicted_pose.right_translation[0], 4, 0.001);
- EXPECT_NEAR(predicted_pose.right_translation[1], 8, 0.001);
- EXPECT_NEAR(predicted_pose.right_translation[2], 12, 0.001);
-}
-
-} // namespace
-
-} // namespace dvr
-} // namespace android
diff --git a/libs/vr/libposepredictor/polynomial_pose_predictor.cpp b/libs/vr/libposepredictor/polynomial_predictor.cpp
similarity index 63%
rename from libs/vr/libposepredictor/polynomial_pose_predictor.cpp
rename to libs/vr/libposepredictor/polynomial_predictor.cpp
index 47eab8a..98fd28a 100644
--- a/libs/vr/libposepredictor/polynomial_pose_predictor.cpp
+++ b/libs/vr/libposepredictor/polynomial_predictor.cpp
@@ -1,7 +1,6 @@
-#include <private/dvr/polynomial_pose_predictor.h>
+#include <polynomial_predictor.h>
-namespace android {
-namespace dvr {
+namespace posepredictor {
// Instantiate the common polynomial types.
template class PolynomialPosePredictor<1, 2>;
@@ -9,5 +8,4 @@
template class PolynomialPosePredictor<3, 4>;
template class PolynomialPosePredictor<4, 5>;
-} // namespace dvr
-} // namespace android
+} // namespace posepredictor
diff --git a/libs/vr/libposepredictor/polynomial_predictor_tests.cpp b/libs/vr/libposepredictor/polynomial_predictor_tests.cpp
new file mode 100644
index 0000000..88cb2b9
--- /dev/null
+++ b/libs/vr/libposepredictor/polynomial_predictor_tests.cpp
@@ -0,0 +1,120 @@
+#include <gtest/gtest.h>
+
+#include <polynomial_predictor.h>
+
+namespace posepredictor {
+
+namespace {
+
+// For comparing expected and actual.
+constexpr real kAbsErrorTolerance = 1e-5;
+
+// Test the linear extrapolation from two samples.
+TEST(PolynomialPosePredictor, Linear) {
+ // Degree = 1, simple line, passing through two points.
+ // Note the regularization is 0 so we expect the exact fit.
+ PolynomialPosePredictor<1, 2> predictor(0);
+
+ // Add two samples.
+ predictor.Add(
+ Pose{.position = {0, 0, 0}, .orientation = {0, 0, 0, 1}, .time_ns = 0});
+
+ predictor.Add(
+ Pose{.position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 10});
+
+ Pose predicted_pose;
+
+ predicted_pose = predictor.Predict(20);
+
+ // Check the x,y,z components for the expected translation.
+ EXPECT_NEAR(predicted_pose.position[0], 2, kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[1], 4, kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[2], 6, kAbsErrorTolerance);
+
+ predicted_pose = predictor.Predict(30);
+ EXPECT_NEAR(predicted_pose.position[0], 3, kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[1], 6, kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[2], 9, kAbsErrorTolerance);
+}
+
+// Test the degree two polynomial fit.
+TEST(PolynomialPosePredictor, Quadric) {
+ // Degree = 2, need three samples to fit a polynomial.
+ // Note the regularization is 0 so we expect the exact fit.
+ PolynomialPosePredictor<2, 3> predictor(0);
+
+ // Add three samples.
+ predictor.Add(
+ Pose{.position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 0});
+
+ predictor.Add(
+ Pose{.position = {0, 0, 0}, .orientation = {0, 0, 0, 1}, .time_ns = 10});
+
+ predictor.Add(
+ Pose{.position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 20});
+
+ // The expected polynomials for x/y/z.
+
+ // x: 0.01 * t^2 - 0.2 * t + 1
+ const auto x = [](auto t) { return 0.01 * t * t - 0.2 * t + 1; };
+
+ // y: 0.02 * t^2 - 0.4 * t + 2
+ const auto y = [](auto t) { return 0.02 * t * t - 0.4 * t + 2; };
+
+ // z: 0.03 * t^2 - 0.6 * t + 3
+ const auto z = [](auto t) { return 0.03 * t * t - 0.6 * t + 3; };
+
+ Pose predicted_pose;
+ predicted_pose = predictor.Predict(40);
+
+ // Check the x,y,z components for the expected translation.
+ EXPECT_NEAR(predicted_pose.position[0], x(40), kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[1], y(40), kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[2], z(40), kAbsErrorTolerance);
+
+ predicted_pose = predictor.Predict(50);
+ EXPECT_NEAR(predicted_pose.position[0], x(50), kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[1], y(50), kAbsErrorTolerance);
+ EXPECT_NEAR(predicted_pose.position[2], z(50), kAbsErrorTolerance);
+}
+
+// Test the degree two polynomial fit with degenerate input.
+//
+// The input samples all lie in a line which would normally make our system
+// degenerate. We will rely on the regularization term to recover the linear
+// solution in a quadric predictor.
+TEST(PolynomialPosePredictor, QuadricDegenate) {
+ // Degree = 2, need three samples to fit a polynomial.
+ // Note that we are using the default regularization term here.
+ // We cannot use 0 regularizer since the input is degenerate.
+ PolynomialPosePredictor<2, 3> predictor(1e-20);
+
+ // Add three samples.
+ predictor.Add(
+ Pose{.position = {0, 0, 0}, .orientation = {0, 0, 0, 1}, .time_ns = 0});
+
+ predictor.Add(
+ Pose{.position = {1, 2, 3}, .orientation = {0, 0, 0, 1}, .time_ns = 10});
+
+ predictor.Add(
+ Pose{.position = {2, 4, 6}, .orientation = {0, 0, 0, 1}, .time_ns = 20});
+
+ Pose predicted_pose;
+
+ predicted_pose = predictor.Predict(30);
+
+ // Check the x,y,z components for the expected translation.
+ // We are using a higher error threshold since this is now approximate.
+ EXPECT_NEAR(predicted_pose.position[0], 3, 0.001);
+ EXPECT_NEAR(predicted_pose.position[1], 6, 0.001);
+ EXPECT_NEAR(predicted_pose.position[2], 9, 0.001);
+
+ predicted_pose = predictor.Predict(40);
+ EXPECT_NEAR(predicted_pose.position[0], 4, 0.001);
+ EXPECT_NEAR(predicted_pose.position[1], 8, 0.001);
+ EXPECT_NEAR(predicted_pose.position[2], 12, 0.001);
+}
+
+} // namespace
+
+} // namespace posepredictor
diff --git a/libs/vr/libposepredictor/pose_predictor.cpp b/libs/vr/libposepredictor/pose_predictor.cpp
deleted file mode 100644
index b09a152..0000000
--- a/libs/vr/libposepredictor/pose_predictor.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#include <private/dvr/pose_predictor.h>
-
-namespace android {
-namespace dvr {
-
-vec3d PosePredictor::AngularVelocity(const quatd& a, const quatd& b,
- double delta_time) {
- const auto delta_q = b.inverse() * a;
- // 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 ? -delta_q.vec() : delta_q.vec()) / delta_time;
-}
-
-void PosePredictor::InitializeFromSample(const Sample& sample,
- DvrPoseAsync* out_pose,
- const vec3d& velocity,
- const vec3d& angular_velocity) {
- out_pose->orientation = {static_cast<float>(sample.orientation.vec().x()),
- static_cast<float>(sample.orientation.vec().y()),
- static_cast<float>(sample.orientation.vec().z()),
- static_cast<float>(sample.orientation.w())};
-
- out_pose->translation = {static_cast<float>(sample.position.x()),
- static_cast<float>(sample.position.y()),
- static_cast<float>(sample.position.z()), 0.0f};
-
- out_pose->right_orientation = {
- static_cast<float>(sample.orientation.vec().x()),
- static_cast<float>(sample.orientation.vec().y()),
- static_cast<float>(sample.orientation.vec().z()),
- static_cast<float>(sample.orientation.w())};
-
- out_pose->right_translation = {static_cast<float>(sample.position.x()),
- static_cast<float>(sample.position.y()),
- static_cast<float>(sample.position.z()), 0.0f};
-
- out_pose->angular_velocity = {static_cast<float>(angular_velocity.x()),
- static_cast<float>(angular_velocity.y()),
- static_cast<float>(angular_velocity.z()), 0.0f};
-
- out_pose->velocity = {static_cast<float>(velocity.x()),
- static_cast<float>(velocity.y()),
- static_cast<float>(velocity.z()), 0.0f};
- out_pose->timestamp_ns = sample.time_ns;
- out_pose->flags = DVR_POSE_FLAG_HEAD | DVR_POSE_FLAG_VALID;
- memset(out_pose->pad, 0, sizeof(out_pose->pad));
-}
-
-} // namespace dvr
-} // namespace android
diff --git a/libs/vr/libposepredictor/predictor.cpp b/libs/vr/libposepredictor/predictor.cpp
new file mode 100644
index 0000000..266e7ef
--- /dev/null
+++ b/libs/vr/libposepredictor/predictor.cpp
@@ -0,0 +1,34 @@
+#include <linear_predictor.h>
+#include <polynomial_predictor.h>
+#include <predictor.h>
+
+namespace posepredictor {
+
+vec3 Predictor::AngularVelocity(const quat& a, const quat& b, real delta_time) {
+ const auto delta_q = b.inverse() * a;
+ // 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 ? -delta_q.vec() : delta_q.vec()) / delta_time;
+}
+
+Velocity Predictor::PredictVelocity(int64_t time_ns) const {
+ const auto a = Predict(time_ns - kFiniteDifferenceNs);
+ const auto b = Predict(time_ns + kFiniteDifferenceNs);
+ const auto delta_time = NsToSeconds(2 * kFiniteDifferenceNs);
+
+ return {(b.position - a.position) / delta_time,
+ AngularVelocity(a.orientation, b.orientation, delta_time)};
+}
+
+// The factory method.
+std::unique_ptr<Predictor> Predictor::Create(PredictorType type) {
+ switch (type) {
+ case PredictorType::Linear:
+ return std::make_unique<LinearPosePredictor>();
+ case PredictorType::Quadric:
+ return std::make_unique<QuadricPosePredictor>();
+ case PredictorType::Cubic:
+ return std::make_unique<CubicPosePredictor>();
+ }
+}
+} // namespace posepredictor
diff --git a/libs/vr/libposepredictor/pose_predictor_tests.cpp b/libs/vr/libposepredictor/predictor_tests.cpp
similarity index 68%
rename from libs/vr/libposepredictor/pose_predictor_tests.cpp
rename to libs/vr/libposepredictor/predictor_tests.cpp
index 1e58b11..e84a93a 100644
--- a/libs/vr/libposepredictor/pose_predictor_tests.cpp
+++ b/libs/vr/libposepredictor/predictor_tests.cpp
@@ -1,36 +1,35 @@
#include <gtest/gtest.h>
-#include <private/dvr/pose_predictor.h>
+#include <predictor.h>
-namespace android {
-namespace dvr {
+namespace posepredictor {
namespace {
// For comparing expected and actual.
-constexpr double kAbsErrorTolerance = 1e-4;
+constexpr real kAbsErrorTolerance = 1e-4;
// Test the angular velocity computation from two orientations.
TEST(PosePredictor, AngularVelocity) {
// Some random rotation axis we will rotate around.
- const vec3d kRotationAxis = vec3d(1, 2, 3).normalized();
+ const vec3 kRotationAxis = vec3(1, 2, 3).normalized();
// Some random angle we will be rotating by.
- const double kRotationAngle = M_PI / 30;
+ const real kRotationAngle = M_PI / 30;
// Random start orientation we are currently at.
- const quatd kStartOrientation = quatd(5, 3, 4, 1).normalized();
+ const quat kStartOrientation = quat(5, 3, 4, 1).normalized();
// The orientation we will end up at.
- const quatd kEndOrientation =
+ const quat kEndOrientation =
kStartOrientation *
- quatd(Eigen::AngleAxis<double>(kRotationAngle, kRotationAxis));
+ quat(Eigen::AngleAxis<real>(kRotationAngle, kRotationAxis));
// The delta time for going from start orientation to end.
- const float kDeltaTime = 1.0;
+ const real kDeltaTime = 1.0;
// Compute the angular velocity from start orientation to end.
- const auto angularVelocity = PosePredictor::AngularVelocity(
+ const auto angularVelocity = Predictor::AngularVelocity(
kStartOrientation, kEndOrientation, kDeltaTime);
// Extract the axis and the angular speed.
@@ -48,5 +47,4 @@
} // namespace
-} // namespace dvr
-} // namespace android
+} // namespace posepredictor
diff --git a/services/surfaceflinger/Android.mk b/services/surfaceflinger/Android.mk
index ec47c8a..24c68ec 100644
--- a/services/surfaceflinger/Android.mk
+++ b/services/surfaceflinger/Android.mk
@@ -189,10 +189,6 @@
LOCAL_INIT_RC := surfaceflinger.rc
-ifneq ($(ENABLE_CPUSETS),)
- LOCAL_CFLAGS += -DENABLE_CPUSETS
-endif
-
ifeq ($(TARGET_USES_HWC2),true)
LOCAL_CFLAGS += -DUSE_HWC2
endif
diff --git a/services/surfaceflinger/main_surfaceflinger.cpp b/services/surfaceflinger/main_surfaceflinger.cpp
index 53a63bd..f151087 100644
--- a/services/surfaceflinger/main_surfaceflinger.cpp
+++ b/services/surfaceflinger/main_surfaceflinger.cpp
@@ -45,12 +45,10 @@
set_sched_policy(0, SP_FOREGROUND);
-#ifdef ENABLE_CPUSETS
// Put most SurfaceFlinger threads in the system-background cpuset
// Keeps us from unnecessarily using big cores
// Do this after the binder thread pool init
- set_cpuset_policy(0, SP_SYSTEM);
-#endif
+ if (cpusets_enabled()) set_cpuset_policy(0, SP_SYSTEM);
// initialize before clients can connect
flinger->init();
diff --git a/services/vr/sensord/pose_service.cpp b/services/vr/sensord/pose_service.cpp
index 34bcccf..40eb21d 100644
--- a/services/vr/sensord/pose_service.cpp
+++ b/services/vr/sensord/pose_service.cpp
@@ -20,9 +20,7 @@
#include <pdx/default_transport/service_endpoint.h>
#include <private/dvr/benchmark.h>
#include <private/dvr/clock_ns.h>
-#include <private/dvr/linear_pose_predictor.h>
#include <private/dvr/platform_defines.h>
-#include <private/dvr/polynomial_pose_predictor.h>
#include <private/dvr/pose-ipc.h>
#include <private/dvr/sensor_constants.h>
#include <utils/Trace.h>
@@ -234,9 +232,11 @@
switch (property_get_int32(kPredictorTypeProp, 0)) {
case 1:
- pose_predictor_ = std::make_unique<QuadricPosePredictor>();
+ pose_predictor_ = posepredictor::Predictor::Create(
+ posepredictor::PredictorType::Quadric);
default:
- pose_predictor_ = std::make_unique<LinearPosePredictor>();
+ pose_predictor_ = posepredictor::Predictor::Create(
+ posepredictor::PredictorType::Linear);
}
enable_pose_recording_ = property_get_bool(kEnablePoseRecordProp, 0) == 1;
@@ -336,10 +336,8 @@
pose_timestamp = GetSystemClockNs() - 1;
// Feed the sample to the predictor
- pose_predictor_->Add(PosePredictor::Sample{.position = start_t_head,
- .orientation = start_q_head,
- .time_ns = pose_timestamp},
- &last_known_pose_);
+ AddPredictorPose(pose_predictor_.get(), start_t_head, start_q_head,
+ pose_timestamp, &last_known_pose_);
// Store one extra value, because the application is working on the next
// frame and expects the minimum count from that frame on.
@@ -361,9 +359,9 @@
// Make a pose prediction
if (enable_pose_prediction_) {
- pose_predictor_->Predict(target_time,
- target_time + right_eye_photon_offset_ns_,
- mapped_pose_buffer_->ring + index);
+ PredictPose(pose_predictor_.get(), target_time,
+ target_time + right_eye_photon_offset_ns_,
+ mapped_pose_buffer_->ring + index);
} else {
mapped_pose_buffer_->ring[index] = last_known_pose_;
}
diff --git a/services/vr/sensord/pose_service.h b/services/vr/sensord/pose_service.h
index 455f98a..899d5fb 100644
--- a/services/vr/sensord/pose_service.h
+++ b/services/vr/sensord/pose_service.h
@@ -12,7 +12,7 @@
#include <pdx/service.h>
#include <private/dvr/buffer_hub_client.h>
#include <private/dvr/pose_client_internal.h>
-#include <private/dvr/pose_predictor.h>
+#include <private/dvr/dvr_pose_predictor.h>
#include <private/dvr/ring_buffer.h>
#include "sensor_fusion.h"
@@ -118,7 +118,7 @@
bool enable_external_pose_ = false;
// The predictor to extrapolate pose samples.
- std::unique_ptr<PosePredictor> pose_predictor_;
+ std::unique_ptr<posepredictor::Predictor> pose_predictor_;
// Pose ring buffer.
std::shared_ptr<BufferProducer> ring_buffer_;