Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 4a59097f authored by Okan Arikan's avatar Okan Arikan
Browse files

Refactor libposepredictor to be self-contained. 

The CL seems big but it is mainly moving stuff around to
make libposepredictor self contained.

libposepredictor/include/predictor.h exposes all functionality and
it only includes Eigen stuff.

Also adds a shared library build step to the makefile.

Bug: 35362498
Test: No functional change.
Change-Id: I0c850711ffde16ee75e4c88b09e47f761c100385
parent 1dc50630

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_