// Copyright 2019 The MediaPipe Authors.
//
// 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.

#include "mediapipe/util/tracking/tracking.h"

#include <algorithm>
#include <cmath>
#include <memory>
#include <numeric>
#include <random>
#include <unordered_set>

#include "Eigen/Core"
#include "Eigen/Dense"
#include "Eigen/SVD"
#include "absl/algorithm/container.h"
#include "absl/memory/memory.h"
#include "mediapipe/framework/port/logging.h"
#include "mediapipe/framework/port/opencv_calib3d_inc.h"
#include "mediapipe/framework/port/opencv_core_inc.h"
#include "mediapipe/framework/port/opencv_imgproc_inc.h"
#include "mediapipe/util/tracking/flow_packager.pb.h"
#include "mediapipe/util/tracking/measure_time.h"
#include "mediapipe/util/tracking/motion_models.h"

namespace mediapipe {

bool MotionBox::print_motion_box_warnings_ = true;

namespace {

static constexpr int kNormalizationGridSize = 10;
constexpr float kShortScale = 16383.0f;
constexpr float kInvShortScale = 1.0f / kShortScale;

// Motion vectors with weights larger than kMinInlierWeight are classified as
// inliers.
constexpr float kMinInlierWeight = 0.5f;
// Motion vectors with weights smaller han kMaxOutlierWeight are classified as
// outliers.
constexpr float kMaxOutlierWeight = 0.1f;

// Lexicographic (first x, then y) comparator for MotionVector::pos.
struct MotionVectorComparator {
  bool operator()(const MotionVector& lhs, const MotionVector& rhs) const {
    return (lhs.pos.x() < rhs.pos.x()) ||
           (lhs.pos.x() == rhs.pos.x() && lhs.pos.y() < rhs.pos.y());
  }
};

void StoreInternalState(const std::vector<const MotionVector*>& vectors,
                        const std::vector<float>& inlier_weights,
                        float aspect_ratio, MotionBoxInternalState* internal) {
  const int num_vectors = vectors.size();
  CHECK_EQ(num_vectors, inlier_weights.size());

  float scale_x = 1.0f;
  float scale_y = 1.0f;
  ScaleFromAspect(aspect_ratio, true, &scale_x, &scale_y);

  internal->Clear();
  for (int k = 0; k < num_vectors; ++k) {
    internal->add_pos_x(vectors[k]->pos.x() * scale_x);
    internal->add_pos_y(vectors[k]->pos.y() * scale_y);
    internal->add_dx(vectors[k]->object.x() * scale_x);
    internal->add_dy(vectors[k]->object.y() * scale_y);
    internal->add_camera_dx(vectors[k]->background.x() * scale_x);
    internal->add_camera_dy(vectors[k]->background.y() * scale_y);
    internal->add_track_id(vectors[k]->track_id);
    internal->add_inlier_score(inlier_weights[k]);
  }
}

// protolite compatible MotionBoxState_TrackStatus_Name
std::string TrackStatusToString(MotionBoxState::TrackStatus status) {
  switch (status) {
    case MotionBoxState::BOX_UNTRACKED:
      return "BOX_UNTRACKED";
    case MotionBoxState::BOX_EMPTY:
      return "BOX_EMPTY";
    case MotionBoxState::BOX_NO_FEATURES:
      return "BOX_NO_FEATURES";
    case MotionBoxState::BOX_TRACKED:
      return "BOX_TRACKED";
    case MotionBoxState::BOX_DUPLICATED:
      return "BOX_DUPLICATED";
    case MotionBoxState::BOX_TRACKED_OUT_OF_BOUND:
      return "BOX_TRACKED_OUT_OF_BOUND";
  }
  LOG(FATAL) << "Should not happen.";
  return "UNKNOWN";
}

void ClearInlierState(MotionBoxState* state) {
  state->clear_inlier_ids();
  state->clear_inlier_length();
  state->clear_inlier_id_match_pos();
  state->clear_outlier_ids();
  state->clear_outlier_id_match_pos();
}

// Returns orthogonal error system from motion_vec scaled by irls_scale.
std::pair<Vector2_f, Vector2_f> ComputeIrlsErrorSystem(
    const Vector2_f& irls_scale, const Vector2_f& motion_vec) {
  Vector2_f irls_vec = motion_vec.Normalize();
  Vector2_f irls_vec_ortho = irls_vec.Ortho();
  return std::make_pair(irls_vec * irls_scale.x(),
                        irls_vec_ortho * irls_scale.y());
}

// Returns error for a given difference vector and error system.
float ErrorDiff(const Vector2_f& diff,
                const std::pair<Vector2_f, Vector2_f>& error_system) {
  // Error system is an orthogonal system of originally unit vectors that were
  // pre-multiplied by the corresponding irls scale.
  // One can think of this function here as L2 norm *after* scaling the whole
  // vector space w.r.t. the error system.
  //
  // In particular, we will project the vector diff onto this system and then
  // scale the magnitude along each direction with the corresponding irls scale.
  // As scalar multiplication is commutative with the dot product of vectors
  // pre-multiplication of the scale with the error system is sufficient.
  return Vector2_f(diff.DotProd(error_system.first),
                   diff.DotProd(error_system.second))
      .Norm();
}

// Returns true if point is within the inlier extent of the passed state (within
// small bound of 5% of frame diameter).
bool PointWithinInlierExtent(const Vector2_f pt, const MotionBoxState& state) {
  // No extent known, assume to be inside.
  if (state.prior_weight() == 0) {
    return true;
  }

  const float width_radius = state.inlier_width() * 0.55f;
  const float height_radius = state.inlier_height() * 0.55f;
  const float left = state.inlier_center_x() - width_radius;
  const float right = state.inlier_center_x() + width_radius;
  const float top = state.inlier_center_y() - height_radius;
  const float bottom = state.inlier_center_y() + height_radius;

  return pt.x() >= left && pt.x() <= right && pt.y() >= top && pt.y() <= bottom;
}

// Taken from MotionEstimation::LinearSimilarityL2SolveSystem.
bool LinearSimilarityL2Solve(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& weights, LinearSimilarityModel* model) {
  CHECK(model);
  if (motion_vectors.size() < 4) {
    LOG(ERROR) << "Requiring at least 4 input vectors for sufficient solve.";
    return false;
  }

  cv::Mat matrix(4, 4, CV_32F);
  cv::Mat solution(4, 1, CV_32F);
  cv::Mat rhs(4, 1, CV_32F);

  matrix.setTo(0);
  rhs.setTo(0);

  CHECK_EQ(motion_vectors.size(), weights.size());
  for (int k = 0; k < motion_vectors.size(); ++k) {
    const float x = motion_vectors[k]->pos.x();
    const float y = motion_vectors[k]->pos.y();
    const float w = weights[k];

    // double J[2 * 4] = {1, 0, x,  -y,
    //                    0, 1, y,   x};
    // Compute J^t * J * w = {1,  0,   x,    -y
    //                        0,  1,   y,     x,
    //                        x,  y,   xx+yy, 0,
    //                       -y,  x,   0,     xx+yy} * w;

    const float x_w = x * w;
    const float y_w = y * w;
    const float xx_yy_w = (x * x + y * y) * w;
    float* matrix_ptr = matrix.ptr<float>(0);
    matrix_ptr[0] += w;
    matrix_ptr[2] += x_w;
    matrix_ptr[3] += -y_w;

    matrix_ptr += 4;
    matrix_ptr[1] += w;
    matrix_ptr[2] += y_w;
    matrix_ptr[3] += x_w;

    matrix_ptr += 4;
    matrix_ptr[0] += x_w;
    matrix_ptr[1] += y_w;
    matrix_ptr[2] += xx_yy_w;

    matrix_ptr += 4;
    matrix_ptr[0] += -y_w;
    matrix_ptr[1] += x_w;
    matrix_ptr[3] += xx_yy_w;

    float* rhs_ptr = rhs.ptr<float>(0);

    const float m_x = motion_vectors[k]->object.x() * w;
    const float m_y = motion_vectors[k]->object.y() * w;

    rhs_ptr[0] += m_x;
    rhs_ptr[1] += m_y;
    rhs_ptr[2] += x * m_x + y * m_y;
    rhs_ptr[3] += -y * m_x + x * m_y;
  }

  // Solution parameters p.
  if (cv::solve(matrix, rhs, solution)) {
    const float* ptr = solution.ptr<float>(0);
    model->set_dx(ptr[0]);
    model->set_dy(ptr[1]);
    model->set_a(ptr[2] + 1.0);  // Identity parametrization.
    model->set_b(ptr[3]);
    return true;
  } else {
    return false;
  }
}

// Taken from MotionEstimation::HomographyL2NormalEquationSolve
bool HomographyL2Solve(const std::vector<const MotionVector*>& motion_vectors,
                       const std::vector<float>& weights, Homography* model) {
  CHECK(model);

  cv::Mat matrix(8, 8, CV_32F);
  cv::Mat solution(8, 1, CV_32F);
  cv::Mat rhs(8, 1, CV_32F);

  matrix.setTo(0);
  rhs.setTo(0);

  // Matrix multiplications are hand-coded for speed improvements vs.
  // opencv's cvGEMM calls.
  CHECK_EQ(motion_vectors.size(), weights.size());
  for (int k = 0; k < motion_vectors.size(); ++k) {
    const float x = motion_vectors[k]->pos.x();
    const float y = motion_vectors[k]->pos.y();
    const float w = weights[k];

    const float xw = x * w;
    const float yw = y * w;
    const float xxw = x * x * w;
    const float yyw = y * y * w;
    const float xyw = x * y * w;
    const float mx = x + motion_vectors[k]->object.x();
    const float my = y + motion_vectors[k]->object.y();

    const float mxxyy = mx * mx + my * my;
    // Jacobian
    // double J[2 * 8] = {x, y, 1,  0,  0,   0, -x * m_x, -y * m_x,
    //                   {0, 0, 0,  x,  y,   1, -x * m_y, -y * m_y}
    //
    // // Compute J^t * J * w =
    // ( xx        xy    x      0       0    0    -xx*mx  -xy*mx    )
    // ( xy        yy    y      0       0    0    -xy*mx  -yy*mx    )
    // ( x         y     1      0       0    0     -x*mx   -y*mx    )
    // ( 0         0     0     xx      xy    x    -xx*my  -xy*my    )
    // ( 0         0     0     xy      yy    y    -xy*my  -yy*my    )
    // ( 0         0     0      x      y     1     -x*my   -y*my    )
    // ( -xx*mx -xy*mx -x*mx -xx*my -xy*my -x*my xx*mxxyy  xy*mxxyy )
    // ( -xy*mx -yy*mx -y*mx -xy*my -yy*my -y*my xy*mxxyy  yy*mxxyy  ) * w

    // 1st row:  xx        xy    x      0       0    0    -xx*mx  -xy*mx
    float* matrix_ptr = matrix.ptr<float>(0);
    matrix_ptr[0] += xxw;
    matrix_ptr[1] += xyw;
    matrix_ptr[2] += xw;
    matrix_ptr[6] += -xxw * mx;
    matrix_ptr[7] += -xyw * mx;

    // 2nd row:  xy       yy   y      0       0    0    -xy*mx  -yy*mx
    matrix_ptr += 8;
    matrix_ptr[0] += xyw;
    matrix_ptr[1] += yyw;
    matrix_ptr[2] += yw;
    matrix_ptr[6] += -xyw * mx;
    matrix_ptr[7] += -yyw * mx;

    // 3rd row: x         y     1      0       0    0     -x*mx   -y*mx
    matrix_ptr += 8;
    matrix_ptr[0] += xw;
    matrix_ptr[1] += yw;
    matrix_ptr[2] += w;
    matrix_ptr[6] += -xw * mx;
    matrix_ptr[7] += -yw * mx;

    // 4th row: 0         0     0     xx      xy    x    -xx*my  -xy*my
    matrix_ptr += 8;
    matrix_ptr[3] += xxw;
    matrix_ptr[4] += xyw;
    matrix_ptr[5] += xw;
    matrix_ptr[6] += -xxw * my;
    matrix_ptr[7] += -xyw * my;

    // 5th row: 0         0     0     xy      yy    y    -xy*my  -yy*my
    matrix_ptr += 8;
    matrix_ptr[3] += xyw;
    matrix_ptr[4] += yyw;
    matrix_ptr[5] += yw;
    matrix_ptr[6] += -xyw * my;
    matrix_ptr[7] += -yyw * my;

    // 6th row:  0         0     0     x     y     1      -x*my    -y*my
    matrix_ptr += 8;
    matrix_ptr[3] += xw;
    matrix_ptr[4] += yw;
    matrix_ptr[5] += w;
    matrix_ptr[6] += -xw * my;
    matrix_ptr[7] += -yw * my;

    // 7th row:  -xx*mx -xy*mx -x*mx -xx*my -xy*my -x*my xx*mxxyy  xy*mxxyy
    matrix_ptr += 8;
    matrix_ptr[0] += -xxw * mx;
    matrix_ptr[1] += -xyw * mx;
    matrix_ptr[2] += -xw * mx;
    matrix_ptr[3] += -xxw * my;
    matrix_ptr[4] += -xyw * my;
    matrix_ptr[5] += -xw * my;
    matrix_ptr[6] += xxw * mxxyy;
    matrix_ptr[7] += xyw * mxxyy;

    // 8th row: -xy*mx -yy*mx -y*mx -xy*my -yy*my -y*my xy*mxxyy  yy*mxxyy
    matrix_ptr += 8;
    matrix_ptr[0] += -xyw * mx;
    matrix_ptr[1] += -yyw * mx;
    matrix_ptr[2] += -yw * mx;
    matrix_ptr[3] += -xyw * my;
    matrix_ptr[4] += -yyw * my;
    matrix_ptr[5] += -yw * my;
    matrix_ptr[6] += xyw * mxxyy;
    matrix_ptr[7] += yyw * mxxyy;

    // Right hand side:
    // b = ( x
    //       y )
    // Compute J^t * b  * w =
    // ( x*mx  y*mx  mx  x*my  y*my  my  -x*mxxyy -y*mxxyy ) * w

    float* rhs_ptr = rhs.ptr<float>(0);
    rhs_ptr[0] += xw * mx;
    rhs_ptr[1] += yw * mx;
    rhs_ptr[2] += mx * w;
    rhs_ptr[3] += xw * my;
    rhs_ptr[4] += yw * my;
    rhs_ptr[5] += my * w;
    rhs_ptr[6] += -xw * mxxyy;
    rhs_ptr[7] += -yw * mxxyy;
  }

  // Solution parameters p.
  if (cv::solve(matrix, rhs, solution)) {
    const float* ptr = solution.ptr<float>(0);
    *model = HomographyAdapter::FromFloatPointer(ptr, false);
    return true;
  }

  return false;
}

void TransformQuadInMotionBoxState(const MotionBoxState& curr_pos,
                                   const Homography& homography,
                                   MotionBoxState* next_pos) {
  CHECK(next_pos != nullptr);
  if (!curr_pos.has_pos_x() || !curr_pos.has_pos_y() || !curr_pos.has_width() ||
      !curr_pos.has_height()) {
    LOG(ERROR) << "Previous box does not exist, cannot transform!";
    return;
  }
  const int kQuadVerticesSize = 8;
  const MotionBoxState::Quad* curr_quad_ptr = nullptr;
  auto quad = absl::make_unique<MotionBoxState::Quad>();
  if (curr_pos.has_quad() &&
      curr_pos.quad().vertices_size() == kQuadVerticesSize) {
    curr_quad_ptr = &curr_pos.quad();
  } else {
    std::array<Vector2_f, 4> corners =
        GetCornersOfRotatedRect(curr_pos, Vector2_f(1.0f, 1.0f));
    for (const auto& vertex : corners) {
      quad->add_vertices(vertex.x());
      quad->add_vertices(vertex.y());
    }
    curr_quad_ptr = quad.get();
  }

  MotionBoxState::Quad* next_pos_quad = next_pos->mutable_quad();
  bool next_pos_quad_existed = true;
  if (next_pos_quad->vertices_size() != kQuadVerticesSize) {
    next_pos_quad_existed = false;
    next_pos_quad->clear_vertices();
  }
  for (int i = 0; i < kQuadVerticesSize / 2; ++i) {
    const auto& curr_pos_quad_vertex = Vector2_f(
        curr_quad_ptr->vertices(i * 2), curr_quad_ptr->vertices(i * 2 + 1));
    const auto& next_pos_quad_vertex_diff =
        HomographyAdapter::TransformPoint(homography, curr_pos_quad_vertex) -
        curr_pos_quad_vertex;
    if (next_pos_quad_existed) {
      next_pos_quad->set_vertices(i * 2, next_pos_quad->vertices(i * 2) +
                                             next_pos_quad_vertex_diff.x());
      next_pos_quad->set_vertices(
          i * 2 + 1,
          next_pos_quad->vertices(i * 2 + 1) + next_pos_quad_vertex_diff.y());
    } else {
      next_pos_quad->add_vertices(curr_pos_quad_vertex.x() +
                                  next_pos_quad_vertex_diff.x());
      next_pos_quad->add_vertices(curr_pos_quad_vertex.y() +
                                  next_pos_quad_vertex_diff.y());
    }
  }
}

void UpdateStatePositionAndSizeFromStateQuad(MotionBoxState* box_state) {
  Vector2_f top_left, bottom_right;
  MotionBoxBoundingBox(*box_state, &top_left, &bottom_right);
  box_state->set_width(bottom_right.x() - top_left.x());
  box_state->set_height(bottom_right.y() - top_left.y());
  box_state->set_pos_x(top_left.x());
  box_state->set_pos_y(top_left.y());
}

void ApplyCameraTrackingDegrees(const MotionBoxState& prev_state,
                                const Homography& background_model,
                                const TrackStepOptions& options,
                                const Vector2_f& domain,
                                MotionBoxState* next_state) {
  // Determine center translation.
  Vector2_f center(MotionBoxCenter(prev_state));
  const Vector2_f background_motion =
      HomographyAdapter::TransformPoint(background_model, center) - center;

  if (options.tracking_degrees() ==
          TrackStepOptions::TRACKING_DEGREE_TRANSLATION ||
      !options.track_object_and_camera()) {
    SetMotionBoxPosition(MotionBoxPosition(*next_state) + background_motion,
                         next_state);
    return;
  }

  // Transform corners and fit similarity.
  // Overall idea:
  // We got corners, x0, x1, x2, x3 of the rect in the previous location
  // transform by background_model H.
  // Assuming H = [A | t], their target location in the next frame
  // is:
  // xi' = A * xi + t for i = 0..3
  // We want to express the location of ci' w.r.t. to the translated center c,
  // to decouple H from the translation of the center.
  // In particular, we are looking for the translation of the center:
  // c* = c + t* and points
  // xi* = xi + t*
  // Express location of xi' w.r.t. c:
  // xi' = A(xi* - c*) + c*
  // Axi + t = A(xi - c) + c + t*
  // Axi + t = Axi - Ac + c + t*
  // t* = Ac - c + t
  std::array<Vector2_f, 4> corners = MotionBoxCorners(prev_state);
  std::vector<MotionVector> corner_vecs(4);
  std::vector<const MotionVector*> corner_vec_ptrs(4);

  for (int k = 0; k < 4; ++k) {
    MotionVector v;
    v.pos = corners[k];
    v.object = HomographyAdapter::TransformPoint(background_model, corners[k]) -
               corners[k];
    corner_vecs[k] = v;
    corner_vec_ptrs[k] = &corner_vecs[k];
  }

  LinearSimilarityModel linear_similarity;
  LinearSimilarityL2Solve(corner_vec_ptrs, std::vector<float>(4, 1.0f),
                          &linear_similarity);

  SimilarityModel similarity =
      LinearSimilarityAdapter::ToSimilarity(linear_similarity);

  // See above derivation, motion of the center is t* = Ac + t - c;
  // Could also make the point that background_model instead of
  // linear_similarity is more accurate here due to the fitting operation above.
  SetMotionBoxPosition(MotionBoxPosition(*next_state) +
                           TransformPoint(linear_similarity, center) - center,
                       next_state);

  switch (options.tracking_degrees()) {
    case TrackStepOptions::TRACKING_DEGREE_TRANSLATION:
      break;
    case TrackStepOptions::TRACKING_DEGREE_CAMERA_SCALE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_SCALE:
      next_state->set_scale(next_state->scale() * similarity.scale());
      break;
    case TrackStepOptions::TRACKING_DEGREE_CAMERA_ROTATION:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION:
      next_state->set_rotation(next_state->rotation() + similarity.rotation());
      break;
    case TrackStepOptions::TRACKING_DEGREE_CAMERA_ROTATION_SCALE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION_SCALE:
      next_state->set_scale(next_state->scale() * similarity.scale());
      next_state->set_rotation(next_state->rotation() + similarity.rotation());
      break;
    case TrackStepOptions::TRACKING_DEGREE_CAMERA_PERSPECTIVE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE:
      TransformQuadInMotionBoxState(prev_state, background_model, next_state);
      if (prev_state.has_pnp_homography()) {
        *(next_state->mutable_pnp_homography()) = HomographyAdapter::Compose(
            prev_state.pnp_homography(), background_model);
        UpdateStatePositionAndSizeFromStateQuad(next_state);
      }
      break;
  }
}

void ApplyObjectMotion(const MotionBoxState& curr_pos,
                       const Vector2_f& object_translation,
                       const LinearSimilarityModel& object_similarity,
                       const Homography& object_homography,
                       const TrackStepOptions& options,
                       MotionBoxState* next_pos) {
  switch (options.tracking_degrees()) {
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION_SCALE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_SCALE: {
      Vector2_f center(MotionBoxCenter(curr_pos));
      // See ApplyCameraTrackingDegrees for derivation.
      SetMotionBoxPosition(MotionBoxPosition(*next_pos) +
                               TransformPoint(object_similarity, center) -
                               center,
                           next_pos);
      SimilarityModel similarity =
          LinearSimilarityAdapter::ToSimilarity(object_similarity);
      if (options.tracking_degrees() !=
          TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION) {
        next_pos->set_scale(next_pos->scale() * similarity.scale());
      }
      if (options.tracking_degrees() !=
          TrackStepOptions::TRACKING_DEGREE_OBJECT_SCALE) {
        next_pos->set_rotation(next_pos->rotation() + similarity.rotation());
      }
      break;
    }

    case TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE: {
      Vector2_f center(MotionBoxCenter(curr_pos));
      SetMotionBoxPosition(
          MotionBoxPosition(*next_pos) +
              HomographyAdapter::TransformPoint(object_homography, center) -
              center,
          next_pos);
      TransformQuadInMotionBoxState(curr_pos, object_homography, next_pos);
      break;
    }
    default:
      // Use translation per default.
      SetMotionBoxPosition(MotionBoxPosition(*next_pos) + object_translation,
                           next_pos);
  }
}

bool IsBoxValid(const MotionBoxState& state) {
  const float kMaxBoxHeight =
      10000.0f;  // as relative to normalized [0, 1] space
  const float kMaxBoxWidth =
      10000.0f;  // as relative to normalized [0, 1] space
  if (state.width() > kMaxBoxWidth) {
    LOG(ERROR) << "box width " << state.width() << " too big";
    return false;
  }
  if (state.height() > kMaxBoxHeight) {
    LOG(ERROR) << "box height " << state.height() << " too big";
    return false;
  }

  return true;
}

Homography PnpHomographyFromRotationAndTranslation(const cv::Mat& rvec,
                                                   const cv::Mat& tvec) {
  cv::Mat homography_matrix(3, 3, CV_64F);
  cv::Rodrigues(rvec, homography_matrix);

  for (int c = 0; c < 3; ++c) {
    homography_matrix.at<double>(c, 2) = tvec.at<double>(c);
  }

  // check non zero
  homography_matrix /= homography_matrix.at<double>(2, 2);

  return HomographyAdapter::FromDoublePointer(homography_matrix.ptr<double>(0),
                                              false);
}

// Translate CameraIntrinsics proto to cv format.
void ConvertCameraIntrinsicsToCvMat(
    const TrackStepOptions::CameraIntrinsics& camera_intrinsics,
    cv::Mat* camera_mat, cv::Mat* dist_coef) {
  *camera_mat = cv::Mat::eye(3, 3, CV_64F);
  *dist_coef = cv::Mat::zeros(1, 5, CV_64FC1);
  camera_mat->at<double>(0, 0) = camera_intrinsics.fx();
  camera_mat->at<double>(1, 1) = camera_intrinsics.fy();
  camera_mat->at<double>(0, 2) = camera_intrinsics.cx();
  camera_mat->at<double>(1, 2) = camera_intrinsics.cy();
  dist_coef->at<double>(0) = camera_intrinsics.k0();
  dist_coef->at<double>(1) = camera_intrinsics.k1();
  dist_coef->at<double>(4) = camera_intrinsics.k2();
}

}  // namespace.

void ScaleFromAspect(float aspect, bool invert, float* scale_x,
                     float* scale_y) {
  *scale_x = aspect >= 1.0f ? 1.0f : aspect;
  *scale_y = aspect >= 1.0f ? 1.0f / aspect : 1.0f;
  if (invert) {
    *scale_x = 1.0f / *scale_x;
    *scale_y = 1.0f / *scale_y;
  }
}

std::array<Vector2_f, 4> MotionBoxCorners(const MotionBoxState& state,
                                          const Vector2_f& scaling) {
  std::array<Vector2_f, 4> transformed_corners;
  if (state.has_quad() && state.quad().vertices_size() == 8) {
    for (int k = 0; k < 4; ++k) {
      transformed_corners[k] = Vector2_f(state.quad().vertices(2 * k),
                                         state.quad().vertices(2 * k + 1))
                                   .MulComponents(scaling);
    }
  } else {
    transformed_corners = GetCornersOfRotatedRect(state, scaling);
  }

  return transformed_corners;
}

bool MotionBoxLines(const MotionBoxState& state, const Vector2_f& scaling,
                    std::array<Vector3_f, 4>* box_lines) {
  CHECK(box_lines);
  std::array<Vector2_f, 4> corners = MotionBoxCorners(state, scaling);
  std::array<Vector3_f, 4> lines;
  for (int k = 0; k < 4; ++k) {
    const Vector2_f diff = corners[(k + 1) % 4] - corners[k];
    const Vector2_f normal = diff.Ortho().Normalize();
    box_lines->at(k).Set(normal.x(), normal.y(), -normal.DotProd(corners[k]));
    // Double check that second point is on the computed line.
    if (box_lines->at(k).DotProd(Vector3_f(corners[(k + 1) % 4].x(),
                                           corners[(k + 1) % 4].y(), 1.0f)) >=
        0.02f) {
      LOG(ERROR) << "box is abnormal. Line equations don't satisfy constraint";
      return false;
    }
  }
  return true;
}

void MotionBoxBoundingBox(const MotionBoxState& state, Vector2_f* top_left,
                          Vector2_f* bottom_right) {
  CHECK(top_left);
  CHECK(bottom_right);

  std::array<Vector2_f, 4> corners = MotionBoxCorners(state);

  // Determine min and max across dimension.
  *top_left = Vector2_f(std::numeric_limits<float>::max(),
                        std::numeric_limits<float>::max());

  *bottom_right = Vector2_f(std::numeric_limits<float>::min(),
                            std::numeric_limits<float>::min());

  for (const Vector2_f& c : corners) {
    top_left->x(std::min(top_left->x(), c.x()));
    top_left->y(std::min(top_left->y(), c.y()));
    bottom_right->x(std::max(bottom_right->x(), c.x()));
    bottom_right->y(std::max(bottom_right->y(), c.y()));
  }
}

void MotionBoxInlierLocations(const MotionBoxState& state,
                              std::vector<Vector2_f>* inlier_pos) {
  CHECK(inlier_pos);
  inlier_pos->clear();
  for (int k = 0; k < state.inlier_id_match_pos_size(); k += 2) {
    inlier_pos->push_back(
        Vector2_f(state.inlier_id_match_pos(k) * kInvShortScale,
                  state.inlier_id_match_pos(k + 1) * kInvShortScale));
  }
}

void MotionBoxOutlierLocations(const MotionBoxState& state,
                               std::vector<Vector2_f>* outlier_pos) {
  CHECK(outlier_pos);
  outlier_pos->clear();
  for (int k = 0; k < state.outlier_id_match_pos_size(); k += 2) {
    outlier_pos->push_back(
        Vector2_f(state.outlier_id_match_pos(k) * kInvShortScale,
                  state.outlier_id_match_pos(k + 1) * kInvShortScale));
  }
}

std::array<Vector2_f, 4> GetCornersOfRotatedRect(const MotionBoxState& state,
                                                 const Vector2_f& scaling) {
  std::array<Vector2_f, 4> transformed_corners;
  // Scale and rotate 4 corner w.r.t. center.
  const Vector2_f center = MotionBoxCenter(state).MulComponents(scaling);
  const Vector2_f top_left = MotionBoxPosition(state).MulComponents(scaling);
  const std::array<Vector2_f, 4> corners{{
      top_left,
      top_left + Vector2_f(0, state.height() * scaling.y()),
      top_left +
          Vector2_f(state.width() * scaling.x(), state.height() * scaling.y()),
      top_left + Vector2_f(state.width() * scaling.x(), 0),
  }};

  const float cos_a = std::cos(state.rotation());
  const float sin_a = std::sin(state.rotation());
  for (int k = 0; k < 4; ++k) {
    // Scale and rotate w.r.t. center.
    const Vector2_f rad = corners[k] - center;
    const Vector2_f rot_rad(cos_a * rad.x() - sin_a * rad.y(),
                            sin_a * rad.x() + cos_a * rad.y());
    const Vector2_f transformed_corner = center + rot_rad * state.scale();
    transformed_corners[k] = transformed_corner;
  }

  return transformed_corners;
}

void InitializeQuadInMotionBoxState(MotionBoxState* state) {
  CHECK(state != nullptr);
  // Every quad has 4 vertices. Each vertex has x and y 2 coordinates. So
  // a total of 8 floating point values.
  if (state->quad().vertices_size() != 8) {
    MotionBoxState::Quad* quad_ptr = state->mutable_quad();
    quad_ptr->clear_vertices();
    std::array<Vector2_f, 4> corners =
        GetCornersOfRotatedRect(*state, Vector2_f(1.0f, 1.0f));
    for (const auto& vertex : corners) {
      quad_ptr->add_vertices(vertex.x());
      quad_ptr->add_vertices(vertex.y());
    }
  }
}

void InitializeInliersOutliersInMotionBoxState(const TrackingData& tracking,
                                               MotionBoxState* state) {
  MotionVectorFrame mvf;  // Holds motion from current to previous frame.
  MotionVectorFrameFromTrackingData(tracking, &mvf);

  std::array<Vector3_f, 4> box_lines;
  if (!MotionBoxLines(*state, Vector2_f(1.0f, 1.0f), &box_lines)) {
    LOG(ERROR) << "Error in computing MotionBoxLines.";
    return;
  }

  // scale for motion vectors.
  float scale_x, scale_y;
  ScaleFromAspect(mvf.aspect_ratio, true, &scale_x, &scale_y);

  state->clear_inlier_ids();
  state->clear_inlier_length();
  state->clear_outlier_ids();

  float inlier_center_x = 0.0f;
  float inlier_center_y = 0.0f;
  int cnt_inlier = 0;

  float min_x = std::numeric_limits<float>::max();
  float max_x = -std::numeric_limits<float>::max();
  float min_y = std::numeric_limits<float>::max();
  float max_y = -std::numeric_limits<float>::max();

  for (const auto& motion_vec : mvf.motion_vectors) {
    const float pos_x = motion_vec.pos.x() * scale_x;
    const float pos_y = motion_vec.pos.y() * scale_y;

    bool insider = true;

    for (const Vector3_f& line : box_lines) {
      if (line.DotProd(Vector3_f(pos_x, pos_y, 1.0f)) > 0.0f) {
        insider = false;
        break;
      }
    }

    if (insider) {
      ++cnt_inlier;
      inlier_center_x += pos_x;
      inlier_center_y += pos_y;

      min_x = std::min(min_x, pos_x);
      max_x = std::max(max_x, pos_x);
      min_y = std::min(min_y, pos_y);
      max_y = std::max(max_y, pos_y);

      state->add_inlier_ids(motion_vec.track_id);
      state->add_inlier_length(1.0f);
    } else {
      state->add_outlier_ids(motion_vec.track_id);
    }
  }

  if (cnt_inlier) {
    state->set_prior_weight(1.0f);
    state->set_inlier_center_x(inlier_center_x / cnt_inlier);
    state->set_inlier_center_y(inlier_center_y / cnt_inlier);
    state->set_inlier_width(max_x - min_x);
    state->set_inlier_height(max_y - min_y);
  }
}

void InitializePnpHomographyInMotionBoxState(
    const TrackingData& tracking, const TrackStepOptions& track_step_options,
    MotionBoxState* state) {
  // Only happen when `quad` and `aspect_ratio` are all specified.
  if (!state->has_quad()) {
    VLOG(1) << "Skip pnp tracking since box does not contain quad info.";
    return;
  }

  const int kQuadCornersSize = 4;
  CHECK_EQ(state->quad().vertices_size(), kQuadCornersSize * 2);
  float scale_x, scale_y;
  ScaleFromAspect(tracking.frame_aspect(), false, &scale_x, &scale_y);
  std::vector<cv::Point2f> corners_2d(kQuadCornersSize);
  if (track_step_options.has_camera_intrinsics()) {
    const auto& camera = track_step_options.camera_intrinsics();
    for (int c = 0; c < kQuadCornersSize; ++c) {
      corners_2d[c].x = state->quad().vertices(c * 2) * camera.w();
      corners_2d[c].y = state->quad().vertices(c * 2 + 1) * camera.h();
    }

    cv::Mat camera_mat, dist_coef;
    ConvertCameraIntrinsicsToCvMat(camera, &camera_mat, &dist_coef);
    cv::undistortPoints(corners_2d, corners_2d, camera_mat, dist_coef);
  } else {
    const float center_x = scale_x * 0.5f;
    const float center_y = scale_y * 0.5f;
    for (int c = 0; c < kQuadCornersSize; ++c) {
      corners_2d[c].x = state->quad().vertices(c * 2) * scale_x - center_x;
      corners_2d[c].y = state->quad().vertices(c * 2 + 1) * scale_y - center_y;
    }
  }

  if (!state->has_aspect_ratio()) {
    if (!track_step_options.forced_pnp_tracking()) {
      VLOG(1) << "Skip pnp tracking since aspect ratio is unknown and "
                 "estimation of it is not forced.";
      return;
    }
    const float u2_u0 = corners_2d[2].x - corners_2d[0].x;
    const float v2_v0 = corners_2d[2].y - corners_2d[0].y;
    const float u3_u1 = corners_2d[3].x - corners_2d[1].x;
    const float v3_v1 = corners_2d[3].y - corners_2d[1].y;

    constexpr float kEpsilon = 1e-6f;
    const float denominator = u2_u0 * v3_v1 - v2_v0 * u3_u1;
    if (std::abs(denominator) < kEpsilon) {
      LOG(WARNING) << "Zero denominator. Failed calculating aspect ratio.";
      return;
    }

    float s[kQuadCornersSize];
    s[0] = ((corners_2d[2].x - corners_2d[3].x) * v3_v1 -
            (corners_2d[2].y - corners_2d[3].y) * u3_u1) *
           2.0f / denominator;
    s[1] = -(u2_u0 * (corners_2d[2].y - corners_2d[3].y) -
             v2_v0 * (corners_2d[2].x - corners_2d[3].x)) *
           2.0f / denominator;
    s[2] = 2.0f - s[0];
    s[3] = 2.0f - s[1];

    std::vector<Vector3_f> corners(kQuadCornersSize);
    for (int i = 0; i < kQuadCornersSize; ++i) {
      if (s[0] <= 0) {
        LOG(WARNING) << "Negative scale. Failed calculating aspect ratio.";
        return;
      }
      corners[i] =
          Vector3_f(corners_2d[i].x * s[i], corners_2d[i].y * s[i], s[i]);
    }

    const Vector3_f width_edge = corners[2] - corners[1];
    const Vector3_f height_edge = corners[0] - corners[1];
    const float height_norm = height_edge.Norm();
    const float width_norm = width_edge.Norm();
    if (height_norm < kEpsilon || width_norm < kEpsilon) {
      LOG(WARNING)
          << "abnormal 3d quadrangle. Failed calculating aspect ratio.";
      return;
    }

    constexpr float kMaxCosAngle = 0.258819;  // which is cos(75 deg)
    if (width_edge.DotProd(height_edge) / height_norm / width_norm >
        kMaxCosAngle) {
      LOG(WARNING)
          << "abnormal 3d quadrangle. Failed calculating aspect ratio.";
      return;
    }

    state->set_aspect_ratio(width_norm / height_norm);
  }

  CHECK_GT(state->aspect_ratio(), 0.0f);

  const float half_width = state->aspect_ratio();
  const float half_height = 1.0f;
  const std::vector<cv::Point3f> corners_3d{
      cv::Point3f(-half_width, -half_height, 0.0f),
      cv::Point3f(-half_width, half_height, 0.0f),
      cv::Point3f(half_width, half_height, 0.0f),
      cv::Point3f(half_width, -half_height, 0.0f),
  };

  std::vector<MotionVector> motion_vectors(kQuadCornersSize);
  std::vector<const MotionVector*> motion_vector_pointers(kQuadCornersSize);

  for (int c = 0; c < kQuadCornersSize; ++c) {
    motion_vectors[c].pos = Vector2_f(corners_3d[c].x, corners_3d[c].y);
    motion_vectors[c].object =
        Vector2_f(corners_2d[c].x, corners_2d[c].y) - motion_vectors[c].pos;

    motion_vector_pointers[c] = &motion_vectors[c];
  }

  const std::vector<float> weights(kQuadCornersSize, 1.0f);
  HomographyL2Solve(motion_vector_pointers, weights,
                    state->mutable_pnp_homography());
}

// Scales velocity and all other velocity dependent fields according to
// temporal_scale.
void ScaleStateTemporally(float temporal_scale, MotionBoxState* state) {
  state->set_dx(state->dx() * temporal_scale);
  state->set_dy(state->dy() * temporal_scale);
  state->set_kinetic_energy(state->kinetic_energy() * temporal_scale);
}

void ScaleStateAspect(float aspect, bool invert, MotionBoxState* state) {
  float scale_x = 1.0f;
  float scale_y = 1.0f;
  ScaleFromAspect(aspect, invert, &scale_x, &scale_y);

  if (state->has_quad() && state->quad().vertices_size() == 8) {
    for (int i = 0; i < 4; ++i) {
      float curr_val = state->quad().vertices(i * 2);
      state->mutable_quad()->set_vertices(i * 2, curr_val * scale_x);
      curr_val = state->quad().vertices(i * 2 + 1);
      state->mutable_quad()->set_vertices(i * 2 + 1, curr_val * scale_y);
    }
  }

  state->set_pos_x(state->pos_x() * scale_x);
  state->set_pos_y(state->pos_y() * scale_y);
  state->set_width(state->width() * scale_x);
  state->set_height(state->height() * scale_y);
  state->set_dx(state->dx() * scale_x);
  state->set_dy(state->dy() * scale_y);
  state->set_inlier_center_x(state->inlier_center_x() * scale_x);
  state->set_inlier_center_y(state->inlier_center_y() * scale_y);
  state->set_inlier_width(state->inlier_width() * scale_x);
  state->set_inlier_height(state->inlier_height() * scale_y);
}

MotionVector MotionVector::FromInternalState(
    const MotionBoxInternalState& internal, int index) {
  CHECK_LT(index, internal.pos_x_size());
  MotionVector v;
  v.pos = Vector2_f(internal.pos_x(index), internal.pos_y(index));
  v.object = Vector2_f(internal.dx(index), internal.dy(index));
  v.background =
      Vector2_f(internal.camera_dx(index), internal.camera_dy(index));
  v.track_id = internal.track_id(index);
  return v;
}

void MotionBox::ResetAtFrame(int frame, const MotionBoxState& state) {
  states_.clear();
  queue_start_ = frame;

  states_.push_back(state);
  states_.back().set_track_status(MotionBoxState::BOX_TRACKED);
  // Init inlier dimensions from state if not set.
  if (states_.back().inlier_width() == 0 ||
      states_.back().inlier_height() == 0) {
    states_.back().set_inlier_width(state.width());
    states_.back().set_inlier_height(state.height());
  }

  initial_state_ = state;
}

bool MotionBox::TrackStep(int from_frame,
                          const MotionVectorFrame& motion_vectors,
                          bool forward) {
  if (!TrackableFromFrame(from_frame)) {
    LOG(WARNING) << "Tracking requested for initial position that is not "
                 << "trackable.";
    return false;
  }
  const int queue_pos = from_frame - queue_start_;

  MotionBoxState new_state;
  if (motion_vectors.is_duplicated) {
    // Do not track or update the state, just copy.
    new_state = states_[queue_pos];
    new_state.set_track_status(MotionBoxState::BOX_DUPLICATED);
  } else {
    // Compile history and perform tracking.
    std::vector<const MotionBoxState*> history;
    constexpr int kHistorySize = 10;
    if (forward) {
      for (int k = queue_pos - 1; k >= std::max(0, queue_pos - kHistorySize);
           --k) {
        history.push_back(&states_[k]);
      }
    } else {
      for (int k = queue_pos + 1;
           k <= std::min<int>(states_.size() - 1, queue_pos + kHistorySize);
           ++k) {
        history.push_back(&states_[k]);
      }
    }

    TrackStepImpl(from_frame, states_[queue_pos], motion_vectors, history,
                  &new_state);
  }

  if (new_state.track_status() < MotionBoxState::BOX_TRACKED) {
    new_state.set_tracking_confidence(0.0f);
  }
  if (!new_state.has_tracking_confidence()) {
    // In this case, track status should be >= MotionBoxState::BOX_TRACKED
    new_state.set_tracking_confidence(1.0f);
  }

  VLOG(1) << "Track status from frame " << from_frame << ": "
          << TrackStatusToString(new_state.track_status())
          << ". Has quad: " << new_state.has_quad();

  constexpr float kFailureDisparity = 0.8f;
  if (new_state.track_status() >= MotionBoxState::BOX_TRACKED) {
    if (forward) {
      const int new_pos = queue_pos + 1;
      if (new_pos < states_.size()) {
        states_[new_pos] = new_state;
      } else {
        states_.push_back(new_state);
      }

      // Check for successive tracking failures of in bound boxes.
      if (new_pos >= options_.max_track_failures()) {
        int num_track_errors = 0;
        // Cancel at the N + 1 tracking failure.
        for (int f = new_pos - options_.max_track_failures(); f <= new_pos;
             ++f) {
          if (states_[f].track_status() !=
              MotionBoxState::BOX_TRACKED_OUT_OF_BOUND) {
            num_track_errors += (fabs(states_[f].motion_disparity()) *
                                     states_[f].prior_weight() >
                                 kFailureDisparity);
          }
        }

        if (num_track_errors >= options_.max_track_failures()) {
          LOG_IF(INFO, print_motion_box_warnings_)
              << "Tracking failed during max track failure "
              << "verification.";
          states_[new_pos].set_track_status(MotionBoxState::BOX_UNTRACKED);
          return false;
        }
      }
    } else {
      // Backward tracking.
      int new_pos = queue_pos - 1;
      if (new_pos >= 0) {
        states_[new_pos] = new_state;
      } else {
        states_.push_front(new_state);
        --queue_start_;
        new_pos = 0;
      }

      // Check for successive tracking failures.
      if (new_pos + options_.max_track_failures() + 1 < states_.size()) {
        int num_track_errors = 0;
        // Cancel at the N + 1 tracking failure.
        for (int f = new_pos; f <= new_pos + options_.max_track_failures();
             ++f) {
          if (states_[f].track_status() !=
              MotionBoxState::BOX_TRACKED_OUT_OF_BOUND) {
            num_track_errors += (fabs(states_[f].motion_disparity()) *
                                     states_[f].prior_weight() >
                                 kFailureDisparity);
          }
        }

        if (num_track_errors >= options_.max_track_failures()) {
          LOG_IF(INFO, print_motion_box_warnings_)
              << "Tracking failed during max track failure "
              << "verification.";
          states_[new_pos].set_track_status(MotionBoxState::BOX_UNTRACKED);
          return false;
        }
      }
    }

    // Signal track success.
    return true;
  } else {
    LOG_IF(WARNING, print_motion_box_warnings_)
        << "Tracking error at " << from_frame
        << " status : " << TrackStatusToString(new_state.track_status());
    return false;
  }
}

namespace {

Vector2_f SpatialPriorPosition(const Vector2_f& location,
                               const MotionBoxState& state) {
  const int grid_size = state.spatial_prior_grid_size();
  return Vector2_f(
      Clamp((location.x() - state.pos_x()) / state.width(), 0, 1) *
          (grid_size - 1),
      Clamp((location.y() - state.pos_y()) / state.height(), 0, 1) *
          (grid_size - 1));
}

// Creates spatial prior for current set of inlier vectors and blends
// it with previous prior (based on blend_prior). If interpolate is set to true,
// uses more accurate interpolation into bins, instead of nearest neighbor
// interpolation. If use_next_position is set to true, the position in
// the next/previous frame is used instead of the current one.
void ComputeSpatialPrior(bool interpolate, bool use_next_position,
                         float blend_prior, MotionBoxState* update_pos) {
  const int grid_size = update_pos->spatial_prior_grid_size();

  std::vector<float> old_prior(update_pos->spatial_prior().begin(),
                               update_pos->spatial_prior().end());
  std::vector<float> old_confidence(update_pos->spatial_confidence().begin(),
                                    update_pos->spatial_confidence().end());

  CHECK_EQ(old_confidence.size(), old_prior.size());
  CHECK(old_confidence.empty() ||
        grid_size * grid_size == old_confidence.size())
      << "Empty or priors of constant size expected";

  update_pos->clear_spatial_prior();
  update_pos->clear_spatial_confidence();
  auto* spatial_prior = update_pos->mutable_spatial_prior();
  auto* spatial_confidence = update_pos->mutable_spatial_confidence();
  spatial_prior->Reserve(grid_size * grid_size);
  spatial_confidence->Reserve(grid_size * grid_size);

  for (int k = 0; k < grid_size * grid_size; ++k) {
    spatial_prior->AddAlreadyReserved(0);
    spatial_confidence->AddAlreadyReserved(0);
  }

  // Aggregate inlier weights (0 = outlier, 1 = total inlier) across grid.
  const MotionBoxInternalState& internal = update_pos->internal();
  const int num_elems = internal.pos_x_size();

  for (int k = 0; k < num_elems; ++k) {
    MotionVector vec = MotionVector::FromInternalState(internal, k);
    const Vector2_f pos =
        use_next_position ? vec.MatchLocation() : vec.Location();
    float weight = internal.inlier_score(k);

    const Vector2_f grid_pos = SpatialPriorPosition(pos, *update_pos);

    if (use_next_position) {
      // Check for out of bound and skip.
      if (grid_pos.x() < 0 || grid_pos.y() < 0 ||
          grid_pos.x() > update_pos->spatial_prior_grid_size() - 1 ||
          grid_pos.y() > update_pos->spatial_prior_grid_size() - 1) {
        continue;
      }
    }

    if (interpolate) {
      const int int_x = static_cast<int>(grid_pos.x());
      const int int_y = static_cast<int>(grid_pos.y());

      CHECK_GE(grid_pos.x(), 0) << pos.x() << ", " << update_pos->pos_x();
      CHECK_GE(grid_pos.y(), 0);
      CHECK_LE(grid_pos.x(), grid_size - 1);
      CHECK_LE(grid_pos.y(), grid_size - 1);

      const float dx = grid_pos.x() - int_x;
      const float dy = grid_pos.y() - int_y;
      const float dx_1 = 1.0f - dx;
      const float dy_1 = 1.0f - dy;
      const int stride = static_cast<int>(dx != 0);

      int grid_pos = int_y * grid_size + int_x;

      // Bilinear interpolation. Total sum of weights across all 4 additions
      // (for prior and confidence each), is one.
      *spatial_prior->Mutable(grid_pos) += dx_1 * dy_1 * weight;
      *spatial_confidence->Mutable(grid_pos) += dx_1 * dy_1;

      *spatial_prior->Mutable(grid_pos + stride) += dx * dy_1 * weight;
      *spatial_confidence->Mutable(grid_pos + stride) += dx * dy_1;

      grid_pos += (dy != 0) * grid_size;
      *spatial_prior->Mutable(grid_pos) += dx_1 * dy * weight;
      *spatial_confidence->Mutable(grid_pos) += dx_1 * dy;

      *spatial_prior->Mutable(grid_pos + stride) += dx * dy * weight;
      *spatial_confidence->Mutable(grid_pos + stride) += dx * dy;
    } else {
      // Nearest neighbor.
      const int grid_bin = static_cast<int>(grid_pos.y() + 0.5f) * grid_size +
                           static_cast<int>(grid_pos.x() + 0.5f);
      *spatial_prior->Mutable(grid_bin) += weight;
      *spatial_confidence->Mutable(grid_bin) += 1;
    }
  }

  // Normalize, i.e. max truncation.
  float total_prior_difference = 0;
  float weight_sum = 0;
  for (int k = 0; k < grid_size * grid_size; ++k) {
    // Convert aggregated inlier weights to grid cell prior.
    // Here we consider a grid cell to be an inlier, if at least two
    // 2 inliers within that cell where found.
    *spatial_prior->Mutable(k) = std::min(1.0f, spatial_prior->Get(k) * 0.5f);
    *spatial_confidence->Mutable(k) =
        std::min(1.0f, spatial_confidence->Get(k) * 0.5f);

    if (!old_prior.empty()) {
      // Truncated error, consider a difference of 0.2 within normal
      // update range.
      const float difference = std::max<float>(
          0.f, fabs(update_pos->spatial_prior(k) - old_prior[k]) - 0.2f);
      // Weight error by confidence.
      total_prior_difference += difference * update_pos->spatial_confidence(k);
      weight_sum += update_pos->spatial_confidence(k);

      // Blend confidence with previous confidence.
      const float curr_confidence =
          update_pos->spatial_confidence(k) * (1.0f - blend_prior);
      const float prev_confidence = old_confidence[k] * blend_prior;

      float summed_confidence = curr_confidence + prev_confidence;
      const float denom =
          summed_confidence > 0 ? 1.0f / summed_confidence : 1.0f;

      // Update prior and confidence as weighted linear combination between
      // current and previous prior.
      *spatial_prior->Mutable(k) =
          (update_pos->spatial_prior(k) * curr_confidence +
           old_prior[k] * prev_confidence) *
          denom;

      *spatial_confidence->Mutable(k) =
          (update_pos->spatial_confidence(k) * curr_confidence +
           prev_confidence * prev_confidence) *
          denom;
    }
  }

  update_pos->set_prior_diff(std::sqrt(
      total_prior_difference * (weight_sum > 0 ? 1.0f / weight_sum : 1.0f)));
}

}  // namespace.

void MotionBox::GetStartPosition(const MotionBoxState& curr_pos,
                                 float aspect_ratio, float* expand_mag,
                                 Vector2_f* top_left,
                                 Vector2_f* bottom_right) const {
  CHECK(top_left);
  CHECK(bottom_right);
  CHECK(expand_mag);

  MotionBoxBoundingBox(curr_pos, top_left, bottom_right);

  if (curr_pos.has_pnp_homography()) {
    *expand_mag = 0.0f;
  } else {
    // Expaned box by the specified minimum expansion_size. For fast moving
    // objects, we ensure that magnitude is twice the box velocity, but not more
    // than 1/4 of the box diameter.
    *expand_mag = std::max(options_.expansion_size(),
                           std::min(MotionBoxSize(curr_pos).Norm() * 0.25f,
                                    MotionBoxVelocity(curr_pos).Norm() * 2.0f));
  }

  // Expansion magnitude is not non-uniformly scaled w.r.t. aspect ratio
  // to ensure inclusion test in GetVectorsAndWeights can assume uniform
  // explansion magnitude.
  const Vector2_f expand = Vector2_f(*expand_mag, *expand_mag);
  *top_left -= expand;
  *bottom_right += expand;
}

void MotionBox::GetSpatialGaussWeights(const MotionBoxState& box_state,
                                       const Vector2_f& inv_box_domain,
                                       float* spatial_gauss_x,
                                       float* spatial_gauss_y) const {
  CHECK(spatial_gauss_x);
  CHECK(spatial_gauss_y);

  // Space sigma depends on how much the tracked object fills the rectangle.
  // We get this information from the inlier extent of the previous
  // estimation.
  // Motivation: Choose sigma s such that the inlier domain equals 90% coverage.
  //             i.e. using z-score one sided of 95% = 1.65
  //             s * 1.65 = domain
  //          ==> s = domain / 1.65f
  const float space_sigma_x = std::max(
      options_.spatial_sigma(), box_state.inlier_width() * inv_box_domain.x() *
                                    0.5f * box_state.prior_weight() / 1.65f);
  const float space_sigma_y = options_.spatial_sigma();
  std::max(options_.spatial_sigma(), box_state.inlier_height() *
                                         inv_box_domain.y() * 0.5f *
                                         box_state.prior_weight() / 1.65f);

  *spatial_gauss_x = -0.5f / (space_sigma_x * space_sigma_x);
  *spatial_gauss_y = -0.5f / (space_sigma_y * space_sigma_y);
}

// Computes for each vector its 2D grid position for a grid spanning
// the domain top_left to bottom_right.
// Note: Passed vectors must lie within the domain or function will return false
// for error.
template <int kGridSize>
bool ComputeGridPositions(const Vector2_f& top_left,
                          const Vector2_f& bottom_right,
                          const std::vector<const MotionVector*>& vectors,
                          std::vector<Vector2_f>* grid_positions) {
  CHECK(grid_positions);

  // Slightly larger domain to avoid boundary issues.
  const Vector2_f inv_grid_domain(
      (1.0f - 1e-3f) / (bottom_right.x() - top_left.x()),
      (1.0f - 1e-3f) / (bottom_right.y() - top_left.y()));

  grid_positions->clear();
  grid_positions->reserve(vectors.size());
  for (const MotionVector* vec : vectors) {
    // Get grid position. Note grid is never rotated, but we only use it for
    // density estimation.
    const Vector2_f grid_pos =
        (vec->pos - top_left).MulComponents(inv_grid_domain) * (kGridSize - 1);
    if (grid_pos.x() < 0 || grid_pos.y() < 0 || grid_pos.x() > kGridSize ||
        grid_pos.y() > kGridSize) {
      return false;
    }

    grid_positions->push_back(grid_pos);
  }

  return true;
}

template <int kGridSize>
void AddToGrid(const Vector2_f& grid_pos, std::vector<float>* grid) {
  const float grid_x = grid_pos.x();
  const float grid_y = grid_pos.y();

  const int int_grid_x = grid_x;
  const int int_grid_y = grid_y;

  const float dx = grid_x - int_grid_x;
  const float dy = grid_y - int_grid_y;
  const float dxdy = dx * dy;
  const float dx_plus_dy = dx + dy;

  const int inc_x = dx != 0;
  const int inc_y = dy != 0;

  int bin_idx = int_grid_y * kGridSize + int_grid_x;
  // (1 - dx)(1 - dy) = 1 - (dx + dy) + dx*dy
  (*grid)[bin_idx] += 1 - dx_plus_dy + dxdy;
  // dx * (1 - dy) = dx - dxdy
  (*grid)[bin_idx + inc_x] += dx - dxdy;

  bin_idx += kGridSize * inc_y;
  // (1 - dx) * dy = dy - dxdy
  (*grid)[bin_idx] += dy - dxdy;
  (*grid)[bin_idx + inc_x] += dxdy;
}

template <int kGridSize>
float SampleFromGrid(const Vector2_f& grid_pos,
                     const std::vector<float>& grid) {
  const float grid_x = grid_pos.x();
  const float grid_y = grid_pos.y();

  const int int_grid_x = grid_x;
  const int int_grid_y = grid_y;

  const float dx = grid_x - int_grid_x;
  const float dy = grid_y - int_grid_y;
  const float dxdy = dx * dy;
  const float dx_plus_dy = dx + dy;
  int inc_x = dx != 0;
  int inc_y = dy != 0;

  float normalizer = 0;
  int bin_idx = int_grid_y * kGridSize + int_grid_x;

  // See above.
  normalizer += grid[bin_idx] * (1 - dx_plus_dy + dxdy);
  normalizer += grid[bin_idx + inc_x] * (dx - dxdy);

  bin_idx += kGridSize * inc_y;
  normalizer += grid[bin_idx] * (dy - dxdy);
  normalizer += grid[bin_idx + inc_x] * dxdy;

  const float inv_normalizer = normalizer > 0 ? 1.0f / normalizer : 0;
  // Density should always decrease weight; never increase.
  return std::min(1.0f, inv_normalizer);
}

MotionBox::DistanceWeightsComputer::DistanceWeightsComputer(
    const MotionBoxState& initial_state, const MotionBoxState& current_state,
    const TrackStepOptions& options) {
  tracking_degrees_ = options.tracking_degrees();
  const Vector2_f box_domain(current_state.width() * current_state.scale(),
                             current_state.height() * current_state.scale());
  CHECK_GT(box_domain.x(), 0.0f);
  CHECK_GT(box_domain.y(), 0.0f);
  inv_box_domain_ = Vector2_f(1.0f / box_domain.x(), 1.0f / box_domain.y());

  // Space sigma depends on how much the tracked object fills the rectangle.
  // We get this information from the inlier extent of the previous
  // estimation.
  // Motivation: Choose sigma s such that the inlier domain equals 90%
  // coverage.
  //             i.e. using z-score one sided of 95% = 1.65
  //             s * 1.65 = domain
  //          ==> s = domain / 1.65f
  const float space_sigma_x =
      std::max(options.spatial_sigma(),
               current_state.inlier_width() * inv_box_domain_.x() * 0.5f *
                   current_state.prior_weight() / 1.65f);

  const float space_sigma_y =
      std::max(options.spatial_sigma(),
               current_state.inlier_height() * inv_box_domain_.y() * 0.5f *
                   current_state.prior_weight() / 1.65f);

  spatial_gauss_x_ = -0.5f / (space_sigma_x * space_sigma_x);
  spatial_gauss_y_ = -0.5f / (space_sigma_y * space_sigma_y);

  if (tracking_degrees_ == TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION ||
      tracking_degrees_ ==
          TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION_SCALE) {
    cos_neg_a_ = std::cos(-current_state.rotation());
    sin_neg_a_ = std::sin(-current_state.rotation());
    if (std::abs(current_state.rotation()) > 0.01f) {
      is_large_rotation_ = true;
    }
  }

  // Compute box center as blend between geometric center and inlier center.
  constexpr float kMaxBoxCenterBlendWeight = 0.5f;
  box_center_ =
      Lerp(MotionBoxCenter(current_state), InlierCenter(current_state),
           std::min(kMaxBoxCenterBlendWeight, current_state.prior_weight()));
  if (tracking_degrees_ ==
      TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE) {
    CHECK(initial_state.has_quad());
    CHECK(current_state.has_quad());
    homography_ =
        ComputeHomographyFromQuad(current_state.quad(), initial_state.quad());
    box_center_transformed_ =
        HomographyAdapter::TransformPoint(homography_, box_center_);
  }
}

float MotionBox::DistanceWeightsComputer::ComputeDistanceWeight(
    const MotionVector& test_vector) {
  // Distance weighting.
  Vector2_f diff_center;
  if (tracking_degrees_ ==
      TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE) {
    Vector2_f test_vector_transformed =
        HomographyAdapter::TransformPoint(homography_, test_vector.pos);
    diff_center = test_vector_transformed - box_center_transformed_;
  } else {
    diff_center = test_vector.pos - box_center_;
    if (is_large_rotation_) {
      // Rotate difference vector to normalized domain.
      diff_center.Set(
          cos_neg_a_ * diff_center.x() - sin_neg_a_ * diff_center.y(),
          sin_neg_a_ * diff_center.x() + cos_neg_a_ * diff_center.y());
    }
  }

  const Vector2_f diff = diff_center.MulComponents(inv_box_domain_);
  // Regular gaussian with variance in each direction, assuming directions
  // are indpendent.
  float weight = std::exp(diff.x() * diff.x() * spatial_gauss_x_ +
                          diff.y() * diff.y() * spatial_gauss_y_);
  return weight;
}

Homography MotionBox::DistanceWeightsComputer::ComputeHomographyFromQuad(
    const MotionBoxState::Quad& src_quad,
    const MotionBoxState::Quad& dst_quad) {
  std::vector<float> src_quad_vec(8);
  std::vector<float> dst_quad_vec(8);
  for (int i = 0; i < 8; ++i) {
    src_quad_vec[i] = src_quad.vertices(i);
    dst_quad_vec[i] = dst_quad.vertices(i);
  }
  // Construct the matrix
  Eigen::Matrix<float, 8, 8> A = Eigen::Matrix<float, 8, 8>::Zero();
  for (int i = 0; i < 4; ++i) {
    const int r0 = 2 * i;
    const int r1 = 2 * i + 1;
    A(r0, 0) = src_quad_vec[r0];
    A(r0, 1) = src_quad_vec[r1];
    A(r0, 2) = 1.f;
    A(r0, 6) = -src_quad_vec[r0] * dst_quad_vec[r0];
    A(r0, 7) = -src_quad_vec[r1] * dst_quad_vec[r0];
    A(r1, 3) = src_quad_vec[r0];
    A(r1, 4) = src_quad_vec[r1];
    A(r1, 5) = 1.f;
    A(r1, 6) = -src_quad_vec[r0] * dst_quad_vec[r1];
    A(r1, 7) = -src_quad_vec[r1] * dst_quad_vec[r1];
  }

  // Map arrays to Eigen vectors without memcpy
  std::vector<float> homography_vector(8);
  Eigen::Map<Eigen::Matrix<float, 8, 1> > x(homography_vector.data());
  Eigen::Map<const Eigen::Matrix<float, 8, 1> > b(dst_quad_vec.data());

  x = A.fullPivLu().solve(b);
  Homography homography;
  homography.set_h_00(homography_vector[0]);
  homography.set_h_01(homography_vector[1]);
  homography.set_h_02(homography_vector[2]);
  homography.set_h_10(homography_vector[3]);
  homography.set_h_11(homography_vector[4]);
  homography.set_h_12(homography_vector[5]);
  homography.set_h_20(homography_vector[6]);
  homography.set_h_21(homography_vector[7]);
  return homography;
}

bool MotionBox::GetVectorsAndWeights(
    const std::vector<MotionVector>& motion_vectors, int start_idx, int end_idx,
    const Vector2_f& top_left, const Vector2_f& bottom_right,
    const MotionBoxState& box_state, bool valid_background_model,
    bool is_chunk_boundary, float temporal_scale, float expand_mag,
    const std::vector<const MotionBoxState*>& history,
    std::vector<const MotionVector*>* vectors, std::vector<float>* weights,
    int* number_of_good_prior, int* number_of_cont_inliers) const {
  CHECK(weights);
  CHECK(vectors);
  CHECK(number_of_good_prior);
  CHECK(number_of_cont_inliers);

  const int num_max_vectors = end_idx - start_idx;
  weights->clear();
  vectors->clear();
  weights->reserve(num_max_vectors);
  vectors->reserve(num_max_vectors);

  const Vector2_f box_domain(box_state.width() * box_state.scale(),
                             box_state.height() * box_state.scale());

  CHECK_GT(box_domain.x(), 0.0f);
  CHECK_GT(box_domain.y(), 0.0f);
  const Vector2_f inv_box_domain(1.0f / box_domain.x(), 1.0f / box_domain.y());

  // The four lines of the rotated and scaled box.
  std::array<Vector3_f, 4> box_lines;
  if (!MotionBoxLines(box_state, Vector2_f(1.0f, 1.0f), &box_lines)) {
    LOG(ERROR) << "Error in computing MotionBoxLines. Return 0 good inits and "
                  "continued inliers";
    return false;
  }

  // Get list of previous tracking inliers and outliers.
  // Ids are used for non-chunk boundaries (faster matching), locations
  // for chunk boundaries.
  std::unordered_map<int, int> inlier_ids;
  std::unordered_set<int> outlier_ids;
  std::vector<Vector2_f> inlier_locations;
  std::vector<Vector2_f> outlier_locations;

  if (!is_chunk_boundary) {
    MotionBoxInliers(box_state, &inlier_ids);
    MotionBoxOutliers(box_state, &outlier_ids);

    // Never map ids in history across a chunk boundary.
    for (const auto* state_ptr : history) {
      MotionBoxOutliers(*state_ptr, &outlier_ids);
    }
    // Why don't we build inlier map from a history of inliers?
    // It is unlikely we skip a feature as an inlier, it is either
    // consistently part of the motion model or it is not.
  } else {
    MotionBoxInlierLocations(box_state, &inlier_locations);
    MotionBoxOutlierLocations(box_state, &outlier_locations);
  }

  // Indicator for each vector, if inlier or outlier from prev. estimation.
  std::vector<uchar> is_inlier;
  std::vector<uchar> is_outlier;
  is_inlier.reserve(num_max_vectors);
  is_outlier.reserve(num_max_vectors);
  int num_cont_inliers = 0;

  // Approx. 2 pix at SD resolution.
  constexpr float kSqProximity = 2e-3 * 2e-3;

  for (int k = start_idx; k < end_idx; ++k) {
    // x is within bound due to sorting.
    const MotionVector& test_vector = motion_vectors[k];

    if (test_vector.pos.y() < top_left.y() ||
        test_vector.pos.y() > bottom_right.y()) {
      continue;
    }

    if (std::abs(box_state.rotation()) > 0.01f ||
        options_.tracking_degrees() ==
            TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE) {
      // Test also if vector is within transformed convex area.
      bool accepted = true;
      for (const Vector3_f& line : box_lines) {
        if (line.DotProd(Vector3_f(test_vector.pos.x(), test_vector.pos.y(),
                                   1.0f)) > expand_mag) {
          // Outside, reject.
          accepted = false;
          break;
        }
      }
      if (!accepted) {
        continue;
      }
    }

    vectors->push_back(&motion_vectors[k]);

    auto is_close_to_test_vector = [test_vector,
                                    kSqProximity](const Vector2_f v) -> bool {
      return (v - test_vector.pos).Norm2() < kSqProximity;
    };

    const bool is_inlier_flag =
        inlier_ids.find(test_vector.track_id) != inlier_ids.end() ||
        std::find_if(inlier_locations.begin(), inlier_locations.end(),
                     is_close_to_test_vector) != inlier_locations.end();
    num_cont_inliers += is_inlier_flag;

    const bool is_outlier_flag =
        outlier_ids.find(test_vector.track_id) != outlier_ids.end() ||
        std::find_if(outlier_locations.begin(), outlier_locations.end(),
                     is_close_to_test_vector) != outlier_locations.end();

    is_inlier.push_back(is_inlier_flag);
    is_outlier.push_back(is_outlier_flag);
  }

  CHECK_EQ(vectors->size(), is_inlier.size());
  CHECK_EQ(vectors->size(), is_outlier.size());

  const float prev_motion_mag = MotionBoxVelocity(box_state).Norm();

  // Try to lock on object again, if disparity is high.
  constexpr float kMinPriorMotionWeight = 0.2f;
  const float prior_motion_weight =
      std::max(kMinPriorMotionWeight, std::abs(box_state.motion_disparity())) *
      box_state.prior_weight();

  const float motion_sigma =
      std::max<float>(options_.min_motion_sigma(),
                      prev_motion_mag * options_.relative_motion_sigma());
  const float motion_gaussian_scale = -0.5f / (motion_sigma * motion_sigma);

  // Maps current kinetic energy to [0, 1] quantifying static (0) vs. moving (1)
  // object.
  // Map normalized thresholds to current frame period.
  const float low_kinetic_energy =
      options_.low_kinetic_energy() * temporal_scale;
  const float high_kinetic_energy =
      options_.high_kinetic_energy() * temporal_scale;
  const float kinetic_identity = LinearRamp(
      box_state.kinetic_energy(), low_kinetic_energy, high_kinetic_energy);
  int num_good_inits = 0;

  // Map number of continued inliers to score in [0, 1].
  const float cont_inlier_score = LinearRamp(num_cont_inliers, 10, 30);

  VLOG(1) << "GetVectorsAndWeights, found cont. inliers: " << num_cont_inliers
          << "  score: " << cont_inlier_score;

  DistanceWeightsComputer distance_weights_computer(initial_state_, box_state,
                                                    options_);
  for (int k = 0; k < vectors->size(); ++k) {
    const MotionVector& test_vector = *(*vectors)[k];

    float weight = distance_weights_computer.ComputeDistanceWeight(test_vector);

    if (valid_background_model) {
      const float motion_diff =
          fabs(prev_motion_mag - test_vector.object.Norm());
      const float motion_weight =
          std::exp(motion_gaussian_scale * motion_diff * motion_diff);

      // Blend with spatial weight, that is the higher the disparity
      // (i.e. we lost tracking, the more inclined we are to lock
      //       onto vectors of similar motion magnitude regardless of their
      //       position).
      // Note: One might feel inclined to always bias towards the previous
      // motion, by multiplying weight with motion_weight. This however fails
      // when tracking objects that start at rest and start moving.
      weight = Lerp(weight, motion_weight, prior_motion_weight);
    }

    // There are two kinds of vectors we are trying to balance here:
    // - inliers from previous estimation
    // - similar vectors
    //
    // Current strategy:
    // - For static objects: Boost inliers a lot, discount outliers a lot,
    // do not care about similar vectors.
    // - For moving objects: Boost inliers proportional to number of continued
    // inliers, discount outliers a lot, boost similar vectors and
    // actively downweight dis-similar objects.
    //
    // Motivation: Inliers are usually not very stable, so if not sufficient
    // have been continued prefer velocity over inliers for moving objects.

    // NOTE: Regarding additive vs. multiplicative weights. We need to
    // multiply the weight here. Adding the weight messes
    // with the gaussian spatial weighting which in turn makes it hard to lock
    // onto moving objects in the first place (as center is assumed to be
    // placed over moving objects, this helps distinguish initial foreground
    // and background).

    // Up-weighting of inlier vectors and vectors of similar motion.
    float upweight = 1.0f;
    if (is_inlier[k]) {
      // Previous track, boost weight significantly.
      //
      // NOTE: Regarding the amount of up-weighting: Long features are not
      // very stable on moving objects. Therefore only upweight strongly for
      // static objects.
      constexpr float kWeakMultiplier = 5.0f;
      constexpr float kStrongMultiplier = 20.0f;

      // Map 0 -> 1 and values >= 0.5 -> 0, because long features are not
      // very stable on moving objects. Therefore only upweight strongly for
      // static objects.
      const float kinetic_alpha =
          std::max(0.0f, 1.0f - 2.0f * kinetic_identity);

      // Choose strong multiplier only when kinetic_alpha OR inlier score
      // support it.
      const float multiplier = Lerp(kWeakMultiplier, kStrongMultiplier,
                                    std::max(cont_inlier_score, kinetic_alpha));
      upweight *= multiplier;
    }

    // Scale weight boost for moving objects by prior, i.e. modulate
    // strength of scale w.r.t. confidence.
    const float kin_scale = Lerp(1, 10, box_state.prior_weight());
    // 80% moving object weighted by prior. This weighting is biasing towards
    // moving object when the prior is low.
    if (kinetic_identity >= 0.8f * box_state.prior_weight() &&
        test_vector.object.Norm() > high_kinetic_energy && !is_outlier[k]) {
      // If we track a moving object, long tracks are less likely to be stable
      // due to appearance variations. In that case boost similar vectors.
      upweight *= 5.f * kin_scale;
    }

    float downweight = 1.0f;
    // Down-weighting of outlier vectors and vectors of different motion.
    if (!is_inlier[k]) {
      // Outlier.
      if (is_outlier[k]) {
        // Note: Outlier ids might overlap with inliers as outliers are built
        // from a history of frames.
        // *Always favor inliers over outliers*! Important to keep!!
        downweight *= 20.0f;
      }

      // Vectors of different motion, for 100% moving object downweight
      // vectors with small motion.
      if (kinetic_identity >= 1.0f * box_state.prior_weight() &&
          test_vector.object.Norm() < low_kinetic_energy) {
        downweight *= 2.f * kin_scale;
      }
    }

    // Cap any kind of up or down weighting so that no vector overwhelms
    // all others.
    const float kMaxWeight = 100.f;
    upweight = std::min(kMaxWeight, upweight);
    downweight = std::min(kMaxWeight, downweight);
    weight *= upweight / downweight;

    num_good_inits += (weight >= 0.1f);
    weights->push_back(weight);
  }

  const int num_vectors = vectors->size();
  CHECK_EQ(num_vectors, weights->size());

  const float weight_sum =
      std::accumulate(weights->begin(), weights->end(), 0.0f);

  // Normalize weights.
  if (weight_sum > 0) {
    const float inv_weight_sum = 1.0f / weight_sum;
    for (auto& weight : *weights) {
      weight *= inv_weight_sum;
    }
  }

  *number_of_good_prior = num_good_inits;
  *number_of_cont_inliers = num_cont_inliers;

  return true;
}

void MotionBox::TranslationIrlsInitialization(
    const std::vector<const MotionVector*>& vectors,
    const Vector2_f& irls_scale, std::vector<float>* weights) const {
  const int num_features = vectors.size();

  const auto& irls_options = options_.irls_initialization();
  if (!irls_options.activated() || !num_features) {
    return;
  }

  // Bool indicator which features agree with model in each round.
  // In case no RANSAC rounds are performed considered all features inliers.
  std::vector<uint8> best_features(num_features, 1);
  std::vector<uint8> curr_features(num_features);
  float best_sum = 0;

  unsigned int seed = 900913;
  std::default_random_engine rand_gen(seed);
  std::uniform_int_distribution<> distribution(0, num_features - 1);

  const float cutoff = irls_options.cutoff();
  const float sq_cutoff = cutoff * cutoff;

  for (int rounds = 0; rounds < irls_options.rounds(); ++rounds) {
    float curr_sum = 0;
    // Pick a random vector.
    const int rand_idx = distribution(rand_gen);
    const Vector2_f flow = vectors[rand_idx]->object;
    const auto error_system = ComputeIrlsErrorSystem(irls_scale, flow);

    // curr_features gets set for every feature below; no need to reset.
    for (int i = 0; i < num_features; ++i) {
      const Vector2_f diff = vectors[i]->object - flow;
      const float error = ErrorDiff(diff, error_system);
      curr_features[i] = static_cast<uint8>(error < sq_cutoff);
      if (curr_features[i]) {
        curr_sum += (*weights)[i];
      }
    }

    if (curr_sum > best_sum) {
      best_sum = curr_sum;
      std::swap(best_features, curr_features);
    }
  }

  std::vector<float> inlier_weights;
  inlier_weights.reserve(num_features);

  // Score outliers low.
  int num_inliers = 0;
  for (int i = 0; i < num_features; ++i) {
    if (best_features[i] == 0) {
      (*weights)[i] = 1e-10f;
    } else {
      ++num_inliers;
      inlier_weights.push_back((*weights)[i]);
    }
  }

  if (!inlier_weights.empty()) {
    // Ensure that all selected inlier features have at least median weight.
    auto median = inlier_weights.begin() + inlier_weights.size() * 0.5f;
    std::nth_element(inlier_weights.begin(), median, inlier_weights.end());

    for (int i = 0; i < num_features; ++i) {
      if (best_features[i] != 0) {
        (*weights)[i] = std::max(*median, (*weights)[i]);
      }
    }
  }
}

void MotionBox::EstimateObjectMotion(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& prior_weights, int num_continued_inliers,
    const Vector2_f& irls_scale, std::vector<float>* weights,
    Vector2_f* object_translation, LinearSimilarityModel* object_similarity,
    Homography* object_homography) const {
  CHECK(object_translation);
  CHECK(object_similarity);
  CHECK(object_homography);

  const int num_vectors = motion_vectors.size();
  CHECK_EQ(num_vectors, prior_weights.size());
  CHECK_EQ(num_vectors, weights->size());

  // Create backup of weights if needed.
  std::vector<float> similarity_weights;

  switch (options_.tracking_degrees()) {
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_SCALE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION_SCALE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE:
      similarity_weights = *weights;
      break;

    default:
      // Nothing to do here.
      break;
  }

  EstimateTranslation(motion_vectors, prior_weights, irls_scale, weights,
                      object_translation);

  TranslationModel translation_model = TranslationAdapter::FromArgs(
      object_translation->x(), object_translation->y());

  // For any additional degrees of freedom, require a good set of inliers.
  if (num_continued_inliers < options_.object_similarity_min_contd_inliers()) {
    if (options_.tracking_degrees() !=
        TrackStepOptions::TRACKING_DEGREE_TRANSLATION) {
      VLOG(2) << "Falling back to translation!!!";
    }
    VLOG(1) << "num_continued_inliers: " << num_continued_inliers << " < "
            << options_.object_similarity_min_contd_inliers()
            << ", fall back to translation";
    *object_similarity = LinearSimilarityAdapter::Embed(translation_model);
    *object_homography = HomographyAdapter::Embed(translation_model);
    return;
  }

  switch (options_.tracking_degrees()) {
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_SCALE:
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_ROTATION_SCALE: {
      if (EstimateSimilarity(motion_vectors, prior_weights, irls_scale,
                             &similarity_weights, object_similarity)) {
        if (!ObjectMotionValidator::IsValidSimilarity(
                *object_similarity, options_.box_similarity_max_scale(),
                options_.box_similarity_max_rotation())) {
          LOG(WARNING) << "Unstable similarity model - falling back to "
                       << "translation.";
          *object_similarity =
              LinearSimilarityAdapter::Embed(translation_model);
        } else {
          // Good estimation, use weights as output.
          weights->swap(similarity_weights);
        }
      } else {
        *object_similarity = LinearSimilarityAdapter::Embed(translation_model);
      }

      break;
    }
    case TrackStepOptions::TRACKING_DEGREE_OBJECT_PERSPECTIVE:
      if (EstimateHomography(motion_vectors, prior_weights, irls_scale,
                             &similarity_weights, object_homography)) {
        if (!ObjectMotionValidator::IsValidHomography(
                *object_homography, options_.quad_homography_max_scale(),
                options_.quad_homography_max_rotation())) {
          LOG(WARNING) << "Unstable homography model - falling back to "
                       << "translation.";
          *object_homography = HomographyAdapter::Embed(translation_model);
        } else {
          weights->swap(similarity_weights);
        }
      } else {
        *object_homography = HomographyAdapter::Embed(translation_model);
      }
      VLOG(1) << "Got homography: "
              << HomographyAdapter::ToString(*object_homography);
      break;
    default:
      // Plenty of CAMERA_ cases are not handled in this function.
      break;
  }
}

void MotionBox::EstimateTranslation(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& prior_weights, const Vector2_f& irls_scale,
    std::vector<float>* weights, Vector2_f* translation) const {
  CHECK(weights);
  CHECK(translation);

  const int iterations = options_.irls_iterations();

  // NOTE: Floating point accuracy is totally sufficient here.
  //                 We tried changing to double now 3 times and it just does
  //                 not matter. Do not do it again.      - Past self
  Vector2_f object_translation;
  const int num_vectors = motion_vectors.size();
  const float kEpsilon = 1e-8f;

  VLOG(1) << "Estimating translation for " << num_vectors << " vectors";

  for (int i = 0; i < iterations; ++i) {
    float flow_sum = 0;
    object_translation = Vector2_f(0.0, 0.0);
    for (int k = 0; k < num_vectors; ++k) {
      const MotionVector& motion_vector = *motion_vectors[k];
      const Vector2_f flow = motion_vector.object;
      object_translation += flow * (*weights)[k];
      flow_sum += (*weights)[k];
    }

    if (flow_sum > 0) {
      object_translation *= (1.0 / flow_sum);

      const auto error_system =
          ComputeIrlsErrorSystem(irls_scale, object_translation);

      // Update irls weights.
      for (int k = 0; k < num_vectors; ++k) {
        const MotionVector& motion_vector = *motion_vectors[k];
        Vector2_f diff(motion_vector.object - object_translation);
        const float error = ErrorDiff(diff, error_system);
        // In last iteration compute weights without any prior bias.
        const float numerator = (i + 1 == iterations) ? 1.0f : prior_weights[k];
        (*weights)[k] = numerator / (error + kEpsilon);
      }
    }
  }
  *translation = object_translation;

  VLOG(1) << "Got translation: " << *translation;
}

bool MotionBox::EstimateSimilarity(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& prior_weights, const Vector2_f& irls_scale,
    std::vector<float>* weights, LinearSimilarityModel* lin_sim) const {
  CHECK(weights);
  CHECK(lin_sim);

  const int iterations = options_.irls_iterations();
  LinearSimilarityModel object_similarity;
  const int num_vectors = motion_vectors.size();
  const float kEpsilon = 1e-8f;

  VLOG(1) << "Estimating similarity for " << num_vectors << " vectors";
  for (int i = 0; i < iterations; ++i) {
    if (LinearSimilarityL2Solve(motion_vectors, *weights, &object_similarity)) {
      // Update irls weights.
      for (int k = 0; k < num_vectors; ++k) {
        const MotionVector& motion_vector = *motion_vectors[k];
        const Vector2_f model_vec =
            TransformPoint(object_similarity, motion_vector.pos) -
            motion_vector.pos;
        const auto error_system = ComputeIrlsErrorSystem(irls_scale, model_vec);

        Vector2_f diff(motion_vector.object - model_vec);
        const float error = ErrorDiff(diff, error_system);
        // In last iteration compute weights without any prior bias.
        const float numerator = (i + 1 == iterations) ? 1.0f : prior_weights[k];
        (*weights)[k] = numerator / (error + kEpsilon);
      }
    } else {
      return false;
    }
  }
  *lin_sim = object_similarity;

  VLOG(1) << "Got similarity: "
          << LinearSimilarityAdapter::ToString(object_similarity);
  return true;
}

bool MotionBox::EstimateHomography(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& prior_weights, const Vector2_f& irls_scale,
    std::vector<float>* weights, Homography* object_homography) const {
  CHECK(weights);

  const int iterations = options_.irls_iterations();
  Homography homography;
  const int num_vectors = motion_vectors.size();
  const float kEpsilon = 1e-8f;

  VLOG(1) << "Estimating homography for " << num_vectors << " vectors";
  for (int i = 0; i < iterations; ++i) {
    if (HomographyL2Solve(motion_vectors, *weights, &homography)) {
      // Update irls weights.
      for (int k = 0; k < num_vectors; ++k) {
        const MotionVector& motion_vector = *motion_vectors[k];
        const Vector2_f model_vec =
            TransformPoint(homography, motion_vector.pos) - motion_vector.pos;
        const auto error_system = ComputeIrlsErrorSystem(irls_scale, model_vec);

        Vector2_f diff(motion_vector.object - model_vec);
        const float error = ErrorDiff(diff, error_system);
        // In last iteration compute weights without any prior bias.
        const float numerator = (i + 1 == iterations) ? 1.0f : prior_weights[k];
        (*weights)[k] = numerator / (error + kEpsilon);
      }
    } else {
      return false;
    }
  }
  *object_homography = homography;

  return true;
}

bool MotionBox::EstimatePnpHomography(
    const MotionBoxState& curr_pos,
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& weights, float domain_x, float domain_y,
    Homography* pnp_homography) const {
  constexpr int kMinVectors = 4;
  if (motion_vectors.size() < kMinVectors) return false;

  Homography inv_h = HomographyAdapter::Invert(curr_pos.pnp_homography());

  std::vector<cv::Point3f> vectors_3d;
  vectors_3d.reserve(motion_vectors.size());
  std::vector<cv::Point2f> vectors_2d;
  vectors_2d.reserve(motion_vectors.size());
  if (options_.has_camera_intrinsics()) {
    const auto& camera = options_.camera_intrinsics();
    cv::Mat camera_mat, dist_coef;
    ConvertCameraIntrinsicsToCvMat(camera, &camera_mat, &dist_coef);
    float scale = std::max(camera.w(), camera.h());

    std::vector<cv::Point2f> mv_p;
    mv_p.reserve(motion_vectors.size());
    std::vector<cv::Point2f> mv_q;
    mv_q.reserve(motion_vectors.size());
    for (int j = 0; j < motion_vectors.size(); ++j) {
      if (weights[j] < kMaxOutlierWeight) continue;
      mv_p.emplace_back(motion_vectors[j]->pos.x() * scale,
                        motion_vectors[j]->pos.y() * scale);

      Vector2_f q = motion_vectors[j]->pos + motion_vectors[j]->object +
                    motion_vectors[j]->background;
      mv_q.emplace_back(q.x() * scale, q.y() * scale);
    }

    if (mv_p.size() < kMinVectors) return false;

    cv::undistortPoints(mv_p, mv_p, camera_mat, dist_coef);
    cv::undistortPoints(mv_q, mv_q, camera_mat, dist_coef);

    vectors_3d.resize(mv_p.size());
    vectors_2d.resize(mv_q.size());
    for (int j = 0; j < mv_p.size(); ++j) {
      Vector2_f p = TransformPoint(inv_h, Vector2_f(mv_p[j].x, mv_p[j].y));
      vectors_3d[j] = cv::Point3f(p.x(), p.y(), 0.0f);
      vectors_2d[j] = cv::Point2f(mv_q[j].x, mv_q[j].y);
    }
  } else {
    Vector2_f center(domain_x * 0.5f, domain_y * 0.5f);
    for (int j = 0; j < motion_vectors.size(); ++j) {
      if (weights[j] < kMaxOutlierWeight) continue;
      Vector2_f p = TransformPoint(inv_h, motion_vectors[j]->pos - center);
      vectors_3d.emplace_back(p.x(), p.y(), 0.0f);

      Vector2_f q = motion_vectors[j]->pos + motion_vectors[j]->object +
                    motion_vectors[j]->background - center;
      vectors_2d.emplace_back(q.x(), q.y());
    }

    if (vectors_3d.size() < kMinVectors) return false;
  }

  // TODO: use previous rvec and tvec to initilize the solver.
  cv::Mat rvec, tvec;
  cv::solvePnP(vectors_3d, vectors_2d,
               cv::Mat::eye(3, 3, CV_64F) /* camera_mat */,
               cv::Mat::zeros(1, 5, CV_64FC1) /* dist_coef */, rvec, tvec);
  *pnp_homography = PnpHomographyFromRotationAndTranslation(rvec, tvec);

  return true;
}

void MotionBox::ApplyObjectMotionPerspectively(const MotionBoxState& curr_pos,
                                               const Homography& pnp_homography,
                                               float domain_x, float domain_y,
                                               MotionBoxState* next_pos) const {
  const float half_width = curr_pos.aspect_ratio();
  const float half_height = 1.0f;

  constexpr int kQuadCornersSize = 4;

  // Omitting the 3rd dimension because they are all zeros.
  const std::vector<Vector2_f> corners_3d{
      Vector2_f(-half_width, -half_height),
      Vector2_f(-half_width, half_height),
      Vector2_f(half_width, half_height),
      Vector2_f(half_width, -half_height),
  };

  std::vector<Vector2_f> corners_2d(kQuadCornersSize);
  for (int c = 0; c < kQuadCornersSize; ++c) {
    corners_2d[c] =
        HomographyAdapter::TransformPoint(pnp_homography, corners_3d[c]);
  }

  if (options_.has_camera_intrinsics()) {
    std::vector<cv::Point3f> cv_points(4);
    for (int c = 0; c < kQuadCornersSize; ++c) {
      cv_points[c] = cv::Point3f(corners_2d[c].x(), corners_2d[c].y(), 1.0);
    }

    const auto& camera = options_.camera_intrinsics();
    cv::Mat camera_mat, dist_coef;
    ConvertCameraIntrinsicsToCvMat(camera, &camera_mat, &dist_coef);
    cv::Mat dummy_zeros = cv::Mat::zeros(1, 3, CV_64FC1);
    std::vector<cv::Point2f> cv_points_distorted;
    cv::projectPoints(cv_points, dummy_zeros /* rvec */, dummy_zeros /* tvec */,
                      camera_mat, dist_coef, cv_points_distorted);
    const float scale = 1.0f / std::max(camera.w(), camera.h());
    for (int c = 0; c < kQuadCornersSize; ++c) {
      next_pos->mutable_quad()->set_vertices(c * 2,
                                             cv_points_distorted[c].x * scale);
      next_pos->mutable_quad()->set_vertices(c * 2 + 1,
                                             cv_points_distorted[c].y * scale);
    }
  } else {
    const float center_x = domain_x * 0.5f;
    const float center_y = domain_y * 0.5f;
    for (int c = 0; c < kQuadCornersSize; ++c) {
      next_pos->mutable_quad()->set_vertices(c * 2,
                                             corners_2d[c].x() + center_x);
      next_pos->mutable_quad()->set_vertices(c * 2 + 1,
                                             corners_2d[c].y() + center_y);
    }
  }

  *next_pos->mutable_pnp_homography() = pnp_homography;
  UpdateStatePositionAndSizeFromStateQuad(next_pos);
}

float MotionBox::ComputeMotionDisparity(
    const MotionBoxState& curr_pos, const Vector2_f& irls_scale,
    float continued_inliers, int num_inliers,
    const Vector2_f& object_translation) const {
  // Motion disparity does not take into account change of direction,
  // only use parallel irls scale.
  const float curr_velocity = MotionBoxVelocity(curr_pos).Norm();
  const float sign = object_translation.Norm() < curr_velocity ? -1.0f : 1.0f;
  const float motion_diff = fabs(object_translation.Norm() - curr_velocity);

  // Score difference.
  const float measured_motion_disparity = LinearRamp(
      motion_diff * irls_scale.x(), options_.motion_disparity_low_level(),
      options_.motion_disparity_high_level());

  // Cap disparity measurement by inlier ratio, to account for objects
  // suddenly stopping/accelerating. In this case measured disparity might be
  // high, whereas inliers continue to be tracked.
  const float max_disparity = 1.0f - continued_inliers;

  const float capped_disparity =
      std::min(max_disparity, measured_motion_disparity);

  // Take into account large disparity in previous frames. Score by prior of
  // previous motion.
  const float motion_disparity =
      std::max(curr_pos.motion_disparity() * options_.disparity_decay(),
               capped_disparity) *
      curr_pos.prior_weight();

  // Map number of inliers to score in [0, 1], assuming a lot of inliers
  // indicate lock onto object.
  const float inlier_score = LinearRamp(num_inliers, 20, 40);

  // Decaying motion disparity faster if number of inliers indicate lock onto
  // tracking objects has occurred.
  return std::min(1.0f - inlier_score, motion_disparity) * sign;
}

void MotionBox::ScoreAndRecordInliers(
    const MotionBoxState& curr_pos,
    const std::vector<const MotionVector*>& vectors,
    const std::vector<Vector2_f>& grid_positions,
    const std::vector<float>& pre_estimation_weights,
    const std::vector<float>& post_estimation_weights,
    float background_discrimination, MotionBoxState* next_pos,
    std::vector<float>* inlier_weights, std::vector<float>* inlier_density,
    int* continued_inliers, int* swapped_inliers, float* motion_inliers_out,
    float* kinetic_average_out) const {
  CHECK(inlier_weights);
  CHECK(inlier_density);
  CHECK(continued_inliers);
  CHECK(swapped_inliers);
  CHECK(motion_inliers_out);
  CHECK(kinetic_average_out);

  std::unordered_map<int, int> prev_inliers;
  MotionBoxInliers(curr_pos, &prev_inliers);

  std::unordered_set<int> prev_outliers;
  MotionBoxOutliers(curr_pos, &prev_outliers);

  ClearInlierState(next_pos);

  // Continued inlier fraction denotes amount of spatial occlusion. Very low
  // values indicate that we are in very difficult tracking territory.
  *continued_inliers = 0;
  *swapped_inliers = 0;
  float kinetic_average = 0;
  float kinetic_average_sum = 0;
  float motion_inliers = 0;
  const int num_vectors = vectors.size();
  inlier_weights->resize(num_vectors);
  inlier_density->resize(num_vectors);

  // Inliers normalization grid.
  std::vector<float> grid_count(kNormalizationGridSize *
                                kNormalizationGridSize);
  const float prev_object_motion = MotionBoxVelocity(curr_pos).Norm();
  // Count number of similar moving inliers as previous object motion.
  const float similar_motion_threshold =
      std::max(2e-3f, prev_object_motion * 0.3f);

  // If background discrimination is low, inliers are ambiguous: Hard to
  // distinguish from earlier outliers. In this case do not record inliers
  // outside our current tracking extent, as everything will look like an
  // inlier.
  //
  // TODO: Compute 2nd moment for inliers and describe as ellipse,
  // improve shape here then.
  bool inlier_ambiguity = background_discrimination < 0.5f;
  int rejected = 0;
  int num_inliers = 0;
  for (int k = 0; k < num_vectors; ++k) {
    (*inlier_weights)[k] =
        LinearRamp(post_estimation_weights[k], options_.inlier_low_weight(),
                   options_.inlier_high_weight());
    const int track_id = vectors[k]->track_id;

    bool is_prev_outlier = prev_outliers.find(track_id) != prev_outliers.end();

    const Vector2_f match_loc = vectors[k]->MatchLocation();
    if ((*inlier_weights)[k] > kMinInlierWeight) {  // Inlier.
      if (is_prev_outlier) {
        ++(*swapped_inliers);
      }

      if (inlier_ambiguity &&
          !PointWithinInlierExtent(vectors[k]->Location(), curr_pos)) {
        ++rejected;
        continue;
      }

      ++num_inliers;

      AddToGrid<kNormalizationGridSize>(grid_positions[k], &grid_count);

      if (track_id >= 0) {
        next_pos->add_inlier_ids(track_id);
        next_pos->add_inlier_id_match_pos(match_loc.x() * kShortScale);
        next_pos->add_inlier_id_match_pos(match_loc.y() * kShortScale);
        auto pos = prev_inliers.find(track_id);
        if (pos != prev_inliers.end()) {
          // Count length of observation.
          next_pos->add_inlier_length(pos->second + 1.0f);
          ++(*continued_inliers);
        } else {
          next_pos->add_inlier_length(1);
        }
      }

      // Note: This should be weighted by the pre estimation weights, simply
      // adding a 1 for each inlier leads to lower irls averages.
      kinetic_average += vectors[k]->object.Norm() * pre_estimation_weights[k];
      kinetic_average_sum += pre_estimation_weights[k];

      // Count number of inliers that agree with previous kinetic energy
      // estimate.
      if (std::abs(vectors[k]->object.Norm() - prev_object_motion) *
              curr_pos.prior_weight() <
          similar_motion_threshold) {
        motion_inliers += pre_estimation_weights[k];
      }
    } else if ((*inlier_weights)[k] < kMaxOutlierWeight) {  // Outlier.
      next_pos->add_outlier_ids(track_id);
      next_pos->add_outlier_id_match_pos(match_loc.x() * kShortScale);
      next_pos->add_outlier_id_match_pos(match_loc.y() * kShortScale);
    }
  }

  // Read out density of inliers.
  for (int k = 0; k < num_vectors; ++k) {
    if ((*inlier_weights)[k] > kMinInlierWeight) {
      (*inlier_density)[k] = 2 * SampleFromGrid<kNormalizationGridSize>(
                                     grid_positions[k], grid_count);
    } else {
      (*inlier_density)[k] = 0;
    }
  }

  if (kinetic_average_sum > 0) {
    kinetic_average *= 1.0f / kinetic_average_sum;
  }

  VLOG(1) << "num inliers: " << num_inliers << " rejected: " << rejected;

  *kinetic_average_out = kinetic_average;
  *motion_inliers_out = motion_inliers;
}

void MotionBox::ComputeInlierCenterAndExtent(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& weights, const std::vector<float>& density,
    const MotionBoxState& box_state, float* min_inlier_sum, Vector2_f* center,
    Vector2_f* extent) const {
  CHECK(min_inlier_sum);
  CHECK(center);
  CHECK(extent);

  float weight_sum = 0;
  float inlier_sum = 0;
  const int num_vectors = motion_vectors.size();
  CHECK_EQ(num_vectors, weights.size());
  CHECK_EQ(num_vectors, density.size());

  Vector2_f first_moment(0.0f, 0.0f);
  Vector2_f second_moment(0.0f, 0.0f);

  Vector2_f top_left;
  Vector2_f bottom_right;
  MotionBoxBoundingBox(box_state, &top_left, &bottom_right);

  for (int k = 0; k < num_vectors; ++k) {
    const MotionVector& motion_vector = *motion_vectors[k];
    const Vector2_f match = motion_vector.MatchLocation();
    float space_multiplier = 1.0f;
    // Decrease contribution of out of bound inliers.
    // Note: If all inliers are out of bound this down weighting will have no
    // effect. It is designed to prevent skewing the inlier center towards
    // similar moving inliers outside the tracked box.
    if (match.x() < top_left.x() || match.x() > bottom_right.x() ||
        match.y() < top_left.y() || match.y() > bottom_right.y()) {
      space_multiplier = 0.25f;
    }
    const float w = weights[k] * density[k] * space_multiplier;
    if (w > 0) {
      first_moment += w * match;
      second_moment +=
          w * Vector2_f(match.x() * match.x(), match.y() * match.y());
      weight_sum += w;
      inlier_sum += weights[k];
    }
  }

  // Update center if sufficient inliers present.
  if (inlier_sum > *min_inlier_sum) {
    const float inv_weight_sum = 1.0f / weight_sum;
    first_moment *= inv_weight_sum;
    second_moment *= inv_weight_sum;

    *center = first_moment;
    *extent = second_moment - Vector2_f(first_moment.x() * first_moment.x(),
                                        first_moment.y() * first_moment.y());

    // 1.645 sigmas in each direction = 90% of the data captured.
    *extent =
        Vector2_f(std::sqrt(extent->x()) * 3.29, std::sqrt(extent->y()) * 3.29);
  } else {
    // Gravitate back to box center with inlier center.
    *center = Lerp(MotionBoxCenter(box_state), InlierCenter(box_state), 0.5f);
  }

  // Record number of inliers present.
  *min_inlier_sum = weight_sum;
}

float MotionBox::ScaleEstimate(
    const std::vector<const MotionVector*>& motion_vectors,
    const std::vector<float>& weights, float min_sum) const {
  const int num_vectors = motion_vectors.size();
  CHECK_EQ(num_vectors, weights.size());

  float scale_sum = 0;

  // First moments.
  Vector2_d sum_coords(0, 0);
  Vector2_d match_sum_coords(0, 0);

  // Second moments.
  Vector2_d sum_sq_coords(0, 0);
  Vector2_d match_sum_sq_coords(0, 0);

  for (int k = 0; k < num_vectors; ++k) {
    const MotionVector& motion_vector = *motion_vectors[k];

    const Vector2_d pos(motion_vector.pos.x(), motion_vector.pos.y());
    double weight = weights[k];
    sum_coords += weight * pos;
    sum_sq_coords += weight * Vector2_d(pos.x() * pos.x(), pos.y() * pos.y());

    const Vector2_d match =
        Vector2_d::Cast<float>(motion_vector.MatchLocation());
    match_sum_coords += weight * match;
    match_sum_sq_coords +=
        weight * Vector2_d(match.x() * match.x(), match.y() * match.y());
    scale_sum += weights[k];
  }

  if (scale_sum > min_sum) {
    const double denom = 1.0f / scale_sum;
    sum_coords *= denom;
    match_sum_coords *= denom;
    sum_sq_coords *= denom;
    match_sum_sq_coords *= denom;

    const float curr_scale =
        sqrt(sum_sq_coords.x() - sum_coords.x() * sum_coords.x() +
             sum_sq_coords.y() - sum_coords.y() * sum_coords.y());
    const float next_scale = sqrt(
        match_sum_sq_coords.x() - match_sum_coords.x() * match_sum_coords.x() +
        match_sum_sq_coords.y() - match_sum_coords.y() * match_sum_coords.y());
    return next_scale / curr_scale;
  }

  return 1.0f;
}

void MotionBox::ApplySpringForce(const Vector2_f& center_of_interest,
                                 const float rel_threshold,
                                 const float spring_force,
                                 MotionBoxState* box_state) const {
  // Apply spring force towards center of interest.
  const Vector2_f center = MotionBoxCenter(*box_state);
  const float center_diff_x = center_of_interest.x() - center.x();
  const float center_diff_y = center_of_interest.y() - center.y();

  const float diff_x = fabs(center_diff_x) - box_state->width() * rel_threshold;
  const float diff_y =
      fabs(center_diff_y) - box_state->height() * rel_threshold;

  if (diff_x > 0) {
    const float correction_mag = diff_x * spring_force;
    const float correction =
        center_diff_x < 0 ? -correction_mag : correction_mag;
    box_state->set_pos_x(box_state->pos_x() + correction);
  }

  if (diff_y > 0) {
    const float correction_mag = diff_y * spring_force;
    const float correction =
        center_diff_y < 0 ? -correction_mag : correction_mag;
    box_state->set_pos_y(box_state->pos_y() + correction);
  }
}

void MotionBox::TrackStepImpl(int from_frame, const MotionBoxState& curr_pos,
                              const MotionVectorFrame& motion_frame,
                              const std::vector<const MotionBoxState*>& history,
                              MotionBoxState* next_pos) const {
  // Create new curr pos with velocity scaled to current duration.
  constexpr float kDefaultPeriodMs = 1000.0f / kTrackingDefaultFps;

  // Scale to be applied to velocity related fields in MotionBoxState
  // to transform state from standard frame period to current one.
  float temporal_scale = (motion_frame.duration_ms == 0)
                             ? 1.0f
                             : motion_frame.duration_ms / kDefaultPeriodMs;

  MotionBoxState curr_pos_normalized = curr_pos;
  ScaleStateTemporally(temporal_scale, &curr_pos_normalized);
  ScaleStateAspect(motion_frame.aspect_ratio, false, &curr_pos_normalized);

  TrackStepImplDeNormalized(from_frame, curr_pos_normalized, motion_frame,
                            history, next_pos);

  // Scale back velocity and aspect to normalized domains.
  ScaleStateTemporally(1.0f / temporal_scale, next_pos);
  ScaleStateAspect(motion_frame.aspect_ratio, true, next_pos);

  // Test if out of bound, only for moving objects.
  const float static_motion =
      options_.static_motion_temporal_ratio() * temporal_scale;
  if (MotionBoxVelocity(*next_pos).Norm() > static_motion) {
    // Test if close to boundary and keep moving towards it.
    constexpr float kRatio = 0.3;
    if ((next_pos->pos_x() < -next_pos->width() * kRatio &&
         next_pos->dx() < -static_motion / 2) ||
        (next_pos->pos_y() < -next_pos->height() * kRatio &&
         next_pos->dy() < -static_motion / 2) ||
        (next_pos->pos_x() > 1.0f - next_pos->width() * (1.0f - kRatio) &&
         next_pos->dx() > static_motion / 2) ||
        (next_pos->pos_y() > 1.0f - next_pos->height() * (1.0f - kRatio) &&
         next_pos->dy() > static_motion / 2)) {
      VLOG(1) << "Tracked box went out of bound.";
      next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
      return;
    }
  }
}

float MotionBox::ComputeTrackingConfidence(
    const MotionBoxState& motion_box_state) const {
  const float inlier_num_lower_bound = 10.0f;
  const float inlier_num_upper_bound = 30.0f;
  const float confidence =
      LinearRamp(motion_box_state.inlier_ids_size(), inlier_num_lower_bound,
                 inlier_num_upper_bound);
  return confidence;
}

// General tracking algorithm overview:
// Tracking algorithm consists of 6 main stages.
// 1.) Selecting features from passed MotionVectorFrame based on incidence with
//     rectangle defined by curr_pos
// 2.) Assigning each vector a prior weight. Vector are mainly scored by a box
//     centered gaussian, giving more weights to vectors within the center of
//     the box. If current state is deemed unreliable, vectors with similar
//     velocity to previously estimated velocity are favored. If current state
//     indicates tracking of a moving object, high velocity vectors are favored.
// 3.) Estimating a translational model via IRLS enforcing the prior of step 2
//     in every iteration.
// 4.) Score how much the estimate model deviates from the previous motion
//     (termed motion disparity) and how discriminative the motion is
//     from the background motion (termed motion discrimination)
// 5.) Computing the inlier center (position of vectors used for the motion
//     model in the next frame) and center of high velocity vectors. Apply a
//     spring force towards each center based on the motion discrimination.
// 6.) Update velocity and kinetic energy by blending current measurment with
//     previous one.
void MotionBox::TrackStepImplDeNormalized(
    int from_frame, const MotionBoxState& curr_pos,
    const MotionVectorFrame& motion_frame,
    const std::vector<const MotionBoxState*>& history,
    MotionBoxState* next_pos) const {
  CHECK(next_pos);

  constexpr float kDefaultPeriodMs = 1000.0f / kTrackingDefaultFps;
  float temporal_scale = (motion_frame.duration_ms == 0)
                             ? 1.0f
                             : motion_frame.duration_ms / kDefaultPeriodMs;

  // Initialize to current position.
  *next_pos = curr_pos;

  if (!IsBoxValid(curr_pos)) {
    LOG(ERROR) << "curr_pos is not a valid box. Stop tracking!";
    next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
    return;
  }

  Vector2_f top_left, bottom_right;
  float expand_mag = 0.0f;
  GetStartPosition(curr_pos, motion_frame.aspect_ratio, &expand_mag, &top_left,
                   &bottom_right);

  const float aspect_ratio = motion_frame.aspect_ratio;
  float domain_x = 1.0f;
  float domain_y = 1.0f;
  ScaleFromAspect(aspect_ratio, false, &domain_x, &domain_y);

  // Binary search for start and end index (lexicographic search, i.e.
  // x indices are guaranteed to be within bounds, but y coordinates could be
  // outside and need to be checked against the domain of the box via
  // GetVectorsAndWeights below).
  MotionVector search_start;
  MotionVector search_end;
  search_start.pos = top_left;
  search_end.pos = bottom_right;

  int start_idx = std::lower_bound(motion_frame.motion_vectors.begin(),
                                   motion_frame.motion_vectors.end(),
                                   search_start, MotionVectorComparator()) -
                  motion_frame.motion_vectors.begin();

  int end_idx = std::lower_bound(motion_frame.motion_vectors.begin(),
                                 motion_frame.motion_vectors.end(), search_end,
                                 MotionVectorComparator()) -
                motion_frame.motion_vectors.begin();

  const float static_motion =
      options_.static_motion_temporal_ratio() * temporal_scale;
  if (start_idx >= end_idx || top_left.x() >= domain_x - expand_mag ||
      top_left.y() >= domain_y - expand_mag || bottom_right.x() <= expand_mag ||
      bottom_right.y() <= expand_mag) {
    // Empty box, no features found. It can happen if box is outside
    // field of view, or there is no features in the box.
    // Move box by background model if it has static motion, else move return
    // tracking error.
    if (MotionBoxVelocity(curr_pos).Norm() > static_motion ||
        (!motion_frame.valid_background_model && from_frame != queue_start_)) {
      next_pos->set_track_status(MotionBoxState::BOX_NO_FEATURES);
    } else {
      // Static object, move by background model.
      next_pos->set_track_status(MotionBoxState::BOX_TRACKED_OUT_OF_BOUND);
      ApplyCameraTrackingDegrees(curr_pos, motion_frame.background_model,
                                 options_, Vector2_f(domain_x, domain_y),
                                 next_pos);

      // The further the quad is away from the FOV (range 0 to 1), the larger
      // scale change will be applied to the quad by homography transform. To
      // some certain extent, the position of vertices will be flipped from
      // positive to negative, or vice versa. Here we reject all the quads with
      // abnormal shape by convexity of the quad..
      if (next_pos->has_quad() &&
          (ObjectMotionValidator::IsQuadOutOfFov(
               next_pos->quad(), Vector2_f(domain_x, domain_y)) ||
           !ObjectMotionValidator::IsValidQuad(next_pos->quad()))) {
        LOG(ERROR) << "Quad is out of fov or not convex. Cancel tracking.";
        next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
        return;
      }
    }
    return;
  }

  float start_x = Clamp(top_left.x(), 0.0f, domain_x);
  float start_y = Clamp(top_left.y(), 0.0f, domain_y);
  float end_x = Clamp(bottom_right.x(), 0.0f, domain_x);
  float end_y = Clamp(bottom_right.y(), 0.0f, domain_y);

  const Vector2_f curr_pos_size = MotionBoxSize(curr_pos);
  constexpr float kMinSize = 1e-3f;  // 1 pix for 1080p.
  if (start_x >= end_x || start_y >= end_y || curr_pos_size.x() < kMinSize ||
      curr_pos_size.y() < kMinSize) {
    next_pos->set_track_status(MotionBoxState::BOX_EMPTY);
    return;
  }

  top_left = Vector2_f(start_x, start_y);
  bottom_right = Vector2_f(end_x, end_y);

  // Get indices of features within box, corresponding priors and position
  // in feature grid.
  std::vector<const MotionVector*> vectors;
  std::vector<float> prior_weights;
  bool valid_background_model = motion_frame.valid_background_model;

  int num_good_inits;
  int num_cont_inliers;
  const bool get_vec_weights_status = GetVectorsAndWeights(
      motion_frame.motion_vectors, start_idx, end_idx, top_left, bottom_right,
      curr_pos, valid_background_model, motion_frame.is_chunk_boundary,
      temporal_scale, expand_mag, history, &vectors, &prior_weights,
      &num_good_inits, &num_cont_inliers);
  if (!get_vec_weights_status) {
    LOG(ERROR) << "error in GetVectorsAndWeights. Terminate tracking.";
    next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
    return;
  }

  // `num_good_inits` comes from motion vector weights, but pnp solver currently
  // does not depend on weights. So for pnp tracking mode, we don't fall back to
  // camera motion tracking based on num_good_inits.
  if (!curr_pos.has_pnp_homography() &&
      (num_good_inits < 3 &&
       MotionBoxVelocity(curr_pos).Norm() <= static_motion)) {
    // Static object, move by background model.
    next_pos->set_track_status(MotionBoxState::BOX_TRACKED);
    ApplyCameraTrackingDegrees(curr_pos, motion_frame.background_model,
                               options_, Vector2_f(domain_x, domain_y),
                               next_pos);
    VLOG(1) << "No good inits; applying camera motion for static object";

    if (next_pos->has_quad() &&
        !ObjectMotionValidator::IsValidQuad(next_pos->quad())) {
      LOG(ERROR) << "Quad is not convex. Cancel tracking.";
      next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
      return;
    }
    return;
  }

  VLOG(1) << "Good inits: " << num_good_inits;

  const int num_vectors = vectors.size();
  CHECK_EQ(num_vectors, prior_weights.size());

  Vector2_f object_translation;

  // Compute a rough estimate of the current motion.
  for (int k = 0; k < num_vectors; ++k) {
    object_translation += vectors[k]->Motion() * prior_weights[k];
  }

  Vector2_f prev_object_motion = MotionBoxVelocity(curr_pos);

  // Estimate expected motion magnitude. In case of low prior, skew more
  // towards rough estimate instead of previous motion.
  const float motion_mag_estimate =
      std::max(object_translation.Norm(),
               prev_object_motion.Norm() * curr_pos.prior_weight());

  // For motivation about this: See MotionEstimation::GetIRLSResidualScale.
  // Assume 1 pixel estimation error for tracked features at 360p video.
  // This serves as absolute minimum of the estimation error, so we do not
  // scale translation fractions below this threshold.
  constexpr float kMinError = 1.25e-3f;

  // We use a combination of absolute and relative error. If a predefined
  // fraction of the motion exceeds the minimum error, we scale the error
  // such that the relative error equals the min error.
  // We use different thresholds parallel and perpendicular to the estimation
  // direction.
  // Motivation is that we allow for more error perpendicular to an estimation
  // (angular difference) than it is direction (magnitude error).

  // Scale in parallel, orthogonal direction.
  Vector2_f irls_scale(1.0f, 1.0f);

  const Vector2_f kMotionPercentage(0.1f, 0.25f);
  const Vector2_f motion_mag_scaled = motion_mag_estimate * kMotionPercentage;

  if (motion_mag_scaled.x() > kMinError) {
    irls_scale.x(kMinError / motion_mag_scaled.x());
  }
  if (motion_mag_scaled.y() > kMinError) {
    irls_scale.y(kMinError / motion_mag_scaled.y());
  }

  // Irls init for translation.
  // TODO: Adjust to object tracking degrees.
  TranslationIrlsInitialization(vectors, irls_scale, &prior_weights);

  LinearSimilarityModel object_similarity;
  Homography object_homography;
  Homography pnp_homography;

  std::vector<float> weights = prior_weights;
  if (num_good_inits > 0) {
    MEASURE_TIME << "Estimate object motion.";
    EstimateObjectMotion(vectors, prior_weights, num_cont_inliers, irls_scale,
                         &weights, &object_translation, &object_similarity,
                         &object_homography);
  } else {
    // There is no hope to estimate a model in a stable manner here.
    object_translation = prev_object_motion;
    VLOG(1) << "No good inits, reusing prev. motion instead of estimation";
  }

  // Multiplier to quanitify how discriminative object motion is
  // (larger motions are more discriminative).
  // Note: Independent from temporal scale.
  float background_discrimination = curr_pos.background_discrimination();
  if (valid_background_model) {
    background_discrimination =
        LinearRamp(object_translation.Norm(),
                   options_.background_discrimination_low_level(),
                   options_.background_discrimination_high_level());
  }

  // Score weights from motion estimation to determine set of inliers.
  std::vector<float> inlier_weights;
  std::vector<float> inlier_density;

  // Compute grid positions for each vector to determine density of inliers.
  std::vector<Vector2_f> grid_positions;
  ComputeGridPositions<kNormalizationGridSize>(top_left, bottom_right, vectors,
                                               &grid_positions);

  // Continued inlier fraction denotes amount of spatial occlusion. Very low
  // values indicate that we are in very difficult tracking territory.
  int continued_inliers = 0;
  int swapped_inliers = 0;
  float kinetic_average = 0;
  float motion_inliers = 0;
  ScoreAndRecordInliers(curr_pos, vectors, grid_positions, prior_weights,
                        weights, background_discrimination, next_pos,
                        &inlier_weights, &inlier_density, &continued_inliers,
                        &swapped_inliers, &motion_inliers, &kinetic_average);

  const int num_prev_inliers = curr_pos.inlier_ids_size();
  int num_prev_inliers_not_actively_discarded = num_prev_inliers;
  if (motion_frame.actively_discarded_tracked_ids != nullptr) {
    num_prev_inliers_not_actively_discarded = std::count_if(
        curr_pos.inlier_ids().begin(), curr_pos.inlier_ids().end(),
        [&motion_frame](int id) {
          return !motion_frame.actively_discarded_tracked_ids->contains(id);
        });
    motion_frame.actively_discarded_tracked_ids->clear();
  }
  const int num_inliers = next_pos->inlier_ids_size();
  // Must be in [0, 1].
  const float continued_inlier_fraction =
      num_prev_inliers_not_actively_discarded == 0
          ? 1.0f
          : continued_inliers * 1.0f / num_prev_inliers_not_actively_discarded;

  // Within [0, M], where M is maximum number of features. Values of > 1
  // indicate significant number of inlieres were outliers in previous frame.
  const float swapped_inlier_fraction =
      num_prev_inliers == 0 ? 0.0f : swapped_inliers * 1.0f / num_prev_inliers;

  if (curr_pos.has_pnp_homography()) {
    MEASURE_TIME << "Estimate pnp homography.";

    // Use IRLS homography `inlier_weights` to determin inliers / outliers.
    // The rationale is: solving homography transform is 20X faster than solving
    // perspective transform (0.05ms vs 1ms). So, we use 5 iterations of
    // reweighted homography to filter out outliers first. And only use inliers
    // to solve for perspective.
    if (!EstimatePnpHomography(curr_pos, vectors, inlier_weights, domain_x,
                               domain_y, &pnp_homography)) {
      // Here, we can either cancel tracking or apply homography or even
      // translation as our best guess. But since some specific use cases of
      // pnp tracking (for example Augmented Images) prefer high precision
      // over high recall, we choose to cancel tracking once and for all.
      VLOG(1) << "Not enough motion vectors to solve pnp. Cancel tracking.";
      next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
      return;
    }
  }

  // Compute disparity.
  if (num_good_inits > 0) {
    next_pos->set_motion_disparity(ComputeMotionDisparity(
        curr_pos, irls_scale, continued_inliers, num_inliers,
        valid_background_model ? object_translation : prev_object_motion));
  } else {
    // No good features, signal error.
    next_pos->set_motion_disparity(1.0);
  }

  VLOG(1) << "Motion inliers: " << motion_inliers
          << ", continued inliers: " << continued_inliers
          << ", continued ratio: " << continued_inlier_fraction
          << ", swapped fraction: " << swapped_inlier_fraction
          << ", motion disparity: " << next_pos->motion_disparity();

  if (options_.cancel_tracking_with_occlusion_options().activated() &&
      curr_pos.track_status() != MotionBoxState::BOX_DUPLICATED &&
      continued_inlier_fraction <
          options_.cancel_tracking_with_occlusion_options()
              .min_motion_continuity()) {
    next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
    LOG(INFO) << "Occlusion detected. continued_inlier_fraction: "
              << continued_inlier_fraction << " too low. Stop tracking";
    return;
  }

  // Force reset of state when inlier continuity is severly violated,
  // disparity maxes out or significant of number of inliers were outliers in
  // the previous frame.
  if (std::max(continued_inlier_fraction, motion_inliers) < 0.15f ||
      std::abs(next_pos->motion_disparity()) >= 1.0f ||
      swapped_inlier_fraction >= 2.5) {
    VLOG(1) << "Track error, state reset.";
    // Bad tracking error occurred.
    // Current set of inliers is not reliable.
    ClearInlierState(next_pos);
    next_pos->set_motion_disparity(1.0f);
    inlier_weights.assign(inlier_weights.size(), 0);

    // Reuse previous motion and discrimination.
    object_translation = prev_object_motion;
    background_discrimination = curr_pos.background_discrimination();
  }

  next_pos->set_inlier_sum(
      std::accumulate(inlier_weights.begin(), inlier_weights.end(), 0.0f));
  if (history.empty()) {
    // Assign full confidence on first frame, otherwise all other stats
    // are zero and there is no way to compute.
    next_pos->set_tracking_confidence(1.0f);
    LOG(INFO) << "no history. confidence : 1.0";
  } else {
    next_pos->set_tracking_confidence(ComputeTrackingConfidence(*next_pos));
    VLOG(1) << "confidence: " << next_pos->tracking_confidence();
  }
  next_pos->set_background_discrimination(background_discrimination);

  // Slowly decay current kinetic energy. Blend with current measurement based
  // on disparity (high = use previous value, low = use current one).
  next_pos->set_kinetic_energy(std::max(
      options_.kinetic_energy_decay() * curr_pos.kinetic_energy(),
      kinetic_average * (1.0f - std::abs(next_pos->motion_disparity()))));

  float inlier_max = curr_pos.inlier_sum();
  int num_tracked_frames_in_history = 0;
  for (auto entry : history) {
    inlier_max = std::max(inlier_max, entry->inlier_sum());
    if (entry->track_status() == MotionBoxState::BOX_TRACKED) {
      num_tracked_frames_in_history++;
    }
  }

  const float inlier_ratio =
      inlier_max > 0 ? (next_pos->inlier_sum() / (inlier_max + 1e-3f)) : 0.0f;

  next_pos->set_inlier_ratio(inlier_ratio);

  const bool is_perfect_fit = inlier_ratio > 0.85f && inlier_ratio < 1.15f;

  // num_tracked_frames_in_history has to be greater than 1, since the first
  // frame is marked as BOX_TRACKED in ResetAtFrame.
  if (options_.cancel_tracking_with_occlusion_options().activated() &&
      curr_pos.track_status() != MotionBoxState::BOX_DUPLICATED &&
      num_tracked_frames_in_history > 1 &&
      inlier_ratio < options_.cancel_tracking_with_occlusion_options()
                         .min_inlier_ratio()) {
    next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
    LOG(INFO) << "inlier_ratio: " << inlier_ratio
              << " too small. Stop tracking. inlier_max: " << inlier_max
              << ". length in history: " << history.size();
    return;
  }

  // Blend measured object motion based on motion disparity, i.e. the more the
  // measured and previous motion agree, the less the smoothing. This is to
  // propagate the box in the direction of the previous object motion
  // in case tracking has been lost.
  // Allow new measurements to propagate slowly.
  if (valid_background_model && !is_perfect_fit) {
    // Always move some fraction in the direction of the measured object
    // even if deemed in disagreement with previous motion.
    const float kMinimumBlend = 0.2f;
    object_translation = Lerp(
        object_translation, prev_object_motion,
        std::min(1.0f - kMinimumBlend, std::abs(next_pos->motion_disparity())));
  }

  if (curr_pos.has_pnp_homography()) {
    ApplyObjectMotionPerspectively(curr_pos, pnp_homography, domain_x, domain_y,
                                   next_pos);
  } else {
    ApplyObjectMotion(curr_pos, object_translation, object_similarity,
                      object_homography, options_, next_pos);

    ApplyCameraTrackingDegrees(curr_pos, motion_frame.background_model,
                               options_, Vector2_f(domain_x, domain_y),
                               next_pos);
  }

  if (next_pos->has_quad() &&
      !ObjectMotionValidator::IsValidQuad(next_pos->quad())) {
    LOG(ERROR) << "Quad is not convex. Cancel tracking.";
    next_pos->set_track_status(MotionBoxState::BOX_UNTRACKED);
    return;
  }

  // Storing pre-comp weights.
  const std::vector<float>& internal_weights =
      options_.use_post_estimation_weights_for_state() ? inlier_weights
                                                       : prior_weights;

  StoreInternalState(vectors, internal_weights, aspect_ratio,
                     next_pos->mutable_internal());

  // Compute center of inliers in next frame and change in scale for inliers.
  Vector2_f inlier_center;
  Vector2_f inlier_extent;
  float min_inlier_weight = 2.0f;  // Only update inlier_center if more
                                   // inliers than specified are found.
  ComputeInlierCenterAndExtent(vectors, inlier_weights, inlier_density,
                               *next_pos, &min_inlier_weight, &inlier_center,
                               &inlier_extent);

  // Determine difference to previous estimate.
  Vector2_f prev_inlier_center(InlierCenter(curr_pos));
  const float rel_inlier_center_diff =
      (inlier_center - prev_inlier_center).Norm() /
      MotionBoxSize(curr_pos).Norm();

  // Smooth with previous location, based on relative inlier difference.
  // A difference of 1.0 is mapped to a weight of 1.0 (total outlier).
  // Blend weight is maxed out at .6f to always allow measurements to propagate
  // over time (assuming high motion discrimination).
  const float center_blend =
      std::min(Lerp(0.95f, 0.6f, background_discrimination),
               rel_inlier_center_diff) *
      curr_pos.prior_weight();
  inlier_center = Lerp(inlier_center, prev_inlier_center, center_blend);

  next_pos->set_inlier_center_x(inlier_center.x());
  next_pos->set_inlier_center_y(inlier_center.y());

  // Update extent only when sufficient inliers are present.
  // TODO: This is too hacky, evaluate.
  if (min_inlier_weight > 30) {
    Vector2_f prev_inlier_extent(curr_pos.inlier_width(),
                                 curr_pos.inlier_height());
    // Blend with previous extent based on prior and discrimination.
    inlier_extent = Lerp(
        inlier_extent, prev_inlier_extent,
        curr_pos.prior_weight() * Lerp(1.0, 0.85, background_discrimination));
    next_pos->set_inlier_width(inlier_extent.x());
    next_pos->set_inlier_height(inlier_extent.y());
  }

  VLOG(1) << "Inlier extent " << next_pos->inlier_width() << " , "
          << next_pos->inlier_height();

  // Spring force applied to the inlier_center is modulated by the background
  // discrimination. Motivation: Low background discrimination leads to inlier
  // center more biased towards previous result due to update weight being
  // tampered down. Always apply a minimum force.
  // TODO: During challenging (low inlier) situations this can
  // save the lock onto objects. Cook up a condition to set min spring force to
  // 0.25 or so.
  constexpr float kMinSpringForceFraction = 0.0;
  ApplySpringForce(inlier_center, options_.inlier_center_relative_distance(),
                   std::min(1.0f, options_.inlier_spring_force() *
                                      std::max(kMinSpringForceFraction,
                                               background_discrimination)),
                   next_pos);

  if (options_.compute_spatial_prior()) {
    // Blend based on object multiplier using high prior weight for low
    // multipliers.
    // Magic update numbers, prior is not important for tracking, only for
    // visualization purposes.
    const float prior_weight = Lerp(0.98, 0.85, background_discrimination);
    ComputeSpatialPrior(true, true, prior_weight, next_pos);
  }

  // Update velocity.
  float velocity_update_weight =
      is_perfect_fit
          ? 0.0f
          : (options_.velocity_update_weight() * curr_pos.prior_weight());
  // Computed object motion is completely random when background model is
  // invalid. Use previous motion in this case.
  if (!valid_background_model) {
    velocity_update_weight = 1.0f;
  }

  next_pos->set_dx(
      Lerp(object_translation.x(), curr_pos.dx(), velocity_update_weight));
  next_pos->set_dy(
      Lerp(object_translation.y(), curr_pos.dy(), velocity_update_weight));

  // Update prior.
  if (valid_background_model) {
    next_pos->set_prior_weight(std::min(
        1.0f, curr_pos.prior_weight() + options_.prior_weight_increase()));
  } else {
    next_pos->set_prior_weight(std::max(
        0.0f, curr_pos.prior_weight() - options_.prior_weight_increase()));
  }

  next_pos->set_track_status(MotionBoxState::BOX_TRACKED);
}

void MotionVectorFrameFromTrackingData(const TrackingData& tracking_data,
                                       MotionVectorFrame* motion_vector_frame) {
  CHECK(motion_vector_frame != nullptr);

  const auto& motion_data = tracking_data.motion_data();
  float aspect_ratio = tracking_data.frame_aspect();
  if (aspect_ratio < 0.1 || aspect_ratio > 10.0f) {
    LOG(ERROR) << "Aspect ratio : " << aspect_ratio << " is out of bounds. "
               << "Resetting to 1.0.";
    aspect_ratio = 1.0f;
  }

  float scale_x, scale_y;
  // Normalize longest dimension to 1 under aspect ratio preserving scaling.
  ScaleFromAspect(aspect_ratio, false, &scale_x, &scale_y);

  scale_x /= tracking_data.domain_width();
  scale_y /= tracking_data.domain_height();

  const bool use_background_model =
      !(tracking_data.frame_flags() & TrackingData::FLAG_BACKGROUND_UNSTABLE);

  Homography homog_scale = HomographyAdapter::Embed(
      AffineAdapter::FromArgs(0, 0, scale_x, 0, 0, scale_y));

  Homography inv_homog_scale = HomographyAdapter::Embed(
      AffineAdapter::FromArgs(0, 0, 1.0f / scale_x, 0, 0, 1.0f / scale_y));

  // Might be just the identity if not set.
  const Homography background_model = tracking_data.background_model();
  const Homography background_model_scaled =
      ModelCompose3(homog_scale, background_model, inv_homog_scale);

  motion_vector_frame->background_model.CopyFrom(background_model_scaled);
  motion_vector_frame->valid_background_model = use_background_model;
  motion_vector_frame->is_duplicated =
      tracking_data.frame_flags() & TrackingData::FLAG_DUPLICATED;
  motion_vector_frame->is_chunk_boundary =
      tracking_data.frame_flags() & TrackingData::FLAG_CHUNK_BOUNDARY;
  motion_vector_frame->aspect_ratio = tracking_data.frame_aspect();
  motion_vector_frame->motion_vectors.reserve(motion_data.row_indices_size());

  motion_vector_frame->motion_vectors.clear();
  const bool long_tracks = motion_data.track_id_size() > 0;

  for (int c = 0; c < motion_data.col_starts_size() - 1; ++c) {
    const float x = c;
    const float scaled_x = x * scale_x;

    for (int r = motion_data.col_starts(c),
             r_end = motion_data.col_starts(c + 1);
         r < r_end; ++r) {
      MotionVector motion_vector;

      const float y = motion_data.row_indices(r);
      const float scaled_y = y * scale_y;

      const float dx = motion_data.vector_data(2 * r);
      const float dy = motion_data.vector_data(2 * r + 1);

      if (use_background_model) {
        Vector2_f loc(x, y);
        Vector2_f background_motion =
            HomographyAdapter::TransformPoint(background_model, loc) - loc;
        motion_vector.background = Vector2_f(background_motion.x() * scale_x,
                                             background_motion.y() * scale_y);
      }

      motion_vector.pos = Vector2_f(scaled_x, scaled_y);
      motion_vector.object = Vector2_f(dx * scale_x, dy * scale_y);

      if (long_tracks) {
        motion_vector.track_id = motion_data.track_id(r);
      }
      motion_vector_frame->motion_vectors.push_back(motion_vector);
    }
  }
}

void FeatureAndDescriptorFromTrackingData(
    const TrackingData& tracking_data, std::vector<Vector2_f>* features,
    std::vector<std::string>* descriptors) {
  const auto& motion_data = tracking_data.motion_data();
  float aspect_ratio = tracking_data.frame_aspect();
  if (aspect_ratio < 0.1 || aspect_ratio > 10.0f) {
    LOG(ERROR) << "Aspect ratio : " << aspect_ratio << " is out of bounds. "
               << "Resetting to 1.0.";
    aspect_ratio = 1.0f;
  }

  if (motion_data.feature_descriptors_size() == 0) {
    LOG(WARNING) << "Feature descriptors not exist";
    return;
  }

  float scale_x, scale_y;
  // Normalize longest dimension to 1 under aspect ratio preserving scaling.
  ScaleFromAspect(aspect_ratio, false, &scale_x, &scale_y);
  scale_x /= tracking_data.domain_width();
  scale_y /= tracking_data.domain_height();

  features->clear();
  descriptors->clear();

  for (int c = 0; c < motion_data.col_starts_size() - 1; ++c) {
    const float x = c;
    const float scaled_x = x * scale_x;

    for (int r = motion_data.col_starts(c),
             r_end = motion_data.col_starts(c + 1);
         r < r_end; ++r) {
      const std::string& descriptor = motion_data.feature_descriptors(r).data();

      if (absl::c_all_of(descriptor, [](char c) { return c == 0; })) {
        continue;
      }

      const float y = motion_data.row_indices(r);
      const float scaled_y = y * scale_y;

      features->emplace_back(scaled_x, scaled_y);
      descriptors->emplace_back(descriptor);
    }
  }
}

void InvertMotionVectorFrame(const MotionVectorFrame& input,
                             MotionVectorFrame* output) {
  CHECK(output != nullptr);

  output->background_model.CopyFrom(ModelInvert(input.background_model));
  output->valid_background_model = input.valid_background_model;
  output->is_duplicated = input.is_duplicated;
  output->is_chunk_boundary = input.is_chunk_boundary;
  output->duration_ms = input.duration_ms;
  output->aspect_ratio = input.aspect_ratio;
  output->motion_vectors.clear();
  output->motion_vectors.reserve(input.motion_vectors.size());
  output->actively_discarded_tracked_ids = input.actively_discarded_tracked_ids;

  const float aspect_ratio = input.aspect_ratio;
  float domain_x = 1.0f;
  float domain_y = 1.0f;
  ScaleFromAspect(aspect_ratio, false, &domain_x, &domain_y);

  // Explicit copy.
  for (auto motion_vec : input.motion_vectors) {
    motion_vec.background *= -1.0f;
    motion_vec.object *= -1.0f;

    motion_vec.pos -= motion_vec.background + motion_vec.object;

    // Inverted vector might be out of bound.
    if (motion_vec.pos.x() < 0.0f || motion_vec.pos.x() > domain_x ||
        motion_vec.pos.y() < 0.0f || motion_vec.pos.y() > domain_y) {
      continue;
    }

    // Approximately 40 - 60% of all inserts happen to be at the end.
    if (output->motion_vectors.empty() ||
        MotionVectorComparator()(output->motion_vectors.back(), motion_vec)) {
      output->motion_vectors.push_back(motion_vec);
    } else {
      output->motion_vectors.insert(
          std::lower_bound(output->motion_vectors.begin(),
                           output->motion_vectors.end(), motion_vec,
                           MotionVectorComparator()),
          motion_vec);
    }
  }
}

float TrackingDataDurationMs(const TrackingDataChunk::Item& item) {
  return (item.timestamp_usec() - item.prev_timestamp_usec()) * 1e-3f;
}

void GetFeatureIndicesWithinBox(const std::vector<Vector2_f>& features,
                                const MotionBoxState& box_state,
                                const Vector2_f& box_scaling,
                                float max_enlarge_size, int min_num_features,
                                std::vector<int>* inlier_indices) {
  CHECK(inlier_indices);
  inlier_indices->clear();

  if (features.empty()) return;
  std::array<Vector3_f, 4> box_lines;
  if (!MotionBoxLines(box_state, box_scaling, &box_lines)) {
    LOG(ERROR) << "Error in computing MotionBoxLines.";
    return;
  }

  // If the box size isn't big enough to cover sufficient features to
  // reacquire the box, the following code will try to iteratively enlarge the
  // box size by half of 'max_enlarge_size' to include more features, but
  // maximimum twice.
  float distance_threshold = 0.0f;
  int inliers_count = 0;
  std::vector<bool> chosen(features.size(), false);
  std::vector<float> signed_distance(features.size());

  for (int j = 0; j < features.size(); ++j) {
    float max_dist = std::numeric_limits<float>::lowest();
    for (const Vector3_f& line : box_lines) {
      float dist =
          line.DotProd(Vector3_f(features[j].x(), features[j].y(), 1.0f));
      if (dist > max_enlarge_size) {
        max_dist = dist;
        break;
      }
      max_dist = std::max(dist, max_dist);
    }

    signed_distance[j] = max_dist;
    if (signed_distance[j] < distance_threshold) {
      ++inliers_count;
      chosen[j] = true;
      inlier_indices->push_back(j);
    }
  }

  const float box_enlarge_step = max_enlarge_size * 0.5f;
  while (inliers_count < min_num_features) {
    distance_threshold += box_enlarge_step;
    if (distance_threshold > max_enlarge_size) break;
    for (int j = 0; j < features.size(); ++j) {
      if (chosen[j]) continue;
      if (signed_distance[j] < distance_threshold) {
        ++inliers_count;
        chosen[j] = true;
        inlier_indices->push_back(j);
      }
    }
  }
}

}  // namespace mediapipe.