#include "Quaca.h"
#include "catch.hpp"
#include <armadillo>
#include <complex>

TEST_CASE("Plate Green's tensor constructors work as expected",
          "[GreensTensorPlate]") {
  SECTION("Direct constructor") {

    double omega_p = 9;
    double gamma = 0.1;
    auto perm = std::make_shared<PermittivityDrude>(omega_p, gamma);
    auto refl = std::make_shared<ReflectionCoefficientsLocBulk>(perm);

    double v = 1E-5;
    double za = 0.1;
    double beta = 1E4;
    double delta_cut = 20;
    vec::fixed<2> rel_err = {1E-8, 1E-6};
    GreensTensorPlate Greens(v, beta, za, refl, delta_cut, rel_err);

    REQUIRE(Greens.get_za() == za);
    REQUIRE(Greens.get_delta_cut() == delta_cut);
    REQUIRE(Greens.get_rel_err_0() == rel_err(0));
    REQUIRE(Greens.get_rel_err_1() == rel_err(1));

    double omega = 1;
    double k = 10;
    std::complex<double> kappa;
    if (k < omega) {
      kappa = std::complex<double>(0., -sqrt(omega * omega - k * k));
    } else {
      kappa = std::complex<double>(sqrt(k * k - omega * omega), 0.);
    }

    std::complex<double> rp, rs;
    refl->calculate(omega, kappa, rp, rs);

    REQUIRE(real(Greens.get_r_s(omega, k)) == rs.real());
    REQUIRE(real(Greens.get_r_p(omega, k)) == rp.real());
    REQUIRE(imag(Greens.get_r_s(omega, k)) == rs.imag());
    REQUIRE(imag(Greens.get_r_p(omega, k)) == rp.imag());
  }

  SECTION("json file constructor") {
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    REQUIRE(Greens.get_za() == 0.1);
    REQUIRE(Greens.get_delta_cut() == 20);
    REQUIRE(Greens.get_rel_err_0() == 1E-8);
    REQUIRE(Greens.get_rel_err_1() == 1E-6);

    std::complex<double> rp, rs;
    double omega = 1;
    double k = 10;
    std::complex<double> kappa;
    if (k < omega) {
      kappa = std::complex<double>(0., -sqrt(omega * omega - k * k));
    } else {
      kappa = std::complex<double>(sqrt(k * k - omega * omega), 0.);
    }

    ReflectionCoefficientsLocBulk refl(
        "../data/test_files/GreensTensorPlate.json");
    refl.calculate(omega, kappa, rp, rs);

    REQUIRE(real(Greens.get_r_s(omega, k)) == rs.real());
    REQUIRE(real(Greens.get_r_p(omega, k)) == rp.real());
    REQUIRE(imag(Greens.get_r_s(omega, k)) == rs.imag());
    REQUIRE(imag(Greens.get_r_p(omega, k)) == rp.imag());
  }
}

TEST_CASE("The operations calculate_tensor and the integrand_2d_k coincide",
          "[GreensTensorPlate]") {
  // Here we considered also the volume element from the integration.
  const std::complex<double> I(0.0, 1.0);
  auto omega = GENERATE(-201,21,-1.2,3.2,103.12);
  auto k_x = GENERATE(-321.46,-3.54,1.21,32.1);
  auto k_y = GENERATE(-123.12,-0.0132,3.87,67.21);
  const double omega_p = 9;
  const double gamma = 0.1;
  const double v = 1e-2;
  const double za = 0.1;
  const double beta = 0.1;
  const double delta_cut = 30;
  vec::fixed<2> rel_err = {1E-8, 1E-6};

  auto perm = std::make_shared<PermittivityDrude>(omega_p, gamma);
  auto refl = std::make_shared<ReflectionCoefficientsLocBulk>(perm);
  GreensTensorPlate Greens(v, za, beta, refl, delta_cut, rel_err);

  // First, the calculate_tensor operation is used to generate the
  // Green's tensor with fancy_I
  cx_mat::fixed<3, 3> Green(fill::zeros);
  cx_mat::fixed<3, 3> Green_fancy_I_ct(fill::zeros);

  vec::fixed<2> kvec = { k_x, k_y };
  double omega_dp = omega + k_x * v;
  double k = sqrt(k_x * k_x + k_y * k_y);
  double cos_phi = k_x / k;
  std::complex<double> kappa =
      sqrt(std::complex<double>(k * k - omega_dp * omega_dp, 0.));
  kappa = std::complex<double>(std::abs(kappa.real()), -std::abs(kappa.imag()));
  double volume_element = std::abs(kappa) * k / (k - cos_phi * v * omega_dp);

  Greens.calculate_tensor(omega_dp, kvec, Green);

  Green *= volume_element;
  Green_fancy_I_ct = (Green - trans(Green)) / (2. * I);

  // Second, the integrand_2d_k operation is used
  cx_mat::fixed<3, 3> Green_fancy_I_ik2d(fill::zeros);
  double kappa_double;
  if (kappa.real() == 0.) {
    kappa_double = kappa.imag();
  } else {
    kappa_double = kappa.real();
  }

  kvec(0) = acos(k_x / k);

  // loop over all indices
  for (size_t i = 0; i < 3; i++) {
    for (size_t j = 0; j < 3; j++) {
      Green_fancy_I_ik2d(i, j) =
          (2 * M_PI) * Greens.integrand_2d_k(kappa_double, omega, kvec(0),
                                             {i, j}, IM, UNIT);
      if (i != j) {
        // As the prefactor I can not be evaluated in a purely real integration
        // routine, it was dropped in integrate_2d_k and has to be inserted here
        Green_fancy_I_ik2d(i, j) *= I;
      }
    }
  }
  //Ensure that the results are non trivial
  REQUIRE(!Green_fancy_I_ct.is_zero());
  REQUIRE(!Green_fancy_I_ik2d.is_zero());

  REQUIRE(approx_equal(Green_fancy_I_ct, Green_fancy_I_ik2d, "reldiff", 10E-4));
}

TEST_CASE("Plate Green's tensor fulfills physical relations",
          "[GreensTensorPlate]") {

  SECTION("Green's tensor obeys reciprocity") {
    auto omega = GENERATE(-201,21,-1.2,3.2,87.2);
    auto k_x = GENERATE(-321.46,-3.54,1.21,32.1);
    auto k_y = GENERATE(-123.12,-0.0132,3.87,67.21);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

    Greens.calculate_tensor(omega, {k_x, k_y}, Greens_lhs);
    Greens.calculate_tensor(omega, {-k_x, -k_y}, Greens_rhs);

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, strans(Greens_rhs), "reldiff", 10E-15));
  }

  SECTION("Green's tensor obeys reality condition") {
    auto omega = GENERATE(-201,21,-1.2,3.2,87.2);
    auto k_x = GENERATE(-321.46,-3.54,1.21,32.1);
    auto k_y = GENERATE(-123.12,-0.0132,3.87,67.21);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

    Greens.calculate_tensor(omega, {k_x, k_y}, Greens_lhs);
    Greens.calculate_tensor(-omega, {k_x, k_y}, Greens_rhs);

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, trans(Greens_rhs), "reldiff", 10E-15));
  }
}

TEST_CASE("Integrated Green's tensor works properly", "[GreensTensorPlate]") {

  SECTION("Integral over Green_fancy_I obeys the crossing relation") {
    auto omega = GENERATE(1.543,23.54,76.12);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

    Greens.integrate_k(omega, Greens_lhs, IM, UNIT);
    Greens.integrate_k(-omega, Greens_rhs, IM, UNIT);

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, -strans(Greens_rhs), "reldiff", 10E-4));
  }

  SECTION("Integral over Green_fancy_R obeys the crossing relation") {
    auto omega = GENERATE(1.543,23.54,89.12);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

    Greens.integrate_k(omega, Greens_lhs, RE, UNIT);
    Greens.integrate_k(-omega, Greens_rhs, RE, UNIT);

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, strans(Greens_rhs), "reldiff", 10E-4));
  }

  SECTION("Integral over Green_fancy_I_kv obeys the crossing relation") {
    auto omega = GENERATE(5.43e-2,2.354e-3,8.12e-4);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);

    Greens.integrate_k(omega, Greens_lhs, IM, KV);
    Greens.integrate_k(-omega, Greens_rhs, IM, KV);

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, strans(Greens_rhs), "reldiff", 10E-4));
  }

  SECTION("Integral over Green fancy_I, temp obeys the crossing relation") {
    auto omega = GENERATE(5.43e-2,2.354e-3,8.12e-4);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs1(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs2(fill::zeros);

    Greens.integrate_k(-omega, Greens_lhs, IM, TEMP);
    Greens.integrate_k(omega, Greens_rhs1, IM, UNIT);
    Greens.integrate_k(omega, Greens_rhs2, IM, TEMP);
    Greens_rhs = -Greens_rhs1 + Greens_rhs2;

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, strans(Greens_rhs), "reldiff", 10E-4));
  }

  SECTION("Integral over Green_fancy_I_kv_temp obeys the crossing relation") {
    auto omega = GENERATE(5.43e-2,2.354e-3,8.12e-4);
    GreensTensorPlate Greens("../data/test_files/GreensTensorPlate.json");

    cx_mat::fixed<3, 3> Greens_lhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs1(fill::zeros);
    cx_mat::fixed<3, 3> Greens_rhs2(fill::zeros);

    Greens.integrate_k(-omega, Greens_lhs, IM, KV_TEMP);
    Greens.integrate_k(omega, Greens_rhs1, IM, KV);
    Greens.integrate_k(omega, Greens_rhs2, IM, KV_TEMP);

    Greens_rhs = -Greens_rhs1 + Greens_rhs2;

    //Ensure that the results are non trivial
    REQUIRE(!Greens_lhs.is_zero());
    REQUIRE(!Greens_rhs.is_zero());

    REQUIRE(approx_equal(Greens_lhs, -strans(Greens_rhs), "reldiff", 10E-4));
  }
}

