<commit_before>#pragma once

#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <ceres/tiny_solver.h>
#include <ceres/rotation.h>
#include <ceres/tiny_solver_autodiff_function.h>
#include "triangulation.h"

template <class IT>
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;

      int are_in_front = 0;
      for (IT it = begin; it != end; ++it) {
        bearings.row(0) = it->first;
        bearings.row(1) = rotation.transpose()*it->second;
        const Eigen::Vector3d point = geometry::TriangulateTwoBearingsMidpointSolve(centers, bearings);
        const bool is_in_front =
            bearings.row(0).dot(point) > 0.0 &&
            bearings.row(1).dot(rotation * point + translation) > 0.0;
        are_in_front += is_in_front;

        const auto maximum_possible = are_in_front + (end - it);
        if (maximum_possible < best_decomposition.first) {
          break;
        }
      }

      if (are_in_front > 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(are_in_front, RT);
      }
    }
  }

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

template< class IT >
struct RelativePoseCost {
  RelativePoseCost(IT begin, IT end): begin_(begin), end_(end){}
  int NumResiduals() const {
    return (end_-begin_)+1;
  }

  template<typename T>
  bool operator()(const T* const parameters, T* residuals) const {
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> rotation(parameters);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> translation(parameters+3);
    Eigen::Matrix<T, 3, 1> rotation_transpose = -rotation;

    Eigen::Matrix<T, 2, 3> bearings;
    Eigen::Matrix<T, 2, 3> centers;
    centers.row(0) = Eigen::Matrix<T, 3, 1>::Zero();<commit_msg>fix: use reprojection based instead<commit_after>#pragma once

#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <ceres/tiny_solver.h>
#include <ceres/rotation.h>
#include <ceres/tiny_solver_autodiff_function.h>
#include "triangulation.h"

template <class IT>
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 Eigen::Vector3d point = geometry::TriangulateTwoBearingsMidpointSolve(centers, bearings);
        
        const auto projected_x = point.normalized();
        const auto projected_y = rotation*point+translation;
        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;
}

template< class IT >
struct RelativePoseCost {
  RelativePoseCost(IT begin, IT end): begin_(begin), end_(end){}
  int NumResiduals() const {
    return (end_-begin_)+1;
  }

  template<typename T>
  bool operator()(const T* const parameters, T* residuals) const {
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> rotation(parameters);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> translation(parameters+3);
    Eigen::Matrix<T, 3, 1> rotation_transpose = -rotation;

    Eigen::Matrix<T, 2, 3> bearings;
    Eigen::Matrix<T, 2, 3> centers;
    centers.row(0) = Eigen::Matrix<T, 3, 1>::Zero();
{'fraction_non_alphanumeric': 0.15628613910457625, 'fraction_numerical': 0.025441929621675203, 'mean_word_length': 3.0740242261103634, 'pattern_counts': {'":': 0, '<': 64, '<?xml version=': 0, '>': 61, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6943085', 'n_tokens_mistral': 2348, 'n_tokens_neox': 2170, 'n_words': 515}