Eigen::Matrix RelativePoseFromEssential()

in opensfm/src/geometry/relative_pose.h [11:82]
59 lines of code
10 McCabe index (conditional complexity)

Eigen::Matrix<double, 3, 4> RelativePoseFromEssential(
    const Eigen::Matrix3d& essential, IT begin, IT end) {
  Eigen::JacobiSVD<Eigen::Matrix3d> SVD(
      essential, Eigen::ComputeFullV | Eigen::ComputeFullU);
  Eigen::Matrix3d U = SVD.matrixU();
  Eigen::Matrix3d Vt = SVD.matrixV().transpose();

  // Last column of U is undetermined since d = (a a 0).
  if (U.determinant() < 0) {
    U.col(2) *= -1;
  }
  // Last row of Vt is undetermined since d = (a a 0).
  if (Vt.determinant() < 0) {
    Vt.row(2) *= -1;
  }

  Eigen::Matrix3d W;
  W << 0, -1, 0, 1, 0, 0, 0, 0, 1;

  Eigen::Matrix<double, 2, 3> bearings;
  Eigen::Matrix<double, 2, 3> centers;
  centers.row(0) = Eigen::Vector3d::Zero();

  auto best_decomposition = std::make_pair(0, Eigen::Matrix<double, 3, 4>());
  for (int i = 0; i < 2; ++i) {
    Eigen::Vector3d translation;
    if (i == 0) {
      translation = U.col(2);
    } else {
      translation = -U.col(2);
    }
    translation.normalize();
    for (int j = 0; j < 2; ++j) {
      Eigen::Matrix3d rotation;
      if (j == 0) {
        rotation = U * W * Vt;
      } else {
        rotation = U * W.transpose() * Vt;
      }

      // Since rotation=Rcamera and translation=Tcamera parametrization
      centers.row(1) = -rotation.transpose() * translation;

      double score = 0;
      for (IT it = begin; it != end; ++it) {
        bearings.row(0) = it->first;
        bearings.row(1) = rotation.transpose() * it->second;
        const auto point =
            geometry::TriangulateTwoBearingsMidpointSolve(centers, bearings);
        if (!point.first) {
          continue;
        }

        const Eigen::Vector3d projected_x = point.second.normalized();
        const Eigen::Vector3d projected_y =
            (rotation * point.second + translation).normalized();
        score +=
            ((projected_x.dot(it->first) + projected_y.dot(it->second)) * 0.5);
      }

      if (score > best_decomposition.first) {
        Eigen::Matrix<double, 3, 4> RT;
        RT.block<3, 3>(0, 0) = rotation;
        RT.block<3, 1>(0, 3) = translation;
        best_decomposition = std::make_pair(score, RT);
      }
    }
  }

  // Rcamera and Tcamera parametrization
  return best_decomposition.second;
}