TEST_CASE("Integrated Green's tensor matches asymptotes",
          "[GreensTensorPlate]") {
  SECTION("Low-frequency asymptote of fancy_I") {
    // \omega << \omega_p and \omega << v/z_a
    std::complex<double> I(0.0, 1.0);
    double omega_p = 9;
    double gamma = 0.1;
    double v = 1e-5;
    double za = 0.1;
    auto omega = GENERATE(-0.1*1e-8, -0.54*10e-8,1.23*1e-7,0.1*1e-6);
    double delta_cut = 30;
    vec::fixed<2> rel_err = {1E-8, 1E-6};

    auto perm = std::make_shared<PermittivityDrude>(omega_p, gamma);
    auto refl = std::make_shared<ReflectionCoefficientsLocBulk>(perm);
    GreensTensorPlate Greens(v, 0.1, za, refl, delta_cut, rel_err);

    cx_mat::fixed<3, 3> GT_Ana(fill::zeros);
    GT_Ana(0, 0) = 2 * omega * gamma / pow(omega_p, 2) / pow(2 * za, 3);
    GT_Ana(1, 1) = GT_Ana(0, 0);
    GT_Ana(2, 2) = 2. * GT_Ana(0, 0);
    GT_Ana(0, 2) = (2 * 3 * v * gamma / (pow(omega_p, 2) * pow(2 * za, 4))) / I;
    GT_Ana(2, 0) = -GT_Ana(0, 2);

    cx_mat::fixed<3, 3> GT_Num(fill::zeros);
    Greens.integrate_k(omega, GT_Num, IM, UNIT);

    //Ensure that the results are non trivial
    REQUIRE(!GT_Ana.is_zero());
    REQUIRE(!GT_Num.is_zero());

    REQUIRE(approx_equal(GT_Ana, GT_Num, "reldiff", 10E-4));
  }

  SECTION("Low-frequency and low temperature asymptote of fancy_I_temp") {
    // \omega << \omega_p and \omega << v/z_a
    std::complex<double> I(0.0, 1.0);
    double omega_p = 9;
    double gamma = 0.1;
    double v = 1e-5;
    double za = 0.1;
    double beta = 1e12;
    auto omega = GENERATE( 5.32*1e-8,3.54*1e-7,1e-6);
    double delta_cut = 30;
    vec::fixed<2> rel_err = {1E-8, 1E-6};

    auto perm = std::make_shared<PermittivityDrude>(omega_p, gamma);
    auto refl = std::make_shared<ReflectionCoefficientsLocBulk>(perm);
    GreensTensorPlate Greens(v, beta, za, refl, delta_cut, rel_err);

    cx_mat::fixed<3, 3> GT_Ana(fill::zeros);
    double eta = omega * 2 * za / v;
    double rho = gamma / pow(omega_p, 2);
    GT_Ana(0, 0) =
        (v * rho * 2. / (pow(2 * za, 4) * M_PI)) * (0.5 * M_PI * eta + 4.);
    GT_Ana(1, 1) =
        (v * rho * 2. / (pow(2 * za, 4) * M_PI)) * (0.5 * M_PI * eta + 2.);
    GT_Ana(2, 2) = (v * rho * 2. / (pow(2 * za, 4) * M_PI)) * (M_PI * eta + 6.);
    GT_Ana(2, 0) = I * (v * rho * 2. / (pow(2 * za, 4) * M_PI)) *
                   (3. / 2. * M_PI + 2. * eta);
    GT_Ana(0, 2) = -GT_Ana(2, 0);

    cx_mat::fixed<3, 3> GT_Num(fill::zeros);
    Greens.integrate_k(omega, GT_Num, IM, TEMP);

    //Ensure that the results are non trivial
    REQUIRE(!GT_Ana.is_zero());
    REQUIRE(!GT_Num.is_zero());

    REQUIRE(approx_equal(GT_Ana, GT_Num, "reldiff", 10E-4));
  }

  SECTION("Low-frequency and low temperature asymptote of fancy_I_temp_kv") {
    // \omega << \omega_p and \omega << v/z_a
    std::complex<double> I(0.0, 1.0);
    double omega_p = 9;
    double gamma = 0.1;
    double v = 1e-5;
    double za = 0.1;
    double beta = 1e12;
    auto omega = GENERATE( 5.32*1e-8,3.54*1e-7,1e-6);
    double delta_cut = 30;
    vec::fixed<2> rel_err = {1E-8, 1E-6};

    auto perm = std::make_shared<PermittivityDrude>(omega_p, gamma);
    auto refl = std::make_shared<ReflectionCoefficientsLocBulk>(perm);
    GreensTensorPlate Greens(v, beta, za, refl, delta_cut, rel_err);

    cx_mat::fixed<3, 3> GT_Ana(fill::zeros);
    double eta = omega * 2 * za / v;
    double rho = gamma / pow(omega_p, 2);
    GT_Ana(0, 0) =
        (v * rho * 2. / (pow(2 * za, 5) * M_PI)) * (0.5 * M_PI * 9 + 4. * eta);
    GT_Ana(1, 1) =
        (v * rho * 2. / (pow(2 * za, 5) * M_PI)) * (0.5 * M_PI * 3 + 2. * eta);
    GT_Ana(2, 2) = GT_Ana(0, 0) + GT_Ana(1, 1);
    GT_Ana(2, 0) = I * (v * rho * 2. / (pow(2 * za, 5) * M_PI)) *
                   (3. / 2. * M_PI * eta + 16.);
    GT_Ana(0, 2) = -GT_Ana(2, 0);

    cx_mat::fixed<3, 3> GT_Num(fill::zeros);
    Greens.integrate_k(omega, GT_Num, IM, KV_TEMP);

    //Ensure that the results are non trivial
    REQUIRE(!GT_Ana.is_zero());
    REQUIRE(!GT_Num.is_zero());

    REQUIRE(approx_equal(GT_Ana, GT_Num, "reldiff", 10E-4));
  }

  SECTION("Low-frequency and high temperature asymptote of fancy_I_temp") {
    // \omega << \omega_p and \omega << v/z_a
    std::complex<double> I(0.0, 1.0);
    double omega_p = 9;
    double gamma = 0.1;
    double v = 1e-5;
    double za = 0.1;
    double beta = 1e-1;
    auto omega = GENERATE(3.21*1e-8,1.23*1e-7);
    double delta_cut = 30;
    vec::fixed<2> rel_err = {1E-8, 1E-6};

    auto perm = std::make_shared<PermittivityDrude>(omega_p, gamma);
    auto refl = std::make_shared<ReflectionCoefficientsLocBulk>(perm);
    GreensTensorPlate Greens(v, beta, za, refl, delta_cut, rel_err);

    cx_mat::fixed<3, 3> GT_lhs(fill::zeros);
    Greens.integrate_k(omega, GT_lhs, IM, TEMP);

    cx_mat::fixed<3, 3> GT_rhs(fill::zeros);
    GT_rhs(0, 0) = 2 * gamma / (pow(omega_p, 2) * pow(2 * za, 3) * beta);
    GT_rhs(1, 1) = GT_rhs(0, 0);
    GT_rhs(2, 2) = GT_rhs(1, 1) + GT_rhs(0, 0);
    GT_rhs(0, 2) =
        0.5 * (2 * 3 * v * gamma / (pow(omega_p, 2) * pow(2 * za, 4))) / I;
    GT_rhs(2, 0) = -GT_rhs(0, 2);

    //Ensure that the results are non trivial
    REQUIRE(!GT_lhs.is_zero());
    REQUIRE(!GT_rhs.is_zero());

    REQUIRE(approx_equal(GT_lhs, GT_rhs, "reldiff", 10E-4));
  }
}


