Merge "Remove libposepredictor." into oc-dev

// Copyright (C) 2008 The Android Open Source Project

//

// Licensed under the Apache License, Version 2.0 (the "License");

// you may not use this file except in compliance with the License.

// You may obtain a copy of the License at

//

//      http://www.apache.org/licenses/LICENSE-2.0

//

// Unless required by applicable law or agreed to in writing, software

// distributed under the License is distributed on an "AS IS" BASIS,

// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

// See the License for the specific language governing permissions and

// limitations under the License.

sourceFiles = [

    "predictor.cpp",

    "buffered_predictor.cpp",

    "linear_predictor.cpp",

    "polynomial_predictor.cpp",

    "dvr_pose_predictor.cpp",

]

includeFiles = [

    "include",

]

staticLibraries = ["libvrsensor"]

sharedLibraries = []

headerLibraries = [ "libeigen" ]

cc_library {

    srcs: sourceFiles,

    cflags: [

      "-DLOG_TAG=\"libposepredictor\"",

      "-DTRACE=0",

    ],

    export_include_dirs: includeFiles,

    static_libs: staticLibraries,

    shared_libs: sharedLibraries,

    header_libs: headerLibraries,

    export_header_lib_headers: headerLibraries,

    name: "libposepredictor",

}

cc_test {

    tags: ["optional"],

    srcs: [

        "predictor_tests.cpp",

        "linear_predictor_tests.cpp",

        "polynomial_predictor_tests.cpp",

    ],

    static_libs: ["libposepredictor"] + staticLibraries,

    shared_libs: sharedLibraries,

    name: "pose_predictor_tests",

}

#include <buffered_predictor.h>

namespace posepredictor {

BufferedPredictor::BufferedPredictor(size_t buffer_size) {

  buffer_.resize(buffer_size);

}

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

  // counter.

  if (sample.time_ns != prev_sample.time_ns) {

    current_pose_index_ = (current_pose_index_ + 1) % buffer_.size();

  }

  buffer_[current_pose_index_] = sample;

  // Make sure the subsequent orientations are the closest in quaternion space.

  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 =

        quat(-sample.orientation.w(), -sample.orientation.x(),

             -sample.orientation.y(), -sample.orientation.z());

  }

  ++num_poses_added_;

}

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 posepredictor

#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

#ifndef POSEPREDICTOR_BUFFERED_PREDICTOR_H_

#define POSEPREDICTOR_BUFFERED_PREDICTOR_H_

#include <vector>

#include "predictor.h"

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 Predictor {

 public:

  BufferedPredictor(size_t buffer_size);

  ~BufferedPredictor() = default;

 protected:

  // Add a pose sample into the buffer.

  void BufferSample(const Pose& sample);

  // Grab a previous sample.

  // index = 0: last sample

  // index = 1: the one before that

  // ...

  const Pose& PrevSample(size_t index) const;

  // Where we keep the last n poses.

  std::vector<Pose> buffer_;

  // Where the last valid pose is in the buffer.

  size_t current_pose_index_ = 0;

  // The number of poses we have added.

  size_t num_poses_added_ = 0;

};

}  // namespace posepredictor

#endif  // POSEPREDICTOR_BUFFERED_PREDICTOR_H_

#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_