Status KeyframeVisualOdometry::EstimatePose(ViewIndex view_index) {
  View::Ptr view = View::GetView(view_index);
  CHECK_NOTNULL(view.get());

  // Use PnP RANSAC to find the pose of this camera using the 2D<-->3D matches.
  PnPRansacProblem pnp_problem;
  pnp_problem.SetIntrinsics(intrinsics_);

  std::vector<Observation::Ptr> matched_observations;
  view->MatchedObservations(&matched_observations);
  printf("(4) at the top, had %lu matches\n", matched_observations.size());

  // Make sure the observations are only of triangulated landmarks.
  std::vector<Observation::Ptr> valid_observations;
  for (const auto& observation : matched_observations) {
    CHECK_NOTNULL(observation.get());
    if (observation->GetLandmark()->IsEstimated())
      valid_observations.push_back(observation);
  }
  pnp_problem.SetData(valid_observations);
  printf("valid observations: %lu\n", valid_observations.size());

  Ransac<Observation::Ptr, PnPRansacModel> pnp_solver;
  pnp_solver.SetOptions(options_.pnp_ransac_options);
  pnp_solver.Run(pnp_problem);

  // If RANSAC fails, set that the next frame should be a keyframe and return.
  if (!pnp_problem.SolutionFound()) {
    initialize_new_keyframe_ = true;
    return Status::Cancelled(
        "Failed to compute new camera pose with PnP RANSAC.");
  }

  // Get the camera pose from RANSAC.
  const CameraExtrinsics& computed_extrinsics =
      pnp_problem.Model().camera_.Extrinsics();
  view->MutableCamera().SetExtrinsics(computed_extrinsics);

  // If the computed relative translation from the last keyframe is large
  // enough, it's time to initialize a new keyframe on the next iteration.
  View::Ptr keyframe = View::GetView(current_keyframe_);
  CHECK_NOTNULL(keyframe.get());
  const Pose T1 = keyframe->Camera().Extrinsics().WorldToCamera();
  const Pose T2 = computed_extrinsics.WorldToCamera();
  const Pose delta = T1.Delta(T2);
  if (delta.Translation().norm() > options_.min_keyframe_translation ||
      delta.AxisAngle().norm() > options_.min_keyframe_rotation) {
    initialize_new_keyframe_ = true;
  }

  return Status::Ok();
}