//  Copyright John Maddock 2006, 2007.
//  Copyright Paul A. Bristow 2006, 2007.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_STATS_TRIANGULAR_HPP
#define BOOST_STATS_TRIANGULAR_HPP

// http://mathworld.wolfram.com/TriangularDistribution.html
// http://en.wikipedia.org/wiki/Triangular_distribution

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/constants/constants.hpp>

#include <utility>

namespace boost{ namespace math
{
  namespace detail
  {
    template <class RealType, class Policy>
    inline bool check_triangular_lower(
      const char* function,
      RealType lower,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(lower))
      { // Any finite value is OK.
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "Lower parameter is %1%, but must be finite!", lower, pol);
        return false;
      }
    } // bool check_triangular_lower(

    template <class RealType, class Policy>
    inline bool check_triangular_mode(
      const char* function,
      RealType mode,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(mode))
      { // any finite value is OK.
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "Mode parameter is %1%, but must be finite!", mode, pol);
        return false;
      }
    } // bool check_triangular_mode(

    template <class RealType, class Policy>
    inline bool check_triangular_upper(
      const char* function,
      RealType upper,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(upper))
      { // any finite value is OK.
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "Upper parameter is %1%, but must be finite!", upper, pol);
        return false;
      }
    } // bool check_triangular_upper(

    template <class RealType, class Policy>
    inline bool check_triangular_x(
      const char* function,
      RealType const& x,
      RealType* result, const Policy& pol)
    {
      if((boost::math::isfinite)(x))
      { // Any finite value is OK
        return true;
      }
      else
      { // Not finite: infinity or NaN.
        *result = policies::raise_domain_error<RealType>(
          function,
          "x parameter is %1%, but must be finite!", x, pol);
        return false;
      }
    } // bool check_triangular_x

    template <class RealType, class Policy>
    inline bool check_triangular(
      const char* function,
      RealType lower,
      RealType mode,
      RealType upper,
      RealType* result, const Policy& pol)
    {
      if ((check_triangular_lower(function, lower, result, pol) == false)
        || (check_triangular_mode(function, mode, result, pol) == false)
        || (check_triangular_upper(function, upper, result, pol) == false))
      { // Some parameter not finite.
        return false;
      }
      else if (lower >= upper) // lower == upper NOT useful.
      { // lower >= upper.
        *result = policies::raise_domain_error<RealType>(
          function,
          "lower parameter is %1%, but must be less than upper!", lower, pol);
        return false;
      }
      else
      { // Check lower <= mode <= upper.
        if (mode < lower)
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "mode parameter is %1%, but must be >= than lower!", lower, pol);
          return false;
        }
        if (mode > upper)
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "mode parameter is %1%, but must be <= than upper!", upper, pol);
          return false;
        }
        return true; // All OK.
      }
    } // bool check_triangular
  } // namespace detail

  template <class RealType = double, class Policy = policies::policy<> >
  class triangular_distribution
  {
  public:
    typedef RealType value_type;
    typedef Policy policy_type;

    triangular_distribution(RealType lower = -1, RealType mode = 0, RealType upper = 1)
      : m_lower(lower), m_mode(mode), m_upper(upper) // Constructor.
    { // Evans says 'standard triangular' is lower 0, mode 1/2, upper 1,
      // has median sqrt(c/2) for c <=1/2 and 1 - sqrt(1-c)/2 for c >= 1/2
      // But this -1, 0, 1 is more useful in most applications to approximate normal distribution,
      // where the central value is the most likely and deviations either side equally likely.
      RealType result;
      detail::check_triangular("boost::math::triangular_distribution<%1%>::triangular_distribution",lower, mode, upper, &result, Policy());
    }
    // Accessor functions.
    RealType lower()const
    {
      return m_lower;
    }
    RealType mode()const
    {
      return m_mode;
    }
    RealType upper()const
    {
      return m_upper;
    }
  private:
    // Data members:
    RealType m_lower;  // distribution lower aka a
    RealType m_mode;  // distribution mode aka c
    RealType m_upper;  // distribution upper aka b
  }; // class triangular_distribution

  typedef triangular_distribution<double> triangular;

  template <class RealType, class Policy>
  inline const std::pair<RealType, RealType> range(const triangular_distribution<RealType, Policy>& /* dist */)
  { // Range of permissible values for random variable x.
    using boost::math::tools::max_value;
    return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
  }

  template <class RealType, class Policy>
  inline const std::pair<RealType, RealType> support(const triangular_distribution<RealType, Policy>& dist)
  { // Range of supported values for random variable x.
    // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
    return std::pair<RealType, RealType>(dist.lower(), dist.upper());
  }

  template <class RealType, class Policy>
  RealType pdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)
  {
    static const char* function = "boost::math::pdf(const triangular_distribution<%1%>&, %1%)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_triangular_x(function, x, &result, Policy()))
    {
      return result;
    }
    if((x < lower) || (x > upper))
    {
      return 0;
    }
    if (x == lower)
    { // (mode - lower) == 0 which would lead to divide by zero!
      return (mode == lower) ? 2 / (upper - lower) : RealType(0);
    }
    else if (x == upper)
    {
      return (mode == upper) ? 2 / (upper - lower) : RealType(0);
    }
    else if (x <= mode)
    {
      return 2 * (x - lower) / ((upper - lower) * (mode - lower));
    }
    else
    {  // (x > mode)
      return 2 * (upper - x) / ((upper - lower) * (upper - mode));
    }
  } // RealType pdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)

  template <class RealType, class Policy>
  inline RealType cdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)
  {
    static const char* function = "boost::math::cdf(const triangular_distribution<%1%>&, %1%)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_triangular_x(function, x, &result, Policy()))
    {
      return result;
    }
    if((x <= lower))
    {
      return 0;
    }
    if (x >= upper)
    {
      return 1;
    }
    // else lower < x < upper
    if (x <= mode)
    {
      return ((x - lower) * (x - lower)) / ((upper - lower) * (mode - lower));
    }
    else
    {
      return 1 - (upper - x) *  (upper - x) / ((upper - lower) * (upper - mode));
    }
  } // RealType cdf(const triangular_distribution<RealType, Policy>& dist, const RealType& x)

  template <class RealType, class Policy>
  RealType quantile(const triangular_distribution<RealType, Policy>& dist, const RealType& p)
  {
    BOOST_MATH_STD_USING  // for ADL of std functions (sqrt).
    static const char* function = "boost::math::quantile(const triangular_distribution<%1%>&, %1%)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks
    if(false == detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_probability(function, p, &result, Policy()))
    {
      return result;
    }
    if(p == 0)
    {
      return lower;
    }
    if(p == 1)
    {
      return upper;
    }
    RealType p0 = (mode - lower) / (upper - lower);
    RealType q = 1 - p;
    if (p < p0)
    {
      result = sqrt((upper - lower) * (mode - lower) * p) + lower;
    }
    else if (p == p0)
    {
      result = mode;
    }
    else // p > p0
    {
      result = upper - sqrt((upper - lower) * (upper - mode) * q);
    }
    return result;

  } // RealType quantile(const triangular_distribution<RealType, Policy>& dist, const RealType& q)

  template <class RealType, class Policy>
  RealType cdf(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)
  {
    static const char* function = "boost::math::cdf(const triangular_distribution<%1%>&, %1%)";
    RealType lower = c.dist.lower();
    RealType mode = c.dist.mode();
    RealType upper = c.dist.upper();
    RealType x = c.param;
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_triangular_x(function, x, &result, Policy()))
    {
      return result;
    }
    if (x <= lower)
    {
      return 1;
    }
    if (x >= upper)
    {
      return 0;
    }
    if (x <= mode)
    {
      return 1 - ((x - lower) * (x - lower)) / ((upper - lower) * (mode - lower));
    }
    else
    {
      return (upper - x) *  (upper - x) / ((upper - lower) * (upper - mode));
    }
  } // RealType cdf(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)

  template <class RealType, class Policy>
  RealType quantile(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)
  {
    BOOST_MATH_STD_USING  // Aid ADL for sqrt.
    static const char* function = "boost::math::quantile(const triangular_distribution<%1%>&, %1%)";
    RealType l = c.dist.lower();
    RealType m = c.dist.mode();
    RealType u = c.dist.upper();
    RealType q = c.param; // probability 0 to 1.
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, l, m, u, &result, Policy()))
    {
      return result;
    }
    if(false == detail::check_probability(function, q, &result, Policy()))
    {
      return result;
    }
    if(q == 0)
    {
      return u;
    }
    if(q == 1)
    {
      return l;
    }
    RealType lower = c.dist.lower();
    RealType mode = c.dist.mode();
    RealType upper = c.dist.upper();

    RealType p = 1 - q;
    RealType p0 = (mode - lower) / (upper - lower);
    if(p < p0)
    {
      RealType s = (upper - lower) * (mode - lower);
      s *= p;
      result = sqrt((upper - lower) * (mode - lower) * p) + lower;
    }
    else if (p == p0)
    {
      result = mode;
    }
    else // p > p0
    {
      result = upper - sqrt((upper - lower) * (upper - mode) * q);
    }
    return result;
  } // RealType quantile(const complemented2_type<triangular_distribution<RealType, Policy>, RealType>& c)

  template <class RealType, class Policy>
  inline RealType mean(const triangular_distribution<RealType, Policy>& dist)
  {
    static const char* function = "boost::math::mean(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0;  // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return (lower + upper + mode) / 3;
  } // RealType mean(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType variance(const triangular_distribution<RealType, Policy>& dist)
  {
    static const char* function = "boost::math::mean(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular(function, lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return (lower * lower + upper * upper + mode * mode - lower * upper - lower * mode - upper * mode) / 18;
  } // RealType variance(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType mode(const triangular_distribution<RealType, Policy>& dist)
  {
    static const char* function = "boost::math::mode(const triangular_distribution<%1%>&)";
    RealType mode = dist.mode();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular_mode(function, mode, &result, Policy()))
    { // This should never happen!
      return result;
    }
    return mode;
  } // RealType mode

  template <class RealType, class Policy>
  inline RealType median(const triangular_distribution<RealType, Policy>& dist)
  {
    BOOST_MATH_STD_USING // ADL of std functions.
    static const char* function = "boost::math::median(const triangular_distribution<%1%>&)";
    RealType mode = dist.mode();
    RealType result = 0; // of checks.
    if(false == detail::check_triangular_mode(function, mode, &result, Policy()))
    { // This should never happen!
      return result;
    }
    RealType lower = dist.lower();
    RealType upper = dist.upper();
    if (mode < (upper - lower) / 2)
    {
      return lower + sqrt((upper - lower) * (mode - lower)) / constants::root_two<RealType>();
    }
    else
    {
      return upper - sqrt((upper - lower) * (upper - mode)) / constants::root_two<RealType>();
    }
  } // RealType mode

  template <class RealType, class Policy>
  inline RealType skewness(const triangular_distribution<RealType, Policy>& dist)
  {
    BOOST_MATH_STD_USING  // for ADL of std functions
    using namespace boost::math::constants; // for root_two
    static const char* function = "boost::math::skewness(const triangular_distribution<%1%>&)";

    RealType lower = dist.lower();
    RealType mode = dist.mode();
    RealType upper = dist.upper();
    RealType result = 0; // of checks.
    if(false == boost::math::detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return root_two<RealType>() * (lower + upper - 2 * mode) * (2 * lower - upper - mode) * (lower - 2 * upper + mode) /
      (5 * pow((lower * lower + upper + upper + mode * mode - lower * upper - lower * mode - upper * mode), RealType(3)/RealType(2)));
  } // RealType skewness(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType kurtosis(const triangular_distribution<RealType, Policy>& dist)
  { // These checks may be belt and braces as should have been checked on construction?
    static const char* function = "boost::math::kurtosis(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType upper = dist.upper();
    RealType mode = dist.mode();
    RealType result = 0;  // of checks.
    if(false == detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return static_cast<RealType>(12)/5; //  12/5 = 2.4;
  } // RealType kurtosis_excess(const triangular_distribution<RealType, Policy>& dist)

  template <class RealType, class Policy>
  inline RealType kurtosis_excess(const triangular_distribution<RealType, Policy>& dist)
  { // These checks may be belt and braces as should have been checked on construction?
    static const char* function = "boost::math::kurtosis_excess(const triangular_distribution<%1%>&)";
    RealType lower = dist.lower();
    RealType upper = dist.upper();
    RealType mode = dist.mode();
    RealType result = 0;  // of checks.
    if(false == detail::check_triangular(function,lower, mode, upper, &result, Policy()))
    {
      return result;
    }
    return static_cast<RealType>(-3)/5; // - 3/5 = -0.6
    // Assuming mathworld really means kurtosis excess?  Wikipedia now corrected to match this.
  }

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_TRIANGULAR_HPP


#include <dlib/python.h>
#include <dlib/image_transforms.h>
#include <dlib/image_io.h>
#include <dlib/data_io.h>
#include <dlib/pipe.h>
#include <dlib/simd.h>
#include <thread>
#include <pybind11/stl.h>
#include "cuda_stuff.h"
#include <dlib/cuda/cuda_dlib.h>

namespace py = pybind11;

using namespace dlib;
using namespace std;

// ----------------------------------------------------------------------------------------

numpy_image<float> coherent_hough_transform (
    const hough_transform& ht,
    const numpy_image<unsigned char>& edges_,
    const numpy_image<float>& horz_,
    const numpy_image<float>& vert_
)
{
    auto edges = make_image_view(edges_);
    auto horz = make_image_view(horz_);
    auto vert = make_image_view(vert_);

    DLIB_CASSERT(have_same_dimensions(edges, horz, vert));

    array2d<matrix<double,2,2>> hcoherent(ht.size(), ht.size());
    for (auto& v : hcoherent)
        v = 0;

    auto record_hit = [&](const point& hough_point, const point& p, float val)
    {
        matrix<double,2,1> v = {horz[p.y()][p.x()],
                                vert[p.y()][p.x()]};

        hcoherent[hough_point.y()][hough_point.x()] += v*trans(v);
    };
    ht.perform_generic_hough_transform(edges, record_hit);

    numpy_image<float> himg_(ht.nr(), ht.nc());
    auto himg = make_image_view(himg_);
    for (long r = 0; r < himg.nr(); ++r)
    {
        for (long c = 0; c < himg.nc(); ++c)
        {
            matrix<double,0,1> ev = real_eigenvalues(hcoherent[r][c]);
            if (min(ev)/max(ev) < 0.30)
                himg[r][c] = max(ev);
            else
                himg[r][c] = 0;
        }
    }

    return himg_;
}

// ----------------------------------------------------------------------------------------

void discard_wacky_edge_groups (
    numpy_image<float>& edges_,
    const numpy_image<float>& horz_,
    const numpy_image<float>& vert_
)
{
    auto edges = make_image_view(edges_);
    auto horz = make_image_view(horz_);
    auto vert = make_image_view(vert_);

    DLIB_CASSERT(have_same_dimensions(edges, horz, vert));

    double dotprod_angle_thresh = cos(25*pi/180);

    auto connected_if = [&](const auto& img, const point& a, const point& b) {
        dpoint v1(horz[a.y()][a.x()], vert[a.y()][a.x()]);
        dpoint v2(horz[b.y()][b.x()], vert[b.y()][b.x()]);
        return (img[a.y()][a.x()] != 0 && img[b.y()][b.x()] != 0) && dot(v1,v2) > dotprod_angle_thresh;
    };

    array2d<uint32> labels;
    auto num_blobs = label_connected_blobs(edges, zero_pixels_are_background(), neighbors_8(), connected_if, labels);

    matrix<unsigned long,0,1> blob_sizes;
    get_histogram(labels, blob_sizes, num_blobs);

    // blank out short edges
    for (long r = 0; r < labels.nr(); ++r)
        for (long c = 0; c < labels.nc(); ++c)
            if (blob_sizes(labels[r][c]) < 20)
                edges[r][c] = 0;
}

// ----------------------------------------------------------------------------------------

std::vector<rectangle> blobs_to_rects (
    const numpy_image<uint32_t>& labels_,
    size_t num_blobs
)
{
    auto labels = make_image_view(labels_);
    std::vector<rectangle> rects(num_blobs);
    for (long r = 0; r < labels.nr(); ++r)
        for (long c = 0; c < labels.nc(); ++c)
            if (labels[r][c] != 0)
                rects[labels[r][c]] += point(c,r);
    return rects;
}

// ----------------------------------------------------------------------------------------

template <typename T>
void zero_pixels_not_labeled_with_val (
    numpy_image<T>& img_,
    const numpy_image<uint32_t>& labels_,
    uint32_t val
)
{
    auto img = make_image_view(img_);
    auto labels = make_image_view(labels_);
    DLIB_CASSERT(have_same_dimensions(img, labels));

    for (long r = 0; r < img.nr(); ++r)
        for (long c = 0; c < img.nc(); ++c)
            if (labels[r][c] != val)
                img[r][c] = 0;
}

// ----------------------------------------------------------------------------------------

py::list hash_images (
    const py::list& images
)
{
    const matrix<float> random_projections = matrix_cast<float>(gaussian_randm(100,75*75));
    py::list result;
    for (auto& filename : images)
    {
        matrix<float> img,img2,h;

        load_image(img, filename.cast<std::string>());
        img2.set_size(75,75);
        resize_image(img, img2);

        img2 -= 110;

        h = random_projections*reshape_to_column_vector(img2);
        h = h>0;

        auto thehash = murmur_hash3_128bit(&h(0),h.size()*sizeof(float)).first;
        result.append(py::make_tuple(thehash, filename));
    }

    return result;
}

py::list hash_images_parallel (
    const std::vector<std::string>& images
)
{
    const matrix<float> random_projections = matrix_cast<float>(gaussian_randm(100,75*75));
    std::mutex m;
    py::list result;
    parallel_for(0, images.size(), [&](long i)
    {
        matrix<float> img,img2,h;

        load_image(img, images[i]);
        img2.set_size(75,75);
        resize_image(img, img2);

        img2 -= 110;

        h = random_projections*reshape_to_column_vector(img2);
        h = h>0;

        auto thehash = murmur_hash3_128bit(&h(0),h.size()*sizeof(float)).first;

        std::lock_guard<std::mutex> lock(m);
        result.append(py::make_tuple(thehash, images[i]));
    });

    return result;
}

// ----------------------------------------------------------------------------------------

class threaded_data_loader
{
public:
    threaded_data_loader(
        const std::vector<string>& filenames_,
        size_t num_threads,
        size_t buffer_size
    ) : filenames(filenames_), images(buffer_size)
    {
        for (size_t i = 0; i < num_threads; ++i)
        {
            threads.emplace_back([&,i](){
                dlib::rand rnd(i);
                matrix<rgb_pixel> img;
                while(images.is_enabled())
                {
                    try
                    {
                        load_image(img, filenames[rnd.get_integer(filenames.size())]);
                        images.enqueue(img);
                    } catch(exception& e)
                    {
                        cout << e.what() << endl;
                    }
                }
            });
        }
    }

    ~threaded_data_loader()
    {
        // Tell the threads to terminate
        images.disable();
        // and you have to join with thread objects before letting them destruct.  This
        // just waits for them to terminate.
        for (auto& t : threads)
            t.join();
    }

    size_t number_of_threads() const { return threads.size(); }

    numpy_image<rgb_pixel> get_next_image()
    {
        matrix<rgb_pixel> img;
        images.dequeue(img);
        return std::move(img);
    }

private:

    std::vector<string> filenames;
    dlib::pipe<matrix<rgb_pixel>> images;
    std::vector<thread> threads;
};

// ----------------------------------------------------------------------------------------

template <typename T>
double sum_row_major_order (const numpy_image<T>& img_)
{
    auto img = make_image_view(img_);

    double val = 0;
    //for (int iter = 0; iter < 2000; ++iter)
    for (long r = 0; r < img.nr(); ++r)
    {
        for (long c = 0; c < img.nc(); ++c)
        {
            val += img[r][c];
        }
    }
    return val;
}

double sum_row_major_order_simd (const numpy_image<float>& img_)
{
    auto img = make_image_view(img_);

    DLIB_CASSERT(img.size()%8 == 0);

    double result = 0;
    const float* data = &img[0][0];
    //for (int iter = 0; iter < 2000; ++iter)
    {
        simd8f val = 0;
        simd8f temp;
        for (long i = 0; i+7 < img.size(); i+=8)
        {
            temp.load(data+i);
            val += temp;
        }
        result += sum(val);
    }

    return result;
}

template <typename T>
double sum_column_major_order (const numpy_image<T>& img_)
{
    auto img = make_image_view(img_);

    double val = 0;
    //for (int iter = 0; iter < 2000; ++iter)
    for (long c = 0; c < img.nc(); ++c)
    {
        for (long r = 0; r < img.nr(); ++r)
        {
            val += img[r][c];
        }
    }
    return val;
}

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

void py_cuda_add_value_to_each_element(
    numpy_image<float>& img_,
    const float value
)
{
    auto img = make_image_view(img_);
    cuda_data_ptr<float> temp(img.size());
    // copy to the GPU
    memcpy(temp, &img[0][0]);

    cuda_add_value_to_each_element(temp, value);

    // copy from GPU back to CPU
    memcpy(&img[0][0], temp);
}

// ----------------------------------------------------------------------------------------

cuda_data_ptr<float> numpy_to_cuda (
    const numpy_image<float>& arr_
)
{
    auto arr = make_image_view(arr_);

    cuda_data_ptr<float> temp(arr.size());
    memcpy(temp, &arr[0][0]);
    return temp;
}

numpy_image<float> cuda_to_numpy (
    const cuda_data_ptr<float>& arr
)
{
    numpy_image<float> temp_(arr.size(),1);
    auto temp = make_image_view(temp_);

    memcpy(&temp[0][0], arr);
    return temp_;
}

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

void raster_scan(
    const numpy_image<float>& img_,
    numpy_image<float>& dist_,
    numpy_image<float>& lower_,
    numpy_image<float>& upper_
)
{
    auto img = make_image_view(img_);
    auto dist = make_image_view(dist_);
    auto lower = make_image_view(lower_);
    auto upper = make_image_view(upper_);
    DLIB_CASSERT(have_same_dimensions(img,dist,lower,upper));

    auto area = shrink_rect(get_rect(img),1);

    auto check_neighbor = [&](long r, long c, long neighbor_r, long neighbor_c)
    {
        auto l = std::min(lower[neighbor_r][neighbor_c], img[r][c]);
        auto u = std::max(upper[neighbor_r][neighbor_c], img[r][c]);
        auto d = u-l;
        if (d < dist[r][c])
        {
            lower[r][c] = l;
            upper[r][c] = u;
            dist[r][c] = d;
        }
    };

    // scan top to bottom
    for (long r = area.top(); r <= area.bottom(); ++r)
    {
        for (long c = area.left(); c <= area.right(); ++c)
        {
            check_neighbor(r,c, r-1,c);
            check_neighbor(r,c, r,c-1);
        }
    }

    // scan bottom to top
    for (long r = area.bottom(); r >= area.top(); --r)
    {
        for (long c = area.right(); c >= area.left(); --c)
        {
            check_neighbor(r,c, r+1,c);
            check_neighbor(r,c, r,c+1);
        }
    }

    // scan left to right
    for (long c = area.left(); c <= area.right(); ++c)
    {
        for (long r = area.top(); r <= area.bottom(); ++r)
        {
            check_neighbor(r,c, r-1,c);
            check_neighbor(r,c, r,c-1);
        }
    }

    // scan right to left
    for (long c = area.right(); c >= area.left(); --c)
    {
        for (long r = area.bottom(); r >= area.top(); --r)
        {
            check_neighbor(r,c, r+1,c);
            check_neighbor(r,c, r,c+1);
        }
    }
}

PYBIND11_MODULE(superfast, m)
{

    m.attr("__version__") = "1.2.3";
    m.attr("__time_compiled__") = std::string(__DATE__) + " " + std::string(__TIME__);

    m.def("hash_images", hash_images, py::arg("image_filenames"));
    m.def("hash_images_parallel", hash_images_parallel, py::arg("image_filenames"));

    m.def("blobs_to_rects", blobs_to_rects,
        py::arg("labels"), py::arg("num_blobs"),
        "a doc string that you should definitely write");

    m.def("coherent_hough_transform", coherent_hough_transform,
        py::arg("ht"), py::arg("edges"), py::arg("horz"), py::arg("vert"),
        "a doc string that you should definitely write");

    m.def("discard_wacky_edge_groups", discard_wacky_edge_groups,
        py::arg("edges"), py::arg("horz"), py::arg("vert"),
        "a doc string that you should definitely write");

    m.def("zero_pixels_not_labeled_with_val", zero_pixels_not_labeled_with_val<float>,
        py::arg("img"), py::arg("labels"), py::arg("val"),
        "a doc string that you should definitely write");
    m.def("zero_pixels_not_labeled_with_val", zero_pixels_not_labeled_with_val<uint8_t>,
        py::arg("img"), py::arg("labels"), py::arg("val"),
        "a doc string that you should definitely write");

    m.def("raster_scan", raster_scan, py::arg("img"), py::arg("dist"), py::arg("lower"), py::arg("upper"));

    m.def("sum_row_major_order", sum_row_major_order<float>);
    m.def("sum_row_major_order", sum_row_major_order<double>);
    m.def("sum_row_major_order_simd", sum_row_major_order_simd);
    m.def("sum_column_major_order", sum_column_major_order<float>);
    m.def("sum_column_major_order", sum_column_major_order<double>);

    py::class_<threaded_data_loader>(m, "threaded_data_loader", "doc string about this object")
        .def(py::init<std::vector<string>,size_t,size_t>(), py::arg("filenames"), py::arg("num_threads"), py::arg("buffer_size")=3)
        .def("get_next_image", &threaded_data_loader::get_next_image)
        .def_property_readonly("number_of_threads", &threaded_data_loader::number_of_threads);

    m.def("cuda_add_value_to_each_element", py_cuda_add_value_to_each_element, py::arg("img"), py::arg("value"));

    py::class_<cuda_data_ptr<float>>(m, "cuda_data", "An array of float32 data on the GPU")
        .def(py::init<size_t>(), "Allocate an array off N float32s on the GPU", py::arg("N"))
        .def(py::init<>(&numpy_to_cuda))
        .def("to_numpy", cuda_to_numpy, "convert this cuda array to a numpy array.")
        .def_property_readonly("size", &cuda_data_ptr<float>::size);

    m.def("cuda_add_value_to_each_element", cuda_add_value_to_each_element, py::arg("img"), py::arg("value"));
    m.def("cuda_add_value_to_each_element_simple", cuda_add_value_to_each_element_simple, py::arg("img"), py::arg("value"));
    m.def("cuda_add_value_to_each_element_ugly", cuda_add_value_to_each_element_ugly, py::arg("img"), py::arg("value"));

    m.def("cuda_dot_product", cuda_dot_product, py::arg("out"), py::arg("a"), py::arg("b"));
    m.def("cuda_matrix_vector_multiply", cuda_matrix_vector_multiply, py::arg("out"), py::arg("M"), py::arg("v"), "compute: out = M*v");

    m.def("cuda_set_device", [](int id){set_device(id); }, py::arg("device_id"));
    m.def("cuda_device_synchronize", [](int id){device_synchronize(id); }, py::arg("device_id"));

}


#include <functional>

#include <Qt3DIncludes.h>
#include <GaussIncludes.h>
#include <FEMIncludes.h>

//Any extra things I need such as constraints
#include <ConstraintFixedPoint.h>
#include <TimeStepperEigenFitSMW.h>
#include <EigenFit.h>
#include <fstream>

using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem; //For Force Spring

/* Tetrahedral finite elements */

//typedef physical entities I need

//typedef scene
typedef PhysicalSystemFEM<double, NeohookeanHFixedTet> FEMLinearTets;

typedef World<double, std::tuple<FEMLinearTets *>,
std::tuple<ForceSpringFEMParticle<double> *, ForceParticlesGravity<double> *>,
std::tuple<ConstraintFixedPoint<double> *> > MyWorld;

//typedef World<double, std::tuple<FEMLinearTets *,PhysicalSystemParticleSingle<double> *>,
//                      std::tuple<ForceSpringFEMParticle<double> *>,
//                      std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
//typedef TimeStepperEigenFitSMW<double, AssemblerEigenSparseMatrix<double>, AssemblerEigenVector<double>> MyTimeStepper;
typedef TimeStepperEigenFitSMW<double, AssemblerParallel<double, AssemblerEigenSparseMatrix<double>>, AssemblerParallel<double, AssemblerEigenVector<double>> > MyTimeStepper;

typedef Scene<MyWorld, MyTimeStepper> MyScene;

//typedef TimeStepperEigenFitSI<double, AssemblerParallel<double, AssemblerEigenSparseMatrix<double> >,
//AssemblerParallel<double, AssemblerEigenVector<double> > > MyTimeStepper;

//typedef Scene<MyWorld, MyTimeStepper> MyScene;

// used for preStepCallback. should be delete
std::vector<ConstraintFixedPoint<double> *> movingConstraints;
Eigen::VectorXi movingVerts;
Eigen::MatrixXd V;
Eigen::MatrixXi F;
char **arg_list;
unsigned int istep;

void preStepCallback(MyWorld &world) {
//    // This is an example callback
//    if (atoi(arg_list[5]) == 2)
//    {
//        // This is an example callback
//
//        //script some motion
////
//        if (istep < 50) {
//
//            for(unsigned int jj=0; jj<movingConstraints.size(); ++jj) {
//                if(movingConstraints[jj]->getImpl().getFixedPoint()[0] > -3) {
//                    Eigen::Vector3d v = V.row(movingVerts[jj]);
//                    Eigen::Vector3d new_p = v + Eigen::Vector3d(0.0,1.0/10,0.0);
//                    movingConstraints[jj]->getImpl().setFixedPoint(new_p);
//                }
//            }
//        }
//    }
}

int main(int argc, char **argv) {
    //    arg list
    //    1: full path to coarse mesh
    //    2: full path to fine mesh
    //    3: youngs modulus (SI unit)
    //    4: constraint threshold (for defualt constraint profile)
    //    5: constraint profile switch
    //    6: name of the initial deformation profile
    //    7. number of time steps
//    8. flag for using hausdorff distance
//    9. number of modes to modifies
    std::cout<<"Test Neohookean FEM EigenFit\n";

    //Setup Physics
    MyWorld world;

    arg_list = argv;

    Eigen::MatrixXd Vf;
    Eigen::MatrixXi Ff;

    //    define the file separator base on system
    const char kPathSeparator =
#ifdef _WIN32
    '\\';
#else
    '/';
#endif

    if (argc > 1) {
        // must supply all 9 parameters

        std::string cmeshname = argv[1];
        std::string fmeshname = argv[2];

        readTetgen(V, F, dataDir()+cmeshname+".node", dataDir()+cmeshname+".ele");
        readTetgen(Vf, Ff, dataDir()+fmeshname+".node", dataDir()+fmeshname+".ele");

        std::string::size_type found = cmeshname.find_last_of(kPathSeparator);
        //    acutal name for the mesh, no path
        std::string cmeshnameActual = cmeshname.substr(found+1);

        //    acutal name for the mesh, no path
        std::string fmeshnameActual = fmeshname.substr(found+1);

        //    parameters
        double youngs = atof(argv[3]);
        double poisson = 0.45;
        int constraint_dir = 0; // constraint direction. 0 for x, 1 for y, 2 for z
        double constraint_tol = atof(argv[4]);
        //
        // send the constraint switch in as well, or the fine embedded mesh. ugly
        // the flag indicate whether to recalculated or not
        // need to pass the material and constraint parameters to embedding too. need to do it again below. ugly
        // also use the last two args to determine how many modes to fix. have to put it here now. ugly
        EigenFit *test = new EigenFit(V,F,Vf,Ff,true,youngs,poisson,constraint_dir,constraint_tol, atoi(argv[5]),atoi(argv[8]),atoi(argv[9]));

        world.addSystem(test);

        // projection matrix for constraints
        Eigen::SparseMatrix<double> P;
        if (atoi(argv[5]) == 0) {
            // constraint switch

            //            zero gravity
            Eigen::Vector3x<double> g;
            g(0) = 0;
            g(1) = 0;
            g(2) = 0;

            for(unsigned int iel=0; iel<test->getImpl().getF().rows(); ++iel) {

                test->getImpl().getElement(iel)->setGravity(g);

            }

            world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

            //            set the projection matrix to identity because there is no constraint to project
//            Eigen::SparseMatrix<double> P;
            P.resize(V.rows()*3,V.rows()*3);
            P.setIdentity();
//            std::cout<<P.rows();
            //            no constraints
        }
        else if(atoi(argv[5]) == 1)
        {
            //    default constraint
            fixDisplacementMin(world, test,constraint_dir,constraint_tol);
            world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

            // construct the projection matrix for stepper
            Eigen::VectorXi indices = minVertices(test, constraint_dir,constraint_tol);
            P = fixedPointProjectionMatrix(indices, *test,world);

        }
        else if (atoi(argv[5]) == 2)
        {

            movingVerts = minVertices(test, constraint_dir, constraint_tol);//indices for moving parts
//
            for(unsigned int ii=0; ii<movingVerts.rows(); ++ii) {
                movingConstraints.push_back(new ConstraintFixedPoint<double>(&test->getQ()[movingVerts[ii]], Eigen::Vector3d(0,0,0)));
                world.addConstraint(movingConstraints[ii]);
            }
            fixDisplacementMin(world, test,constraint_dir,constraint_tol);

            world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

            P = fixedPointProjectionMatrix(movingVerts, *test,world);

        }

        // set material
        for(unsigned int iel=0; iel<test->getImpl().getF().rows(); ++iel) {

            test->getImpl().getElement(iel)->setParameters(youngs, poisson);

        }

        auto q = mapStateEigen(world);
        auto fine_q = mapStateEigen(test->getFineWorld());
        //    default to zero deformation
        q.setZero();

        if (strcmp(argv[6],"0")==0) {

            q.setZero();
        }
        else
        {

            std::string qfileName(argv[6]);
            Eigen::VectorXd  tempv;
            loadMarketVector(tempv,qfileName);

            q = tempv;

        }

        MyTimeStepper stepper(0.01,P);

        //         the number of steps to take

        unsigned int file_ind = 0;
        std::string name = "pos";
        std::string fformat = ".obj";
        std::string filename = name + std::to_string(file_ind) + fformat;
        std::string qname = cmeshnameActual + "ExampleQ";
        std::string qfformat = ".mtx";
        std::string qfilename = qname + std::to_string(file_ind) + qfformat;
        std::string qname2 = fmeshnameActual + "ExampleQ";
        std::string qfilename2 = qname2 + std::to_string(file_ind) + qfformat;

        struct stat buf;
        unsigned int idx;

        for(istep=0; istep<atoi(argv[7]) ; ++istep) {
            stepper.step(world);

            // acts like the "callback" block
            if (atoi(arg_list[5]) == 2)
            {
                //script some motion
                //

                for(unsigned int jj=0; jj<movingConstraints.size(); ++jj) {

                    auto v_q = mapDOFEigen(movingConstraints[jj]->getDOF(0), world.getState());
//
//                    if ((istep%150) < 50) {
//                        Eigen::Vector3d new_q = (istep%150)*Eigen::Vector3d(0.0,-1.0/100,0.0);
//                        v_q = new_q;
//                    }
//                    else if ((istep%150) < 100)
//                    {}
//                    else
//                    {
//                        Eigen::Vector3d new_q =  (150-(istep%150))*Eigen::Vector3d(0.0,-1.0/100,0.0);
//                        v_q = new_q;
//                    }
                    Eigen::Vector3d new_q = (istep)*Eigen::Vector3d(0.0,-1.0/100,0.0);
                    v_q = new_q;

                }
            }

            //output data here
            std::ofstream ofile;
            ofile.open("KE.txt", std::ios::app); //app is append which means it will put the text at the end
            ofile << std::get<0>(world.getSystemList().getStorage())[0]->getImpl().getKineticEnergy(world.getState()) << std::endl;
            ofile.close();

            while (stat(filename.c_str(), &buf) != -1)
            {
                file_ind++;
                filename = name + std::to_string(file_ind) + fformat;
                qfilename = qname + std::to_string(file_ind) + qfformat;
                qfilename2 = qname2 + std::to_string(file_ind) + qfformat;

            }

            idx = 0;
            // getGeometry().first is V
            Eigen::MatrixXd V_disp = std::get<0>(world.getSystemList().getStorage())[0]->getGeometry().first;

            for(unsigned int vertexId=0;  vertexId < std::get<0>(world.getSystemList().getStorage())[0]->getGeometry().first.rows(); ++vertexId) {

                // because getFinePosition is in EigenFit, not another physical system Impl, so don't need getImpl()
                V_disp(vertexId,0) += q(idx);
                idx++;
                V_disp(vertexId,1) += q(idx);
                idx++;
                V_disp(vertexId,2) += q(idx);
                idx++;
            }
            igl::writeOBJ(filename,V_disp,std::get<0>(world.getSystemList().getStorage())[0]->getGeometry().second);
            // coarse mesh data
            q = mapStateEigen(world);
            saveMarketVector(q, qfilename);
            // fine mesh data from embedded mesh in eigenfit
            fine_q = mapStateEigen(test->getFineWorld());
            saveMarketVector(fine_q, qfilename2);
        }
    }
    else
    {
        // using all default paramters for eigenfit

        //    default example meshes
        std::string cmeshname = "/meshesTetgen/arma/arma_6";
        std::string fmeshname = "/meshesTetgen/arma/arma_1";

        readTetgen(V, F, dataDir()+cmeshname+".node", dataDir()+cmeshname+".ele");
        readTetgen(Vf, Ff, dataDir()+fmeshname+".node", dataDir()+fmeshname+".ele");

        std::string::size_type found = cmeshname.find_last_of(kPathSeparator);
        //    acutal name for the mesh, no path
        std::string cmeshnameActual = cmeshname.substr(found+1);

        //    default parameters
        double youngs = 5e5;
        double poisson = 0.45;
        int constraint_dir = 0; // constraint direction. 0 for x, 1 for y, 2 for z
        double constraint_tol = 2e-1;

        // no constraint switch so just create the eigenfit obj with constraint switch set to 1
        // the flag indicate whether to recalculated or not
        // need to pass the material and constraint parameters to embedding too. need to do it again below. ugly
        // also use the last two args to determine how many modes to fix. default not using hausdorff distance, and use 10 modes. have to put it here now.  ugly
        EigenFit *test = new EigenFit(V,F,Vf,Ff,true,youngs,poisson,constraint_dir,constraint_tol, 1,false,10);

        // set material
        for(unsigned int iel=0; iel<test->getImpl().getF().rows(); ++iel) {

            test->getImpl().getElement(iel)->setParameters(youngs, poisson);

        }

        world.addSystem(test);

        world.finalize(); //After this all we're ready to go (clean up the interface a bit later)
        // IMPORTANT, need to finalized before fix boundary

        //    default constraint
        fixDisplacementMin(world, test,constraint_dir,constraint_tol);

        // construct the projection matrix for stepper
        Eigen::VectorXi indices = minVertices(test, constraint_dir,constraint_tol);
        Eigen::SparseMatrix<double> P = fixedPointProjectionMatrix(indices, *test,world);

        auto q = mapStateEigen(world);

        //    default to zero deformation
        q.setZero();

        MyTimeStepper stepper(0.01,P);

        //Display
        QGuiApplication app(argc, argv);

        MyScene *scene = new MyScene(&world, &stepper, preStepCallback);
        GAUSSVIEW(scene);

        return app.exec();

    }

}


// Copyright (C) 2018 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@mail.wvu.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <utility>
#include <vector>

#include "theia/math/util.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/pose/upnp.h"
#include "theia/util/random.h"

namespace theia {
namespace {

using Eigen::AngleAxisd;
using Eigen::Quaterniond;
using Eigen::Vector3d;

RandomNumberGenerator rng(57);

struct InputDatum {
  std::vector<Eigen::Vector3d> ray_origins;
  std::vector<Eigen::Vector3d> ray_directions;
  std::vector<Eigen::Vector3d> world_points;
};

InputDatum ComputeInputDatum(const std::vector<Vector3d>& world_points,
                             const std::vector<Vector3d>& camera_centers,
                             const Quaterniond& expected_rotation,
                             const Vector3d& expected_translation) {
  const int num_points = world_points.size();
  const int num_cameras = camera_centers.size();

  InputDatum input_datum;
  std::vector<Vector3d>& ray_directions = input_datum.ray_directions;
  std::vector<Vector3d>& ray_origins = input_datum.ray_origins;
  ray_directions.reserve(num_points);
  ray_origins.reserve(num_points);

  for (int i = 0; i < num_points; ++i) {
    // Ray origin wrt to coordinate system of the camera rig.
    const Vector3d ray_origin =
        expected_rotation * camera_centers[i % num_cameras] +
        expected_translation;

    ray_origins.emplace_back(std::move(ray_origin));

    // Reproject 3D points into camera frame.
    ray_directions.emplace_back((expected_rotation * world_points[i] +
                                 expected_translation - ray_origins[i])
                                    .normalized());
  }

  input_datum.world_points = world_points;
  return input_datum;
}

bool CheckReprojectionErrors(const InputDatum& input_datum,
                             const Eigen::Quaterniond& soln_rotation,
                             const Eigen::Vector3d& soln_translation,
                             const double kMaxReprojectionError) {
  const int num_points = input_datum.world_points.size();
  double good_reprojection_errors = true;
  for (int i = 0; i < num_points; ++i) {
    const double reprojection_error =
        Upnp::ComputeResidual(input_datum.ray_origins[i],
                              input_datum.ray_directions[i],
                              input_datum.world_points[i],
                              soln_rotation,
                              soln_translation);

    good_reprojection_errors = (good_reprojection_errors &&
                                (reprojection_error < kMaxReprojectionError));
  }
  return good_reprojection_errors;
}

void TestUpnpPoseEstimationWithNoise(const Quaterniond& expected_rotation,
                                     const Vector3d& expected_translation,
                                     const double projection_noise_std_dev,
                                     const double max_reprojection_error,
                                     const double max_rotation_difference,
                                     const double max_translation_difference,
                                     InputDatum* input_datum) {
  const int num_points = input_datum->world_points.size();
  // Add noise to ray.
  if (projection_noise_std_dev > 0.0) {
    for (int i = 0; i < input_datum->world_points.size(); ++i) {
      AddNoiseToRay(
          projection_noise_std_dev, &rng, &input_datum->ray_directions[i]);
    }
  }

  // Estimate pose.
  std::vector<Quaterniond> solution_rotations;
  std::vector<Vector3d> solution_translations;
  const Upnp::CostParameters upnp_params = Upnp(input_datum->ray_origins,
                                                input_datum->ray_directions,
                                                input_datum->world_points,
                                                &solution_rotations,
                                                &solution_translations);
  const double upnp_cost = Upnp::EvaluateCost(upnp_params, expected_rotation);
  VLOG(3) << "Upnp cost with expected rotation: " << upnp_cost;

  // Check solutions and verify at least one is close to the actual solution.
  const int num_solutions = solution_rotations.size();
  EXPECT_GT(num_solutions, 0);
  bool matched_transform = false;
  for (int i = 0; i < num_solutions; ++i) {
    const bool good_reprojection_errors =
        CheckReprojectionErrors(*input_datum,
                                solution_rotations[i],
                                solution_translations[i],
                                max_reprojection_error);
    const double rotation_difference =
        expected_rotation.angularDistance(solution_rotations[i]);
    const bool matched_rotation = rotation_difference < max_rotation_difference;
    const double translation_difference =
        (expected_translation - solution_translations[i]).squaredNorm();
    const bool matched_translation =
        translation_difference < max_translation_difference;
    VLOG(3) << "Matched rotation: " << matched_rotation
            << " rotation error [deg]=" << RadToDeg(rotation_difference);
    VLOG(3) << "Matched translation: " << matched_translation
            << " translation error=" << translation_difference;
    VLOG(3) << "Good reprojection errors: " << good_reprojection_errors;
    if (matched_rotation && matched_translation && good_reprojection_errors) {
      matched_transform = true;
    }
  }
  EXPECT_TRUE(matched_transform);
}

// Verifies that the cost-function parameters are correct for central cameras.
TEST(UpnpTests, ComputeCostParametersForCentralCameraPoseEstimation) {
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 0.0;
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);

  std::vector<Eigen::Quaterniond> solution_rotations;
  std::vector<Eigen::Vector3d> solution_translations;
  const Upnp::CostParameters upnp_params = Upnp(input_datum.ray_origins,
                                                input_datum.ray_directions,
                                                input_datum.world_points,
                                                &solution_rotations,
                                                &solution_translations);

  const double upnp_cost = Upnp::EvaluateCost(upnp_params, soln_rotation);
  EXPECT_NEAR(upnp_cost, 0.0, 1e-6);
}

// Verifies that the cost-function parameters are correct for non-central
// cameras.
TEST(UpnpTests, ComputeCostParametersForNonCentralCameraPoseEstimation) {
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation =
      Quaterniond(AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 0.0;
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);

  std::vector<Eigen::Quaterniond> solution_rotations;
  std::vector<Eigen::Vector3d> solution_translations;
  const Upnp::CostParameters upnp_params = Upnp(input_datum.ray_origins,
                                                input_datum.ray_directions,
                                                input_datum.world_points,
                                                &solution_rotations,
                                                &solution_translations);

  const double upnp_cost = Upnp::EvaluateCost(upnp_params, soln_rotation);
  EXPECT_NEAR(upnp_cost, 0.0, 1e-6);
}

// Verifies that the computation of the residual is correct.
TEST(UpnpTests, EvaluateResidualAtOptimalSolution) {
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
}

// Checks the case of a minimal sample and central camera pose estimation.
TEST(UpnpTests, MinimalSampleCentralCameraPoseEstimation) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks the case of a minimal sample and central camera pose estimation.
TEST(UpnpTests, MinimalSampleCentralCameraPoseEstimationWithNoise) {
  const double kNoiseStdDev = 0.4 / 512.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1.0);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks the case of a minimal sample and a non-central camera pose estimation.
TEST(UpnpTests, MinimalSampleNonCentralCameraPoseEstimation) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks the case of a minimal sample and a non-central camera pose estimation
// with noise.
TEST(UpnpTests, MinimalSampleNonCentralCameraPoseEstimationWithNoise) {
  const double kNoiseStdDev = 1e-4;
  const double kMaxReprojectionError = 3.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1.0);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks that the estimator works well using a non-minimal sample of data
// points.
TEST(UpnpTests, NonMinimalSampleCentralCameraPoseEstimation) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(0.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks that the estimator works well using a non-minimal sample of data
// points.
TEST(UpnpTests, NonMinimalSampleCentralCameraPoseEstimationWithNoise) {
  const double kNoiseStdDev = 5e-4;
  const double kMaxReprojectionError = 3.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1.0);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(0.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks that the estimator works well using a non-minimal sample of data.
TEST(UpnpTests, NonMinimalSampleNonCentralCameraPoseEstimation) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Checks that the estimator works well using a non-minimal sample of data
// points with noise.
TEST(UpnpTests, NonMinimalSampleNonCentralCameraPoseEstimationWithNoise) {
  const double kNoiseStdDev = 1e-3;
  const double kMaxReprojectionError = 3.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1.0);
  const double kMaxAllowedTranslationDifference = 1e-3;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only translation occurs on a central camera.
TEST(UpnpTests, NoRotationOnMinimalSampleAndCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(0.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only translation occurs on a non-central
// camera.
TEST(UpnpTests, NoRotationOnMinimalSampleAndNonCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(0.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only translation occurs on a central camera.
TEST(UpnpTests, NoRotationOnNonMinimalSampleAndCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(0.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only translation occurs on a central camera.
TEST(UpnpTests, NoRotationOnNonMinimalSampleAndNonCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(0.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only rotation occurs on a central camera.
TEST(UpnpTests, NoTranslationOnMinimalSampleAndCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(10.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only rotation occurs on a non-central
// camera.
TEST(UpnpTests, NoTranslationOnMinimalSampleAndNonCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(10.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only rotation occurs on a central camera.
TEST(UpnpTests, NoTranslationOnNonMinimalSampleAndCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigin = {Vector3d(2.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(10.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigin, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

// Tests estimation of pose when only translation occurs on a central camera.
TEST(UpnpTests, NoTranslationOnNonMinimalSampleAndNonCentralCamera) {
  const double kNoiseStdDev = 0.0;
  const double kMaxReprojectionError = 1.0 / 512.0;
  const double kMaxAllowedRotationDifference = DegToRad(1e-4);
  const double kMaxAllowedTranslationDifference = 1e-6;
  const std::vector<Vector3d> kPoints3d = {Vector3d(-1.0, 3.0, 3.0),
                                           Vector3d(1.0, -1.0, 2.0),
                                           Vector3d(-1.0, 1.0, 2.0),
                                           Vector3d(2.0, 1.0, 3.0),
                                           Vector3d(-1.0, -3.0, 2.0),
                                           Vector3d(1.0, -2.0, 1.0),
                                           Vector3d(-1.0, 4.0, 2.0),
                                           Vector3d(-2.0, 2.0, 3.0)};
  const std::vector<Vector3d> kImageOrigins = {Vector3d(-1.0, 0.0, 0.0),
                                               Vector3d(0.0, 0.0, 0.0),
                                               Vector3d(2.0, 0.0, 0.0),
                                               Vector3d(3.0, 0.0, 0.0)};
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(10.0), Vector3d(1.0, 0.0, 1.0).normalized()));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  // Compute input datum.
  InputDatum input_datum = ComputeInputDatum(
      kPoints3d, kImageOrigins, soln_rotation, soln_translation);
  // Execute test.
  TestUpnpPoseEstimationWithNoise(soln_rotation,
                                  soln_translation,
                                  kNoiseStdDev,
                                  kMaxReprojectionError,
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedTranslationDifference,
                                  &input_datum);
}

}  // namespace
}  // namespace theia