Merge "Fix pose predictor jank." into oc-dev

 public:

  PolynomialPosePredictor(real regularization = 1e-9)

      : BufferedPredictor(TrainingWindow), regularization_(regularization) {

    static_assert(PolynomialDegree + 1 >= TrainingWindow,

    static_assert(TrainingWindow >= PolynomialDegree + 1,

                  "Underconstrained polynomial regressor");

  }

namespace posepredictor {

vec3 Predictor::AngularVelocity(const quat& a, const quat& b, real delta_time) {

  const auto delta_q = b.inverse() * a;

  const auto delta_q = a.inverse() * b;

  // Check that delta_q.w() == 1, Eigen doesn't respect this convention. If

  // delta_q.w() == -1, we'll get the opposite velocity.

  return 2.0 * (delta_q.w() < 0 ? static_cast<vec3>(-delta_q.vec()) : delta_q.vec()) / delta_time;

sourceFiles = [

    "pose_client.cpp",

    "sensor_client.cpp",

    "latency_model.cpp",

]

includeFiles = [

#ifndef ANDROID_DVR_LATENCY_MODEL_H_

#define ANDROID_DVR_LATENCY_MODEL_H_

#include <vector>

namespace android {

namespace dvr {

// This class holds a rolling average of the sensor latency.

class LatencyModel {

 public:

  LatencyModel(size_t window_size, double weight_mass_in_window);

  ~LatencyModel() = default;

  void AddLatency(int64_t latency_ns);

  int64_t CurrentLatencyEstimate() const {

    return static_cast<int64_t>(rolling_average_);

  }

 private:

  // The rolling average of the latencies.

  double rolling_average_ = 0;

  // The alpha parameter for an exponential moving average.

  double alpha_;

};

}  // namespace dvr

}  // namespace android

#endif  // ANDROID_DVR_LATENCY_MODEL_H_

#include <private/dvr/latency_model.h>

#include <cmath>

namespace android {

namespace dvr {

LatencyModel::LatencyModel(size_t window_size, double weight_mass_in_window) {

  // Compute an alpha so the weight of the last window_size measurements is

  // weight_mass_in_window of the total weights.

  // The weight in a series of k measurements:

  // alpha + (1 + (1 - alpha) + (1 - alpha)^2 + ... (1 - alpha)^k-1)

  // = alpha x (1 - (1 - alpha) ^ k) / alpha

  // = 1 - (1 - alpha) ^ k

  // weight_mass_in_window = 1 - (1 - alpha) ^ k / lim_k->inf (1 - alpha) ^ k

  // weight_mass_in_window = 1 - (1 - alpha) ^ k / 1

  // 1 - weight_mass_in_window = (1 - alpha) ^ k

  // log(1 - weight_mass_in_window) = k * log(1 - alpha)

  // 10 ^ (log(1 - weight_mass_in_window) / k) = 1 - alpha

  // alpha = 1 - 10 ^ (log(1 - weight_mass_in_window) / k)

  // alpha = 1 - 10 ^ (log(1 - weight_mass_in_window) / window_size)

  alpha_ = 1 - std::pow(10.0, std::log10(1 - weight_mass_in_window) /

                                  static_cast<double>(window_size));

}

void LatencyModel::AddLatency(int64_t latency_ns) {

  rolling_average_ = latency_ns * alpha_ + rolling_average_ * (1 - alpha_);

}

}  // namespace dvr

}  // namespace android