Merge "Refactor libposepredictor to be self-contained." (4257b1ee) · Commits · e / os / android_frameworks_native-old

libs/vr/libposepredictor/Android.mk

+9 −10

Original line number	Diff line number	Diff line
		@@ -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_SHARED_LIBRARIES := $(sharedLibraries)
		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)

libs/vr/libposepredictor/buffered_predictor.cpp

+7 −9

Original line number	Diff line number	Diff line
		#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 @@ void BufferedPredictor::BufferSample(const Sample& sample) {
		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(),
		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

libs/vr/libposepredictor/dvr_pose_predictor.cpp

0 → 100644

+70 −0

Original line number	Diff line number	Diff line
		#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

libs/vr/libposepredictor/include/private/dvr/buffered_predictor.h→libs/vr/libposepredictor/include/buffered_predictor.h

+10 −12

Original line number	Diff line number	Diff line
		#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 @@ class BufferedPredictor : public PosePredictor {
		size_t num_poses_added_ = 0;
		};

		} // namespace dvr
		} // namespace android
		} // namespace posepredictor

		#endif // ANDROID_DVR_BUFFERED_PREDICTOR_H_
		#endif // POSEPREDICTOR_BUFFERED_PREDICTOR_H_

libs/vr/libposepredictor/include/private/dvr/linear_pose_predictor.h→libs/vr/libposepredictor/include/linear_predictor.h

+43 −0

Original line number	Diff line number	Diff line
		#ifndef ANDROID_DVR_POSE_PREDICTOR_H_
		#define ANDROID_DVR_POSE_PREDICTOR_H_
		#ifndef POSEPREDICTOR_LINEAR_POSE_PREDICTOR_H_
		#define POSEPREDICTOR_LINEAR_POSE_PREDICTOR_H_

		#include <private/dvr/pose_predictor.h>
		#include "predictor.h"

		namespace android {
		namespace dvr {
		namespace posepredictor {

		// This class makes a linear prediction using the last two samples we received.
		class LinearPosePredictor : public PosePredictor {
		class LinearPosePredictor : public Predictor {
		public:
		LinearPosePredictor() = default;

		// Add a new sample.
		void Add(const Sample& sample, DvrPoseAsync* out_pose) override;
		void Add(const Pose& sample) override;

		// Predict using the last two samples.
		void Predict(int64_t left_time_ns, int64_t right_time_ns,
		DvrPoseAsync* out_pose) const override;
		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.
		Sample samples_[2];
		Pose 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;
		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 dvr
		} // namespace android
		} // namespace posepredictor

		#endif // ANDROID_DVR_POSE_PREDICTOR_H_
		#endif // POSEPREDICTOR_LINEAR_POSE_PREDICTOR_H_