#include "visual_utility/TransformEstimator-Inl.h"

#include <gtest/gtest.h>
#include <boost/scoped_ptr.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "cv_utils/DisplayImages.h"

using namespace cv;
using namespace std;
using namespace boost;

namespace visual_utility {

class EstimateAffineTransformTest : public ::testing::Test {

 protected:
  Mat_<double> image;
  scoped_ptr<AffineTransformEstimator> estimator;

  virtual void SetUp() {
    estimator.reset(new AffineTransformEstimator(100, 1e-6, 1.0));

    // Create a simple image
    image = Mat::zeros(20, 30, CV_64F);

    for (int i = 3; i < 14; i++) {
      image.at<double>(Point((i+10)/2, i)) += i/20.0;
    }

    for (int i = 9; i < 18; i++) {
      image.at<double>(Point(i, 6)) = 0.6;
    }
  }

  void ExpectMatrixNear(const Mat& a, const Mat& b, double error=1e-5) {
    ASSERT_EQ(a.rows, b.rows);
    ASSERT_EQ(a.cols, b.cols);

    for (int i = 0; i < a.rows; i++) {
      for (int j = 0; j < b.cols; j++) {
        EXPECT_NEAR(a.at<double>(Point(j,i)), b.at<double>(Point(j,i)), error);
      }
    }
  }

  Mat CreateWarpedImage(const Mat_<double>& src, const Mat& M) {
    return estimator->ApplyTransform(src, M, geo::BORDER_CONSTANT, 0.0);
  }

};

TEST_F(EstimateAffineTransformTest, SmallRotation) {
  double angle = 1.0;
  Mat desiredTransform = Mat::zeros(2,3,CV_64F);
  desiredTransform.at<double>(Point(0,0)) = cos(angle * M_PI / 180.0);
  desiredTransform.at<double>(Point(0,1)) = -sin(angle * M_PI /180.0);
  desiredTransform.at<double>(Point(1,0)) = sin(angle * M_PI / 180.0);
  desiredTransform.at<double>(Point(1,1)) = cos(angle * M_PI / 180.0);

  Mat warpedImage = CreateWarpedImage(image, desiredTransform);

  Mat foundTransform = estimator->EstimateTransform(image, warpedImage);
  ExpectMatrixNear(desiredTransform, foundTransform);
}

TEST_F(EstimateAffineTransformTest, SmallTranslation) {
  Mat_<double> desiredTransform = Mat::eye(2,3,CV_64F);
  desiredTransform(0,2) = 2;
  desiredTransform(1,2) = -2;

  Mat warpedImage = CreateWarpedImage(image, desiredTransform);

  Mat foundTransform = estimator->EstimateTransform(image, warpedImage);
  ExpectMatrixNear(desiredTransform, foundTransform);
}

TEST_F(EstimateAffineTransformTest, TranslationWithRescaling) {
  Mat_<double> desiredTransform = Mat::eye(2,3,CV_64F);
  desiredTransform(0,2) = 2;
  desiredTransform(1,2) = -2;

  Mat warpedImage = CreateWarpedImage(image, desiredTransform);

  estimator.reset(new AffineTransformEstimator(100, 1e-6, 2.0));
  Mat foundTransform = estimator->EstimateTransform(image, warpedImage);
  ExpectMatrixNear(desiredTransform, foundTransform);
}

TEST_F(EstimateAffineTransformTest, ImageWithRescaling) {
  // Do a small rotation on an image that is resized
  double angle = 8.0;
  Mat_<double> desiredTransform = Mat::zeros(2,3,CV_64F);
  desiredTransform(0,0) = cos(angle * M_PI / 180.0);
  desiredTransform(0,1) = -sin(angle * M_PI /180.0);
  desiredTransform(1,0) = sin(angle * M_PI / 180.0);
  desiredTransform(1,1) = cos(angle * M_PI / 180.0);

  Mat biggerImage;
  resize(image, biggerImage, Size(), 2.0, 2.0, INTER_CUBIC);

  Mat warpedImage = CreateWarpedImage(biggerImage, desiredTransform);

  Mat foundTransform = estimator->EstimateTransform(biggerImage, warpedImage);
  ExpectMatrixNear(desiredTransform, foundTransform);
}

TEST_F(EstimateAffineTransformTest, TestIdentityTransform) {
  Mat desiredTransform = Mat::eye(2,3,CV_64F);

  Mat warpedImage = CreateWarpedImage(image, desiredTransform);

  Mat foundTransform = estimator->EstimateTransform(image, warpedImage);

  ExpectMatrixNear(desiredTransform, foundTransform);
  ExpectMatrixNear(warpedImage, image);
}

TEST_F(EstimateAffineTransformTest, TestNoTransform) {

  Mat warpedImage = image.clone();

  Mat foundTransform = estimator->EstimateTransform(image, warpedImage);

  ExpectMatrixNear(Mat::eye(2,3,CV_64F), foundTransform);
}

// mdesnoyer: disable the test because OpenCV isn't throwing a normal
// exception, it's killing the program.
TEST_F(EstimateAffineTransformTest, DISABLED_SrcNotGreyscale) {
  Mat colorImage;

  cvtColor(image, colorImage, CV_GRAY2BGR);

  EXPECT_THROW(estimator->EstimateTransform(colorImage, image), cv::Exception);
}

// mdesnoyer: disable the test because OpenCV isn't throwing a normal
// exception, it's killing the program.
TEST_F(EstimateAffineTransformTest, DISABLED_DestNotGreyscale) {
  Mat colorImage;

  cvtColor(image, colorImage, CV_GRAY2BGR);

  EXPECT_THROW(estimator->EstimateTransform(image, colorImage), cv::Exception);
}

TEST_F(EstimateAffineTransformTest, ReadDataTest) {
  Mat img1 = imread("test/testFishFrame1.png", -1);
  Mat img2 = imread("test/testFishFrame2.png", -1);

  Mat img1Float;
  Mat img2Float;
  img1.convertTo(img1Float, CV_64F, 1./255);
  img2.convertTo(img2Float, CV_64F, 1./255);

  estimator.reset(new AffineTransformEstimator(50, 1e-8, 4.0));

  Mat M = estimator->EstimateTransform(img1Float, img2Float);
  ASSERT_EQ(M.rows, 2);
  ASSERT_EQ(M.cols, 3);
}

TEST_F(EstimateAffineTransformTest, RealImageRotateAndTranslate) {
  // Rotate
  double angle = 0.5;
  Mat_<double> desiredTransform = Mat::zeros(2,3,CV_64F);
  desiredTransform(0,0) = cos(angle * M_PI / 180.0);
  desiredTransform(0,1) = -sin(angle * M_PI /180.0);
  desiredTransform(1,0) = sin(angle * M_PI / 180.0);
  desiredTransform(1,1) = cos(angle * M_PI / 180.0);

  // Zoom
  desiredTransform(0,0) *= 1.02;
  desiredTransform(1,1) *= 1.05;

  // Shift
  desiredTransform(0,2) = 1;
  desiredTransform(1,2) = -2;

  estimator.reset(new AffineTransformEstimator(100, 1e-8, 4.0));

  Mat img1 = imread("test/hima_set22_404.bmp", 0);
  Mat img1Float;
  img1.convertTo(img1Float, CV_64F, 1./255);

  Mat warpedImage = CreateWarpedImage(img1Float, desiredTransform);

  Mat foundTransform = estimator->EstimateTransform(img1Float, warpedImage);

  ExpectMatrixNear(desiredTransform, foundTransform);

  cv_utils::DisplayNormalizedImage(img1, "img1");
  cv_utils::DisplayNormalizedImage(warpedImage, "img2");
  cv_utils::DisplayNormalizedImage(abs(warpedImage - CreateWarpedImage(img1Float,
                                                                       foundTransform)),
                                   "diff");
  cv_utils::DisplayNormalizedImage(CreateWarpedImage(img1Float,
                                                     foundTransform),
                                   "warped");
  cv_utils::ShowWindowsUntilKeyPress();
}

}; // namespace

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <utility>

//
// Created by lei on 4/21/19.
//

#include "topography.hpp"

#include <algorithm>
#include <fmt/format.h>
#include <armadillo>

Topography::Topography(const arma::vec &x,
                       const arma::vec &fev, int nk):
                       x_(x), topo_(nk, x.size()) {
    if (not x.is_sorted()) {
        fmt::print("{}\n", "input vector is not a sorted one.");
        exit(-1);
    }
    int nx = x.size();
    int nk2 = 2*nk;
    for (auto i=0; i<nx; ++i) {
        int ind_beg;
        if (i < nk) {
            ind_beg = 0;
        } else if (i >= nx-nk) {
            ind_beg = nx - 1 - nk2;
        } else {
            ind_beg = i - nk;
        }
        arma::vec x_slice = x.subvec(ind_beg, ind_beg+nk2);
        x_slice = arma::abs(x_slice -x[i]);
        arma::uvec ind_sort = arma::sort_index(x_slice);
        for (auto j=0; j<nk; ++j) {
            int ind_global = ind_sort(j+1) + ind_beg;
            double f_diff = fev(ind_global) - fev(i);
            int sign = (f_diff > 0 )  - (f_diff < 0);
            topo_(j, i) = sign * ind_global;
        }
    }
}

std::vector<double> Topography::minimize_pool() const {
    std::vector<double> min_pool;
    for (auto i=0U; i<x_.size(); ++i) {
        if (arma::all(topo_.col(i) > 0)) {
            min_pool.push_back(x_(i));
        }
    }
    return min_pool;
}


#include <lib_template/dummy.h>

#include <boost/any.hpp>

#include <plog/Log.h>

#include <seqan/graph_algorithms.h>

#include <mpir.h>
#include <boost/multiprecision/gmp.hpp>

#include <sstream>

namespace boost_mp = boost::multiprecision;
using int_type = boost_mp::number<boost_mp::backends::gmp_int,
                                  boost_mp::expression_template_option::et_off>;

using namespace seqan;

using TGraph = Graph<Directed<>>;
using TVertexDescriptor = VertexDescriptor<TGraph>::Type;
using TEdgeIterator = Iterator<TGraph, EdgeIterator>::Type;
using TSize = Size<TGraph>::Type;

Dummy::Dummy()
{
    LOG_INFO << "Dummy lib constructed";

    // Create graph with 9 directed edges (0,1), (0,2)
    TSize numEdges = 9;
    TVertexDescriptor edges[] = {0, 1, 0, 2, 0, 4, 1, 3, 1, 4, 2, 1, 3, 0, 3, 2, 4, 3};
    TGraph g;
    addEdges(g, edges, numEdges);
    // Print graph.
    std::stringstream gs;
    gs << g;
    LOG_INFO << gs.str();

    // Fill external property map with edge weights and assign to graph.
    int_type weights[] = {3, 8, -4, 1, 7, 4, 2, -5, 6};
    String<int_type> weightMap;
    assignEdgeMap(weightMap, g, weights);

    // Run Floyd-Warshall algorithm.
    String<int_type> distMat;
    String<TVertexDescriptor> predMat;
    floydWarshallAlgorithm(distMat, predMat, g, weightMap);

    // Print result to stdout.
    unsigned int len = static_cast<unsigned>(std::sqrt(static_cast<double >(length(distMat))));
    for (TSize row = 0; row < len; ++row)
        for (TSize col = 0; col < len; ++col)
        {
            std::stringstream s;
            s << row << "," << col << " (Distance="
                 <<  getValue(distMat, row * len + col) << "): ";
            LOG_INFO << s.str();
        }

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_LOG_2OLOG_10_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOG_2OLOG_10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_2olog_10 : \f$\frac{\log(2)}{\log(10)}\f$.

    @par Header <boost/simd/constant/log_2olog_10.hpp>

    @par Semantic:

    @code
    T r = Log_2olog_10<T>();
    @endcode

    is similar to:

    @code
      r =  T(0.3010299956639811952137388947244930267681898814621085);
    @endcode

**/
  template<typename T> T Log_2olog_10();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_2olog_10.  (\f$\frac{\log(2)}{\log(10)}\f$)

      Generate the  constant log_2olog_10.

      @return The Log_2olog_10 constant for the proper type
    **/
    Value Log_2olog_10<Value>();
  }
} }
#endif

#include <boost/simd/constant/scalar/log_2olog_10.hpp>
#include <boost/simd/constant/simd/log_2olog_10.hpp>

#endif


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm trigonometry tan
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/trigonometry/tan.h"

namespace fa = fern::algorithm;

template<
    class Value>
using OutOfDomainPolicy = fa::tan::OutOfDomainPolicy<Value>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<double> policy;
        BOOST_CHECK(policy.within_domain(5));
        BOOST_CHECK(policy.within_domain(-5));
        BOOST_CHECK(policy.within_domain(0));

        BOOST_CHECK(!policy.within_domain(fern::infinity<double>()));
        BOOST_CHECK(!policy.within_domain(-fern::infinity<double>()));

        BOOST_CHECK(!policy.within_domain(1.0 * fern::half_pi<double>()));
        BOOST_CHECK( policy.within_domain(2.0 * fern::half_pi<double>()));
        BOOST_CHECK(!policy.within_domain(3.0 * fern::half_pi<double>()));

        BOOST_CHECK(!policy.within_domain(-1.0 * fern::half_pi<double>()));
        BOOST_CHECK( policy.within_domain(-2.0 * fern::half_pi<double>()));
        BOOST_CHECK(!policy.within_domain(-3.0 * fern::half_pi<double>()));
    }
}

template<
    class Value,
    class Result>
void verify_zero(
    Value const& value)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_want{0};
    Result result_we_get;
    fa::trigonometry::tan(sequential, value, result_we_get);
    BOOST_CHECK_CLOSE(1.0 + result_we_get, 1.0 + result_we_want, 1e-10);
}

template<
    class Value,
    class Result>
void verify_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::trigonometry::tan(sequential, value, result_we_get);

    // TODO mingw 32 bit / gcc 4.8.2 requires us to use 4e-3. Other compilers
    //      allow the use of 1e-10.
    //      Check compiler and version and restore original epsilon.
    BOOST_CHECK_CLOSE(result_we_get, result_we_want, 4e-3);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_zero<double, double>(0.0);
    verify_zero<double, double>(-0.0);

    verify_zero<double, double>(1.0 * fern::pi<double>());
    verify_zero<double, double>(2.0 * fern::pi<double>());
    verify_zero<double, double>(-1.0 * fern::pi<double>());
    verify_zero<double, double>(-2.0 * fern::pi<double>());

    verify_value<double, double>(fern::half_pi<double>(),
        std::tan(fern::half_pi<double>()));
    verify_value<double, double>(-fern::half_pi<double>(),
        std::tan(-fern::half_pi<double>()));
}


/**
 *
 * Copyright (C) 2021 Mohammad Javad Dousti, Qing Xie, Mahdi Nazemi,
 * and Massoud Pedram. All rights reserved.
 *
 * Please refer to the LICENSE file for terms of use.
 *
 */

#pragma once

#include <Eigen/SparseCore>
#include <Eigen/Core>

#include "device.hpp"
#include "general.hpp"
#include "rc_utils.hpp"
#include "utils.hpp"

/* model specific constants */
/* changed from 1/2 to 1/3 due to the difference from traditional Elmore Delay
 * scenario */
//#define C_FACTOR    0.33       /* fitting factor to match floworks (due to
// lumping)    */

class Model {
private:
  Device *device;

  Eigen::SparseMatrix<VALUE> g_matrix_;

  // A = C^-1 * G
  Eigen::SparseMatrix<VALUE> a_matrix_;

  // Inverted form of diagonal matrix C
  Eigen::DiagonalMatrix<VALUE, Eigen::Dynamic, Eigen::Dynamic> inv_c_;

  /**
   * This saves "device->getTemperature() * K1",
   * which comes from the lhs of the equation due to dropping
   * the term corresponds to the thermal coupling to the ambient.
   */
  Eigen::Matrix<VALUE, Eigen::Dynamic, 1> amb_vector_;
  Eigen::Matrix<VALUE, Eigen::Dynamic, 1> p_vector_;
  Eigen::Matrix<VALUE, Eigen::Dynamic, 1> t_vector_;

  bool p_vector_made_;
  bool g_matrix_made_;
  bool c_vector_made_;
  bool t_vector_made_;
  bool transient_;
  int elements_no_;
  unordered_map<string, int> powerMappingDeviceOrder;
  unordered_map<int, string> powerMappingTraceOrder;
  std::ifstream powerTraceFile;
  bool isComment(string s);
  /**
   * This designates the number of power consumers we
   * expect to find in the power trace file It is used to
   * check if the power trace has enough info
   */
  int pwr_consumers_cnt;
  string powerTraceFileAddr;

  void initPowerVector();
  void preparePVector();

public:
  void makePVector();
  void makeResistanceModel();
  // void makeResistanceModel2();
  void makeCapacitanceModel();

  /**
   * This function reads one line from the power trace file and place it
   * in the power vector
   *
   * @return Number of succesfully read power values from the last read row.
   */
  unsigned read_power();

  void solveSteadyState();
  void solveTransientState();
  void printSubComponentTemp();
  void printComponentTemp(string file_output);
  void printComponentTemp(string file_output, unsigned step_no);
  void printGMatrix(string file_output);
  void printAMatrix(string file_output);
  void printInvCVector(string file_output);
  void printPVector(string file_output);
  void printTVector(string file_output);
  void printElementCount(string file_output);
  Model(Device *device, bool isTransient);
  virtual ~Model();
};


//  (C) Copyright Jeremy William Murphy 2016.

//  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_MATH_TOOLS_POLYNOMIAL_GCD_HPP
#define BOOST_MATH_TOOLS_POLYNOMIAL_GCD_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/polynomial.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/type_traits/is_pod.hpp>

namespace boost{

   namespace integer {

      namespace gcd_detail {

         template <class T>
         struct gcd_traits;

         template <class T>
         struct gcd_traits<boost::math::tools::polynomial<T> >
         {
            inline static const boost::math::tools::polynomial<T>& abs(const boost::math::tools::polynomial<T>& val) { return val; }

            static const method_type method = method_euclid;
         };

      }
}

namespace math{ namespace tools{

/* From Knuth, 4.6.1:
*
* We may write any nonzero polynomial u(x) from R[x] where R is a UFD as
*
*      u(x) = cont(u) · pp(u(x))
*
* where cont(u), the content of u, is an element of S, and pp(u(x)), the primitive
* part of u(x), is a primitive polynomial over S.
* When u(x) = 0, it is convenient to define cont(u) = pp(u(x)) = O.
*/

template <class T>
T content(polynomial<T> const &x)
{
    return x ? gcd_range(x.data().begin(), x.data().end()).first : T(0);
}

// Knuth, 4.6.1
template <class T>
polynomial<T> primitive_part(polynomial<T> const &x, T const &cont)
{
    return x ? x / cont : polynomial<T>();
}

template <class T>
polynomial<T> primitive_part(polynomial<T> const &x)
{
    return primitive_part(x, content(x));
}

// Trivial but useful convenience function referred to simply as l() in Knuth.
template <class T>
T leading_coefficient(polynomial<T> const &x)
{
    return x ? x.data().back() : T(0);
}

namespace detail
{
    /* Reduce u and v to their primitive parts and return the gcd of their
    * contents. Used in a couple of gcd algorithms.
    */
    template <class T>
    T reduce_to_primitive(polynomial<T> &u, polynomial<T> &v)
    {
        using boost::math::gcd;
        T const u_cont = content(u), v_cont = content(v);
        u /= u_cont;
        v /= v_cont;
        return gcd(u_cont, v_cont);
    }
}

/**
* Knuth, The Art of Computer Programming: Volume 2, Third edition, 1998
* Algorithm 4.6.1C: Greatest common divisor over a unique factorization domain.
*
* The subresultant algorithm by George E. Collins [JACM 14 (1967), 128-142],
* later improved by W. S. Brown and J. F. Traub [JACM 18 (1971), 505-514].
*
* Although step C3 keeps the coefficients to a "reasonable" size, they are
* still potentially several binary orders of magnitude larger than the inputs.
* Thus, this algorithm should only be used where T is a multi-precision type.
*
* @tparam   T   Polynomial coefficient type.
* @param    u   First polynomial.
* @param    v   Second polynomial.
* @return       Greatest common divisor of polynomials u and v.
*/
template <class T>
typename enable_if_c< std::numeric_limits<T>::is_integer, polynomial<T> >::type
subresultant_gcd(polynomial<T> u, polynomial<T> v)
{
    using std::swap;
    BOOST_ASSERT(u || v);

    if (!u)
        return v;
    if (!v)
        return u;

    typedef typename polynomial<T>::size_type N;

    if (u.degree() < v.degree())
        swap(u, v);

    T const d = detail::reduce_to_primitive(u, v);
    T g = 1, h = 1;
    polynomial<T> r;
    while (true)
    {
        BOOST_ASSERT(u.degree() >= v.degree());
        // Pseudo-division.
        r = u % v;
        if (!r)
            return d * primitive_part(v); // Attach the content.
        if (r.degree() == 0)
            return d * polynomial<T>(T(1)); // The content is the result.
        N const delta = u.degree() - v.degree();
        // Adjust remainder.
        u = v;
        v = r / (g * detail::integer_power(h, delta));
        g = leading_coefficient(u);
        T const tmp = detail::integer_power(g, delta);
        if (delta <= N(1))
            h = tmp * detail::integer_power(h, N(1) - delta);
        else
            h = tmp / detail::integer_power(h, delta - N(1));
    }
}

/**
 * @brief GCD for polynomials with unbounded multi-precision integral coefficients.
 *
 * The multi-precision constraint is enforced via numeric_limits.
 *
 * Note that intermediate terms in the evaluation can grow arbitrarily large, hence the need for
 * unbounded integers, otherwise numeric loverflow would break the algorithm.
 *
 * @tparam  T   A multi-precision integral type.
 */
template <typename T>
typename enable_if_c<std::numeric_limits<T>::is_integer && !std::numeric_limits<T>::is_bounded, polynomial<T> >::type
gcd(polynomial<T> const &u, polynomial<T> const &v)
{
    return subresultant_gcd(u, v);
}
// GCD over bounded integers is not currently allowed:
template <typename T>
typename enable_if_c<std::numeric_limits<T>::is_integer && std::numeric_limits<T>::is_bounded, polynomial<T> >::type
gcd(polynomial<T> const &u, polynomial<T> const &v)
{
   BOOST_STATIC_ASSERT_MSG(sizeof(v) == 0, "GCD on polynomials of bounded integers is disallowed due to the excessive growth in the size of intermediate terms.");
   return subresultant_gcd(u, v);
}
// GCD over polynomials of floats can go via the Euclid algorithm:
template <typename T>
typename enable_if_c<!std::numeric_limits<T>::is_integer && (std::numeric_limits<T>::min_exponent != std::numeric_limits<T>::max_exponent) && !std::numeric_limits<T>::is_exact, polynomial<T> >::type
gcd(polynomial<T> const &u, polynomial<T> const &v)
{
   return boost::integer::gcd_detail::Euclid_gcd(u, v);
}

}
//
// Using declaration so we overload the default implementation in this namespace:
//
using boost::math::tools::gcd;

}

namespace integer
{
   //
   // Using declaration so we overload the default implementation in this namespace:
   //
   using boost::math::tools::gcd;
}

} // namespace boost::math::tools

#endif


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    typedef std::vector<std::pair<std::string, nombre>> S;
    typedef std::map<nombre, std::set<S>> C;

    C construire(const S &score, const C &combinaisons) {
        auto suivant = combinaisons;
        for (const auto &s: score) {
            for (const auto &r: combinaisons) {
                auto c = r.second;
                for (auto f: c) {
                    f.push_back(s);
                    std::sort(f.begin(), f.end());
                    suivant[s.second + r.first].insert(f);
                }
            }
        }
        return suivant;
    };
}

ENREGISTRER_PROBLEME(109, "Darts") {
    // In the game of darts a player throws three darts at a target board which is split into twenty equal sized
    // sections numbered one to twenty.
    //
    // The score of a dart is determined by the number of the region that the dart lands in. A dart landing outside the
    // red/green outer ring scores zero. The black and cream regions inside this
    // ring represent single scores. However, the red/green outer ring and middle ring score double and treble scores
    // respectively.
    //
    // At the centre of the board are two concentric circles called the bull region, or bulls-eye.
    // The outer bull is worth 25 points and the inner bull is a double, worth 50 points.
    //
    // There are many variations of rules but in the most popular game the players will begin with a score 301 or 501
    // and the first player to reduce their running total to zero is a winner.
    // However, it is normal to play a "doubles out" system, which means that the player must land a double (including
    // the double bulls-eye at the centre of the board) on their final dart to win;
    // any other dart that would reduce their running total to one or lower means the score for that set of three darts
    // is "bust".
    //
    // When a player is able to finish on their current score it is called a "checkout" and the highest checkout is 170:
    // T20 T20 D25 (two treble 20s and double bull).
    //
    // There are exactly eleven distinct ways to checkout on a score of 6:
    //
    //      D3
    //      D1	D2
    //      S2	D2
    //      D2	D1
    //      S4	D1
    //      S1	S1	D2
    //      S1	T1	D1
    //      S1	S3	D1
    //      D1	D1	D1
    //      D1	S2	D1
    //      S2	S2	D1
    //
    // Note that D1 D2 is considered different to D2 D1 as they finish on different doubles. However, the combination
    // S1 T1 D1 is considered the same as T1 S1 D1.
    //
    // In addition we shall not include misses in considering combinations; for example, D3 is the same as 0 D3 and
    // 0 0 D3.
    //
    // Incredibly there are 42336 distinct ways of checking out in total.
    //
    // How many distinct ways can a player checkout with a score less than 100?
    S score;
    S score_double;

    for (nombre n = 1; n < 21; ++n) {
        score.emplace_back(utilitaires::concatener("S", n), n);
        score.emplace_back(utilitaires::concatener("D", n), 2 * n);
        score.emplace_back(utilitaires::concatener("T", n), 3 * n);
        score_double.emplace_back(utilitaires::concatener("D", n), 2 * n);
    }

    score.emplace_back(utilitaires::concatener("S", 25), 25);
    score.emplace_back(utilitaires::concatener("D", 25), 2 * 25);
    score_double.emplace_back(utilitaires::concatener("D", 25), 2 * 25);

    const S zero{std::make_pair("0", 0)};
    C combinaisons;
    combinaisons[0].insert(zero);

    combinaisons = construire(score, combinaisons);
    combinaisons = construire(score, combinaisons);

    C solution;
    for (const auto &s: score_double) {
        for (const auto &r: combinaisons) {
            auto c = r.second;
            for (auto f: c) {
                f.push_back(s);
                solution[s.second + r.first].insert(f);
            }
        }
    }

    nombre resultat = 0;
    for (const auto &r: solution) {
        if (r.first < 100)
            resultat += r.second.size();
    }

    return std::to_string(resultat);
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#ifndef __Thea_Algorithms_StdLinearSolver_hpp__
#define __Thea_Algorithms_StdLinearSolver_hpp__

#include "../Common.hpp"
#include "../Array.hpp"
#include "../MatVec.hpp"
#include "../SparseMatVec.hpp"
#include "ILinearSolver.hpp"
#include "NNLS/nnls.h"
#include <Eigen/QR>
#include <Eigen/SVD>

namespace Thea {
namespace Algorithms {

// Forward declarations
namespace StdLinearSolverInternal { class StdLinearSolverImpl; }

/**
 * Solve dense and sparse linear systems of the form Ax = b for x. This class implements the ILinearSolver interface to provide a
 * variety of built-in algorithms. Other solvers may be provided by plugins implementing the ILinearSolver interface.
 */
class THEA_API StdLinearSolver : public virtual ILinearSolver, public NamedObject
{
  public:
    THEA_DECL_SMART_POINTERS(StdLinearSolver)

    /** The constraints to be imposed on the solution (enum class). */
    struct THEA_API Constraint
    {
      /** Supported values. */
      enum Value
      {
        UNCONSTRAINED,  ///< The solution is not constrained.
        NON_NEGATIVE    ///< The solution must have non-negative elements only.
      };

      THEA_ENUM_CLASS_BODY(Constraint)

      THEA_ENUM_CLASS_STRINGS_BEGIN(Constraint)
        THEA_ENUM_CLASS_STRING(UNCONSTRAINED,  "unconstrained")
        THEA_ENUM_CLASS_STRING(NON_NEGATIVE,   "non-negative")
      THEA_ENUM_CLASS_STRINGS_END(Constraint)
    };

    /** Solution methods (enum class). */
    struct THEA_API Method
    {
      /** Supported values. */
      enum Value
      {
        DEFAULT,                             ///< Automatically pick a solution method.

        // Dense solvers
        HOUSEHOLDER_QR,                      ///< Eigen's HouseholderQR solver.
        COL_PIV_HOUSEHOLDER_QR,              ///< Eigen's ColPivHouseholderQR solver.
        FULL_PIV_HOUSEHOLDER_QR,             ///< Eigen's FullPivHouseholderQR solver.
        COMPLETE_ORTHOGONAL_DECOMPOSITION,   ///< Eigen's CompleteOrthogonalDecomposition solver.
        BDCSVD,                              ///< Eigen's BDCSVD solver.
        NNLS,                                ///< NNLS non-negative least squares solver.

        // Sparse solvers
        SIMPLICIALT_LLT,                     ///< Eigen's SimplicialLLT solver.
        SIMPLICIALT_LDLT,                    ///< Eigen's SimplicialLDLT solver.
        SPARSE_LU,                           ///< Eigen's SparseLU solver.
        SPARSE_QR,                           ///< Eigen's SparseQR solver.
        CONJUGATE_GRADIENT,                  ///< Eigen's ConjugateGradient solver.
        LEAST_SQUARES_CONJUGATE_GRADIENT,    ///< Eigen's LeastSquaresConjugateGradient solver.
        BICGSTAB,                            ///< Eigen's BiCGSTAB solver.
      };

      THEA_ENUM_CLASS_BODY(Method)

      THEA_ENUM_CLASS_STRINGS_BEGIN(Method)
        THEA_ENUM_CLASS_STRING(DEFAULT,                            "default")

        THEA_ENUM_CLASS_STRING(HOUSEHOLDER_QR,                     "HouseholderQR")
        THEA_ENUM_CLASS_STRING(COL_PIV_HOUSEHOLDER_QR,             "ColPivHouseholderQR")
        THEA_ENUM_CLASS_STRING(FULL_PIV_HOUSEHOLDER_QR,            "FullPivHouseholderQR")
        THEA_ENUM_CLASS_STRING(COMPLETE_ORTHOGONAL_DECOMPOSITION,  "CompleteOrthogonalDecomposition")
        THEA_ENUM_CLASS_STRING(BDCSVD,                             "BDSVD")
        THEA_ENUM_CLASS_STRING(NNLS,                               "NNLS")

        THEA_ENUM_CLASS_STRING(SIMPLICIALT_LLT,                    "SimplicialLLT")
        THEA_ENUM_CLASS_STRING(SIMPLICIALT_LDLT,                   "SimplicialLDLT")
        THEA_ENUM_CLASS_STRING(SPARSE_LU,                          "SparseLU")
        THEA_ENUM_CLASS_STRING(SPARSE_QR,                          "SparseQR")
        THEA_ENUM_CLASS_STRING(CONJUGATE_GRADIENT,                 "ConjugateGradient")
        THEA_ENUM_CLASS_STRING(LEAST_SQUARES_CONJUGATE_GRADIENT,   "LeastSquaresConjugateGradient")
        THEA_ENUM_CLASS_STRING(BICGSTAB,                           "BiCGSTAB")
      THEA_ENUM_CLASS_STRINGS_END(Method)
    };

    /** Constructor. */
    StdLinearSolver(Method method_ = Method::DEFAULT, Constraint constraint_ = Constraint::UNCONSTRAINED);

    /** Destructor. */
    ~StdLinearSolver();

    /** Get the solution method. */
    Method getMethod() const;

    /** Get the solution constraint. */
    Constraint getConstraint() const;

    /* Get the solution tolerance/threshold. A negative number implies the default tolerance. */
    double getTolerance() const;

    /* Get the maximum number of solver iterations, if the solver is iterative. */
    intx maxIterations() const;

    /** Set the solution method. */
    void setMethod(Method method_);

    /** Set the solution constraint. */
    void setConstraint(Constraint constraint_);

    /* Set the solution tolerance/threshold. */
    void setTolerance(float64 tol);

    /* Get the maximum number of solver iterations, if the solver is iterative. A negative number implies the default value. */
    void setMaxIterations(intx max_iters_);

    /** Solve the linear system Ax = b for a dense double-precision matrix A. */
    bool solve(Eigen::Ref< MatrixXd > const & a, double const * b, IOptions const * options = nullptr);

    /** Solve the linear system Ax = b for a sparse double-precision matrix A. */
    bool solve(Eigen::Ref< SparseMatrix<double> > const & a, double const * b, IOptions const * options = nullptr);

    // Functions from ILinearSolver
    int8 THEA_ICALL solve(IMatrix<float64> const * a, float64 const * b, IOptions const * options = nullptr);
    int64 THEA_ICALL dims() const;
    int8 THEA_ICALL hasSolution() const;
    float64 const * THEA_ICALL getSolution() const;
    int8 THEA_ICALL getSquaredError(float64 * err) const;

  private:
    StdLinearSolverInternal::StdLinearSolverImpl * impl;  ///< Contains base function implementations using PIMPL idiom.

}; // class StdLinearSolver

} // namespace Algorithms
} // namespace Thea

#endif


extern "C"  {
        #include "postgres.h"
        #include "catalog/pg_type.h"
        #include "fmgr.h"
}
#include "random.h"
#include "../array.h"

#include <boost/random.hpp>

template<class D> static Datum _gen_rand(PG_FUNCTION_ARGS, D& dist, int dim_arr_idx = 0) {
  typedef typename D::result_type T;
  ArrayType* arr = PG_GETARG_ARRAYTYPE_P_COPY(dim_arr_idx);

  if (ARR_NDIM(arr) == 0 ) {
     PG_RETURN_POINTER(construct_empty_array(oid_type<double>()));
  } else if (ARR_NDIM(arr) != 1 ) {
     elog(ERROR, "GEN_RAND: Size specification can only be 1-dimension array");
     PG_RETURN_NULL();
  } else if (ARR_DIMS(arr)[0] >= 4 ) {
     elog(ERROR, "GEN_RAND: Maximum of 4 dimensions in array generation.");
     PG_RETURN_NULL();
  }

  //compute number of dimensions
  ArrDims rd = extract_shape(arr);
  Datum* r = (Datum*)palloc(rd.n * sizeof(Datum));
  for (size_t i = 0; i < rd.n; ++i ) {
     r[i] = _ret<T>(dist(rgen));
  }

  ArrayType* res = construct_md_array(r,
                                      NULL,
                                      rd.ndims,
                                      rd.dims,
                                      rd.lbs,
                                      oid_type<T>(),
                                      sizeof(T),
                                      true,
                                      oid_align<T>());
  PG_RETURN_POINTER(res);
}

#define RAND_FUNCTION(NAME) \
	extern "C" { PG_FUNCTION_INFO_V1(NAME); Datum NAME(PG_FUNCTION_ARGS); } \
        Datum NAME(PG_FUNCTION_ARGS)

RAND_FUNCTION(gen_rand) {
  std::uniform_real_distribution<double> dist{0.0, 1.0};
  return _gen_rand(fcinfo, dist);
}

RAND_FUNCTION(gen_randn) {
  //for soem reason can't determine "std' variant is producing warnings.
  std::normal_distribution<double> dist{0.0, 1.0};
  return _gen_rand(fcinfo, dist);
}

RAND_FUNCTION(gen_randint) {
  long low = get_param<long>(fcinfo, 0);
  long high = get_param<long>(fcinfo, 1);
  if (high <= low ){
     elog(ERROR, "GEN_RANDINT: Cannot have high bound lower then low bound");
     PG_RETURN_NULL();
  } else {
     std::uniform_int_distribution<> dist(low, high);
     return _gen_rand(fcinfo, dist, 2);
  }
}

RAND_FUNCTION(gen_beta) {
  float alpha = get_param<double>(fcinfo, 0);
  float beta  = get_param<double>(fcinfo, 1);
  if (alpha < 0 || beta < 0){
     elog(ERROR, "BETA: Can't have negative alpha/beta parameters");
     PG_RETURN_NULL();
  } else {
     boost::random::beta_distribution<double> dist(alpha, beta);
     return _gen_rand(fcinfo, dist, 2);
  }
}

RAND_FUNCTION(gen_binomial) {
  int t = get_param<int>(fcinfo, 0);
  float p  = get_param<double>(fcinfo, 1);
  if (t < 0 || p < 0 || p > 1){
     elog(ERROR, "BINOMIAL: Input parameters outside of range");
     PG_RETURN_NULL();
  } else {
     boost::random::binomial_distribution<int> dist(t,p);
     return _gen_rand(fcinfo, dist, 2);
  }
}


#include <iostream>
#include <math.h>
#include "Vector.h"
#include <boost/multiprecision/gmp.hpp>

using namespace std;

int target = 1000;

int main(int argc, char** argv) {
  boost::multiprecision::mpz_int min = 1;
  for (int i = 1; i < target; i++) {
    min *= 10;
  }
  Vector<boost::multiprecision::mpz_int> fibonacci;
  fibonacci.insertBack(1);
  fibonacci.insertBack(1);
  while (fibonacci[fibonacci.length() - 1] < min) {
    int end = fibonacci.length() - 1;
    fibonacci.insertBack(fibonacci[end] + fibonacci[end - 1]);
  }
  cout << "Index of first Fibonacci number with " << target << " digits: " << fibonacci.length() << endl;
  return 0;
}


/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#include <Eigen/Dense>

#include "fvElement.h"

#include "fvElementBuilder.h"

namespace Vitelotte
{

template < class _Mesh, typename _Scalar >
FVElementBuilder<_Mesh, _Scalar>::FVElementBuilder(Scalar sigma)
  : m_sigma(sigma)
{
}

template < class _Mesh, typename _Scalar >
unsigned
FVElementBuilder<_Mesh, _Scalar>::
    nCoefficients(const Mesh& mesh, Face element,
                  SolverError* /*error*/) const
{
    return mesh.nVertexGradientConstraints(element)? 61: 45;
}

template < class _Mesh, typename _Scalar >
unsigned
FVElementBuilder<_Mesh, _Scalar>::
    nExtraConstraints(const Mesh& mesh, Face element) const
{
    return mesh.nVertexGradientConstraints(element)? 2: 0;
}

template < class _Mesh, typename _Scalar >
template < typename Inserter >
void
FVElementBuilder<_Mesh, _Scalar>::
    addCoefficients(Inserter& inserter, const Mesh& mesh,
                    Face element, SolverError* error)
{
    if(mesh.valence(element) != 3)
    {
        if(error) error->error("Non-triangular face");
        return;
    }

    typedef Eigen::Matrix<Scalar, 9, 9> Matrix9;
    Matrix9 sm;

    int nodes[9];

    typename Mesh::HalfedgeAroundFaceCirculator hit = mesh.halfedges(element);
    typename Mesh::HalfedgeAroundFaceCirculator hend = hit;
    do ++hit;
    while(!mesh.isGradientConstraint(mesh.toVertex(*hit)) && hit != hend);
    bool isPgc = mesh.isGradientConstraint(mesh.toVertex(*hit));

    bool orient[3];
    // TODO: remove dynamic allocation with dynamic dims.
    Vector p[3];
    --hit;
    for(int i = 0; i < 3; ++i)
    {
        orient[i] = mesh.halfedgeOrientation(*hit);
        nodes[3+i] = mesh.edgeValueNode(*hit).idx();
        nodes[6+i] = mesh.edgeGradientNode(*hit).idx();
        ++hit;
        nodes[i] = mesh.toVertexValueNode(*hit).idx();
        p[i] = mesh.position(mesh.toVertex(*hit)).template cast<Scalar>();
    }

    for(int i = 0; i < 9; ++i)
    {
        if(nodes[i] < 0)
        {
            if(error) error->error("Invalid node");
            return;
        }
    }

    typedef FVElement<Scalar> Elem;
    Elem elem(p);

    if(elem.doubleArea() <= 0)
    {
        if(error) error->warning("Degenerated or reversed triangle");
    }

    typedef Eigen::Array<Scalar, 3, 1> Array3;
    Array3 dx2[9];
    Array3 dy2[9];
    Array3 dxy[9];
    for(int pi = 0; pi < 3; ++pi)
    {
        Vector3 bc((pi == 0)? 0: .5,
                   (pi == 1)? 0: .5,
                   (pi == 2)? 0: .5);
        typename Elem::Hessian hessians[9];
        elem.hessian(bc, hessians);

        for(int bi = 0; bi < 9; ++bi)
        {
            dx2[bi](pi) = hessians[bi](0, 0);
            dy2[bi](pi) = hessians[bi](1, 1);
            dxy[bi](pi) = hessians[bi](0, 1);
        }
    }

    for(size_t i = 0; i < 9; ++i)
    {
        for(size_t j = i; j < 9; ++j)
        {
            EIGEN_ASM_COMMENT("MYBEGIN");

            Array3 quadPointValue =
                    (dx2[i]+dy2[i]) * (dx2[j]+dy2[j])
                  + (1.-m_sigma) * (
                        2. * dxy[i] * dxy[j]
                      - dx2[i] * dy2[j]
                      - dx2[j] * dy2[i]);

            Scalar value = quadPointValue.sum() * (elem.doubleArea() / 6);

            EIGEN_ASM_COMMENT("MYEND");

            if((i < 6 || orient[i%3]) != (j < 6 || orient[j%3]))
            {
                value *= -1;
            }

            sm(i, j) = value;
            sm(j, i) = value;
        }
    }

    for(size_t i = 0; i < 9; ++i)
    {
        for(size_t j = 0; j < 9; ++j)
        {
            if(nodes[i] < nodes[j]) continue;
            inserter.addCoeff(nodes[i], nodes[j], sm(i, j));
        }
    }

    if(isPgc)
    {
        typedef Eigen::Matrix<Scalar, 9, 1> Vector9;
        Vector9 fde1, fde2;
        fde1 <<
            -1.0L/2.0L*(elem.doubleArea()*(2*elem.dldn(0, 1) + elem.dldn(1, 1)) + 7*elem.edgeLength(1))/(elem.edgeLength(1)*elem.edgeLength(2)),
            (1.0L/2.0L)*(elem.doubleArea()*(elem.dldn(0, 0) + 2*elem.dldn(1, 0)) - elem.edgeLength(0))/(elem.edgeLength(0)*elem.edgeLength(2)),
            -1.0L/2.0L*elem.doubleArea()*(elem.edgeLength(0)*(elem.dldn(1, 1) + 2*elem.dldn(2, 1)) - elem.edgeLength(1)*(elem.dldn(0, 0) + 2*elem.dldn(2, 0)))/(elem.edgeLength(0)*elem.edgeLength(1)*elem.edgeLength(2)),
            -4/elem.edgeLength(2),
            4/elem.edgeLength(2),
            4/elem.edgeLength(2),
            elem.doubleArea()/(elem.edgeLength(0)*elem.edgeLength(2)),
            -elem.doubleArea()/(elem.edgeLength(1)*elem.edgeLength(2)),
            0;
        fde2 <<
            -1.0L/2.0L*(elem.doubleArea()*(2*elem.dldn(0, 2) + elem.dldn(2, 2)) + 7*elem.edgeLength(2))/(elem.edgeLength(1)*elem.edgeLength(2)),
            -1.0L/2.0L*elem.doubleArea()*(elem.edgeLength(0)*(2*elem.dldn(1, 2) + elem.dldn(2, 2)) - elem.edgeLength(2)*(elem.dldn(0, 0) + 2*elem.dldn(1, 0)))/(elem.edgeLength(0)*elem.edgeLength(1)*elem.edgeLength(2)),
            (1.0L/2.0L)*(elem.doubleArea()*(elem.dldn(0, 0) + 2*elem.dldn(2, 0)) - elem.edgeLength(0))/(elem.edgeLength(0)*elem.edgeLength(1)),
            -4/elem.edgeLength(1),
            4/elem.edgeLength(1),
            4/elem.edgeLength(1),
            elem.doubleArea()/(elem.edgeLength(0)*elem.edgeLength(1)),
            0,
            -elem.doubleArea()/(elem.edgeLength(1)*elem.edgeLength(2));

        for(size_t i = 0; i < 9; ++i)
        {
            Scalar f = (i < 6 || orient[i%3])? 1: -1;
            if(i != 8 /*fde1(i) != Scalar(0.)*/)
            {
                inserter.addExtraCoeff(element, 1, nodes[i], fde1(i) * f);
            }
            if(i != 7 /*fde2(i) != Scalar(0.)*/)
            {
                inserter.addExtraCoeff(element, 0, nodes[i], fde2(i) * f);
            }
        }
    }
}

template < class _Mesh, typename _Scalar >
template < typename Inserter >
void
FVElementBuilder<_Mesh, _Scalar>::
    addExtraConstraints(Inserter& inserter, const Mesh& mesh,
                                Face element, SolverError* /*error*/)
{
    typename Mesh::HalfedgeAroundFaceCirculator hit = mesh.halfedges(element);
    typename Mesh::HalfedgeAroundFaceCirculator hend = hit;
    do ++hit;
    while(!mesh.isGradientConstraint(mesh.toVertex(*hit)) && hit != hend);
    if(!mesh.isGradientConstraint(mesh.toVertex(*hit))) {
        return;
    }

    for(unsigned hi = 0; hi < 2; ++hi)
    {
        typename Mesh::Halfedge h = *hit;

        typename Mesh::Vertex from = mesh.fromVertex(h);
        typename Mesh::Vertex to   = mesh.  toVertex(h);

        bool v0c = mesh.isGradientConstraint(from);
        const typename Mesh::Gradient& grad = mesh.gradientConstraint(v0c? from: to);
        typename Mesh::Vector v = mesh.position(to) - mesh.position(from);
        if(!v0c) v = -v;
        typename Mesh::Value cons = grad * v;
        inserter.setExtraRhs(element, hi, cons.template cast<Scalar>());

        ++hit;
    }

}

}


#include <iostream>
#include <cmath>
#include <chrono>
#include <limits>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/core/eigen.hpp>

#include "pose2d.h"
#include "Ransac.hh"
#include "PoseRANSAC.hh"

//#define USE_QUATERNION
#define USE_ROTATION_MATRIX

namespace pose2d
{
/* Matrix coefficients for the non-depth case matrix to be solved by SVD */
   inline double tx_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//---------------------------------------------------------------------------------------------------
   {
      return (R(1, 2) + R(1, 0) * xp1 + R(1, 1) * yp1 - R(2, 2) * yq1 + (-(R(2, 0) * xp1) - R(2, 1) * yp1) * yq1);
      //return (R(1,2) + R(1,0)*xq1 - R(2,2)*yp1 - R(2,0)*xq1*yp1 + R(1,1)*yq1 - R(2,1)*yp1*yq1);
   }

   inline double ty_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//----------------------------------------------------------------------------------------------------
   {
      return (-R(0, 2) - R(0, 0) * xp1 + R(2, 2) * xq1 + R(2, 0) * xp1 * xq1 - R(0, 1) * yp1 + R(2, 1) * xq1 * yp1);
      //return ((-R(0,2) + R(2,2)*xp1 - R(0,0)*xq1 + R(2,0)*xp1*xq1 - R(0,1)*yq1 + R(2,1)*xp1*yq1));
   }

   inline double tz_coeff(double xp1, double yp1, double xq1, double yq1, const Eigen::Matrix3d& R)
//---------------------------------------------------------------------------------------------------
   {
      return (-(R(1, 2) * xq1) - R(1, 0) * xp1 * xq1 - R(1, 1) * xq1 * yp1 +
              (R(0, 2) + R(0, 0) * xp1 + R(0, 1) * yp1) * yq1);
//   return (-(R(1,2)*xp1) - R(1,0)*xp1*xq1 + R(0,2)*yp1 + R(0,0)*xq1*yp1 - R(1,1)*xp1*yq1 + R(0,1)*yp1*yq1);
   }

/* RHS values for depth solution*/
   inline double b0(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0, const double y_1)
   {
      return d_0 *
             (-R(1, 0) * x_0 - R(1, 1) * y_0 - R(1, 2) + R(2, 0) * x_0 * y_1 + R(2, 1) * y_0 * y_1 + R(2, 2) * y_1);
   }

   inline double b1(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0, const double x_1)
   {
      return -d_0 *
             (-R(0, 0) * x_0 - R(0, 1) * y_0 - R(0, 2) + R(2, 0) * x_0 * x_1 + R(2, 1) * x_1 * y_0 + R(2, 2) * x_1);
   }

   inline double b2(const Eigen::Matrix3d& R, const double x_0, const double y_0, const double d_0,
                    const double x_1, const double y_1)
   {
      return d_0 *
             (-R(0, 0) * x_0 * y_1 - R(0, 1) * y_0 * y_1 - R(0, 2) * y_1 + R(1, 0) * x_0 * x_1 + R(1, 1) * x_1 * y_0 +
              R(1, 2) * x_1);
   }

   inline double b0(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double y_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQx = Qw * Qx;
      return d * (-Qw2 * y_1 - 2 * QwQx * y_0 * y_1 - 2 * QwQx + 2 * Qw * Qy * x_0 * y_1 + Qx2 * y_1 -
                  2 * Qx * Qz * x_0 * y_1 +
                  Qy2 * y_1 - 2 * Qy * Qz * y_0 * y_1 + 2 * Qy * Qz - Qz2 * y_1 + 2 * x_0 * (Qw * Qz + Qx * Qy) +
                  y_0 * (Qw2 -
                         Qx2 + Qy2 - Qz2));
   }

   inline double b1(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double x_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQy = Qw * Qy, QxQz = Qx * Qz;
      return -d *
             (-Qw2 * x_1 - 2 * Qw * Qx * x_1 * y_0 + 2 * QwQy * x_0 * x_1 + 2 * QwQy - 2 * Qw * Qz * y_0 + Qx2 * x_1 +
              2 * Qx * Qy * y_0 - 2 * QxQz * x_0 * x_1 + 2 * QxQz + Qy2 * x_1 - 2 * Qy * Qz * x_1 * y_0 - Qz2 * x_1 +
              x_0 * (Qw2 + Qx2 - Qy2 - Qz2));
   }

   inline double b2(double Qw, double Qx, double Qy, double Qz, double x_0, double y_0, double x_1, double y_1, double d)
   {
      double Qw2 = Qw * Qw, Qx2 = Qx * Qx, Qy2 = Qy * Qy, Qz2 = Qz * Qz, QwQz = Qw * Qz, QxQy = Qx * Qy;
      return -d *
             (-Qw2 * x_0 * y_1 - 2 * Qw * Qx * x_1 - 2 * Qw * Qy * y_1 + 2 * QwQz * x_0 * x_1 + 2 * QwQz * y_0 * y_1 -
              Qx2 * x_0 * y_1 + 2 * QxQy * x_0 * x_1 - 2 * QxQy * y_0 * y_1 - 2 * Qx * Qz * y_1 + Qy2 * x_0 * y_1 +
              2 * Qy * Qz * x_1 + Qz2 * x_0 * y_1 + x_1 * y_0 * (Qw2 - Qx2 + Qy2 - Qz2));

   }

   inline Eigen::Vector3d homogeneous(Eigen::Matrix3d A)
   //---------------------------------------------------------------------------------------------------------------------------
   {
      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
      return V.col(V.cols() - 1);
   }

   inline Eigen::Quaterniond rotation(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                      const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts, const Eigen::Vector3d& train_g,
             const Eigen::Vector3d query_g, const cv::Mat& intrinsics,
             Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      pose(pts, train_g, query_g, K, Q, translation);
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
             const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //--------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d KI = K.inverse();
      //   std::cout << K << std::endl << KI << std::endl;
      //   if (std::isnan(KI(0,0))) KI = K;

      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
      //   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      Eigen::Matrix3d R = Q.toRotationMatrix();
      size_t m = pts.size();
      Eigen::MatrixXd A(m, 3);
      for (size_t row = 0; row < m; row++)
      {
         const cv::Point3d& tpt = pts[row].first;
         const cv::Point3d& qpt = pts[row].second;
         Eigen::Vector3d train_ray = KI*Eigen::Vector3d(tpt.x, tpt.y, 1);
         Eigen::Vector3d query_ray = KI*Eigen::Vector3d(qpt.x, qpt.y, 1);
         double xt1 = train_ray[0], yt1 = train_ray[1], xq1 = query_ray[0], yq1 = query_ray[1];
         A.row(row) << tx_coeff(xt1, yt1, xq1, yq1, R),
               ty_coeff(xt1, yt1, xq1, yq1, R),
               tz_coeff(xt1, yt1, xq1, yq1, R);
      }

      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto V = svd.matrixV();
      translation = V.col(V.cols() - 1);
//   assert(mut::check_essential(R, translation));
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g, const double depth,
             const cv::Mat& intrinsics, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
//-----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      pose(pts, train_g, query_g, depth, K, Q, translation);
   }

   void pose(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
             const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g, const double depth,
             const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation)
   //--------------------------------------------------------------------------------------
   {
      size_t m = pts.size();
//   if ( (! std::isnan(depth)) && (m > 6 ) )
//   {
//      pose_ransac(pts, train_g, query_g, depth, K, Q, translation, 3);
//      return;
//   }
      Eigen::Matrix3d KI = K.inverse();
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera

      Eigen::Matrix3d R = Q.toRotationMatrix();
      Eigen::MatrixXd A(3 * m, 3);
      Eigen::VectorXd b(m * 3);
      for (size_t row = 0, ri = 0; row < m; row++)
      {
         const cv::Point3d& tpt = pts[row].first;
         const cv::Point3d& qpt = pts[row].second;
         Eigen::Vector3d train_ray = KI*Eigen::Vector3d(tpt.x, tpt.y, 1);
         Eigen::Vector3d query_ray = KI*Eigen::Vector3d(qpt.x, qpt.y, 1);
         double xt1 = train_ray[0], yt1 = train_ray[1], xq1 = query_ray[0], yq1 = query_ray[1];
#ifdef USE_ROTATION_MATRIX
         A.row(ri) << 0, 1, -yq1;
         b(ri++) = b0(R, xt1, yt1, depth, yq1);
         A.row(ri) << -1, 0, xq1;
         b(ri++) = b1(R, xt1, yt1, depth, xq1);
         A.row(ri) << yq1, -xq1, 0;
         b(ri++) = b2(R, xt1, yt1, depth, xq1, yq1);
#endif
#ifdef USE_QUATERNION
         A.row(ri) << 0, -1, yq1;
         b(ri++) = b0(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, yq1, d);
         A.row(ri) <<  1, 0, -xq1;
         b(ri++) = b1(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, xq1, d);
         A.row(ri) << -yq1, xq1, 0;
         b(ri++) = b2(Q.w(), Q.x(), Q.y(), Q.z(), xt1, yt1, xq1, yq1, d);
#endif
      }
   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).rank() << std::endl;
//      Eigen::ColPivHouseholderQR<Eigen::MatrixXd> MQR(A);
//      translation = MQR.solve(b);
   translation = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const cv::Mat& intrinsics, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      return pose_ransac(pts, train_g, query_g, K, Q, translation, RANSAC_params, samples);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const Eigen::Matrix3d& K, Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //-----------------------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose2d::pose_ransac (no depth): RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav2DRansacEstimator estimator(K, Q, -1, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav2DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;
         ransac->Estimate(pts, &best_model, &summary);
         confidence = summary.confidence;
         if (confidence > 0)
         {
            Q = best_model.rotation;
            translation = best_model.translation;
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav2DRansacEstimator estimator(K, Q);
      Grav2DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel> > results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1,
                                       results, inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;
      }
#endif
      return confidence;
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const double depth, const cv::Mat& intrinsics,
                      Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void* RANSAC_params, int samples)
   //----------------------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K((double*) intrinsics.data);
      return pose_ransac(pts, train_g, query_g, depth, K, Q, translation, RANSAC_params, samples);
   }

   double pose_ransac(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                      const Eigen::Vector3d& train_g, const Eigen::Vector3d query_g,
                      const double depth, const Eigen::Matrix3d& K,
                      Eigen::Quaterniond& Q, Eigen::Vector3d& translation,
                      void *RANSAC_params, int samples)
   //---------------------------------------------------------------------------------
   {
      if (RANSAC_params == nullptr) throw std::logic_error("pose2d::pose_ransac (with depth): RANSAC params are null");
      Eigen::Vector3d train_gravity(train_g), query_gravity(query_g);
      train_gravity.normalize();
      query_gravity.normalize();
//   Q = rotation(train_gravity, query_gravity); // camera to model
      Q = rotation(query_gravity, train_gravity); // model to camera
      double confidence = -1;
#ifdef USE_THEIA_RANSAC
      Grav2DRansacEstimator estimator(K, Q, depth, samples);
      theia::RansacParameters* parameters = static_cast<theia::RansacParameters*>(RANSAC_params);
      theia::RansacSummary summary;
      std::unique_ptr<theia::SampleConsensusEstimator<Grav2DRansacEstimator>> ransac =
            theia::CreateAndInitializeRansacVariant(theia::RansacType::RANSAC, *parameters, estimator);
      if (ransac)
      {
         GravPoseRansacModel best_model;

         if (ransac->Estimate(pts, &best_model, &summary))
         {
            confidence = summary.confidence;
            if (confidence > 0)
            {
               Q = best_model.rotation;
               translation = best_model.translation;
            }
         }
      }
#else
      templransac::RANSACParams* parameters = static_cast<templransac::RANSACParams*>(RANSAC_params);
      Grav2DDepthRansacEstimator estimator(K, Q, depth);
      Grav2DRansacData data(pts);
      std::vector<std::pair<double, GravPoseRansacModel>> results;
      std::vector<std::vector<size_t>> inlier_indices;
      std::stringstream errs;
      confidence = templransac::RANSAC(*parameters, estimator, data, pts.size(), samples, 1,
                                       results, inlier_indices, &errs);
      if (confidence > 0)
      {
         GravPoseRansacModel& model = results[0].second;
         translation = model.translation;

//      for (size_t k=0; k<results.size(); k++)
//      {
//         std::pair<double, Grav2DRansacModel> pp = results[k];
//         std::cout << "RANSAC Result " << pp.second.translation.transpose() << " ";
//         std::vector<size_t> inliers = inlier_indices[k];
//         for (size_t inlier : inliers)
//            std::cout << train_img_pts[inlier] << " -> " << query_image_pts[inlier] << " | ";
//         std::cout << std::endl;
//      }
      }
#endif
      return confidence;
   }

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
//----------------------------------------------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = pts[0].first;
      const cv::Point3d& qpt0 = pts[0].second;
      const cv::Point3d& tpt1 = pts[1].first;
      const cv::Point3d& qpt1 = pts[1].second;
      const cv::Point3d& tpt2 = pts[2].first;
      const cv::Point3d& qpt2 = pts[2].second;
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
      Eigen::Matrix3d A;
      A << tx_coeff(xt1, yt1, xq1, yq1, R),
            ty_coeff(xt1, yt1, xq1, yq1, R),
            tz_coeff(xt1, yt1, xq1, yq1, R),

            tx_coeff(xt2, yt2, xq2, yq2, R),
            ty_coeff(xt2, yt2, xq2, yq2, R),
            tz_coeff(xt2, yt2, xq2, yq2, R),

            tx_coeff(xt3, yt3, xq3, yq3, R),
            ty_coeff(xt3, yt3, xq3, yq3, R),
            tz_coeff(xt3, yt3, xq3, yq3, R);
      translation = homogeneous(A);
   }

   //Called by RANSAC estimation
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
//------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = train_img_pts[0];
      const cv::Point3d& tpt1 = train_img_pts[1];
      const cv::Point3d& tpt2 = train_img_pts[2];
      const cv::Point3d& qpt0 = query_img_pts[0];
      const cv::Point3d& qpt1 = query_img_pts[1];
      const cv::Point3d& qpt2 = query_img_pts[2];
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
      Eigen::Matrix3d A;
      A << tx_coeff(xt1, yt1, xq1, yq1, R),
            ty_coeff(xt1, yt1, xq1, yq1, R),
            tz_coeff(xt1, yt1, xq1, yq1, R),

            tx_coeff(xt2, yt2, xq2, yq2, R),
            ty_coeff(xt2, yt2, xq2, yq2, R),
            tz_coeff(xt2, yt2, xq2, yq2, R),

            tx_coeff(xt3, yt3, xq3, yq3, R),
            ty_coeff(xt3, yt3, xq3, yq3, R),
            tz_coeff(xt3, yt3, xq3, yq3, R);
      translation = homogeneous(A);
   }

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = pts[0].first;
      const cv::Point3d& qpt0 = pts[0].second;
      const cv::Point3d& tpt1 = pts[1].first;
      const cv::Point3d& qpt1 = pts[1].second;
      const cv::Point3d& tpt2 = pts[2].first;
      const cv::Point3d& qpt2 = pts[2].second;
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
//   Eigen::MatrixXd A(9, 3);
//   Eigen::VectorXd b(9);
      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
      A << 0, 1, -yq1,
            -1, 0, xq1,
            yq1, -xq1, 0,

            0, 1, -yq2,
            -1, 0, xq2,
            yq2, -xq2, 0,

            0, 1, -yq3,
            -1, 0, xq3,
            yq3, -xq3, 0;

      b << b0(R, xt1, yt1, depth, yq1), b1(R, xt1, yt1, depth, xq1), b2(R, xt1, yt1, depth, xq1, yq1),
            b0(R, xt2, yt2, depth, yq2), b1(R, xt2, yt2, depth, xq2), b2(R, xt2, yt2, depth, xq2, yq2),
            b0(R, xt3, yt3, depth, yq3), b1(R, xt3, yt3, depth, xq3), b2(R, xt3, yt3, depth, xq3, yq3);
//      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
//      translation = MQR.solve(b);
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).rank() << std::endl;
   }

   //Called by RANSAC estimation
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation)
   //-------------------------------------------------------------------------
   {
      const cv::Point3d& tpt0 = train_img_pts[0];
      const cv::Point3d& tpt1 = train_img_pts[1];
      const cv::Point3d& tpt2 = train_img_pts[2];
      const cv::Point3d& qpt0 = query_img_pts[0];
      const cv::Point3d& qpt1 = query_img_pts[1];
      const cv::Point3d& qpt2 = query_img_pts[2];
      Eigen::Vector3d train_ray1 = Kinv*Eigen::Vector3d(tpt0.x, tpt0.y, 1);
      Eigen::Vector3d query_ray1 = Kinv*Eigen::Vector3d(qpt0.x, qpt0.y, 1);
      Eigen::Vector3d train_ray2 = Kinv*Eigen::Vector3d(tpt1.x, tpt1.y, 1);
      Eigen::Vector3d query_ray2 = Kinv*Eigen::Vector3d(qpt1.x, qpt1.y, 1);
      Eigen::Vector3d train_ray3 = Kinv*Eigen::Vector3d(tpt2.x, tpt2.y, 1);
      Eigen::Vector3d query_ray3 = Kinv*Eigen::Vector3d(qpt2.x, qpt2.y, 1);
      double xt1 = train_ray1[0], yt1 = train_ray1[1], xq1 = query_ray1[0], yq1 = query_ray1[1];
      double xt2 = train_ray2[0], yt2 = train_ray2[1], xq2 = query_ray2[0], yq2 = query_ray2[1];
      double xt3 = train_ray3[0], yt3 = train_ray3[1], xq3 = query_ray3[0], yq3 = query_ray3[1];
//   Eigen::MatrixXd A(9, 3);
//   Eigen::VectorXd b(9);
      Eigen::Matrix<double, 9, 3> A;
      Eigen::Matrix<double, 9, 1> b;
      A << 0, 1, -yq1,
            -1, 0, xq1,
            yq1, -xq1, 0,

            0, 1, -yq2,
            -1, 0, xq2,
            yq2, -xq2, 0,

            0, 1, -yq3,
            -1, 0, xq3,
            yq3, -xq3, 0;

      b << b0(R, xt1, yt1, depth, yq1), b1(R, xt1, yt1, depth, xq1), b2(R, xt1, yt1, depth, xq1, yq1),
            b0(R, xt2, yt2, depth, yq2), b1(R, xt2, yt2, depth, xq2), b2(R, xt2, yt2, depth, xq2, yq2),
            b0(R, xt3, yt3, depth, yq3), b1(R, xt3, yt3, depth, xq3), b2(R, xt3, yt3, depth, xq3, yq3);
//      Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 9, 3>> MQR(A);
//      translation = MQR.solve(b);
      translation = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//   std::cout << "Rank " << A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).rank() << std::endl;
   }
}


#pragma once

#define BOOST_MATH_PROMOTE_DOUBLE_POLICY false
#include "tools.hpp"
#include <Eigen/Dense>
#include <algorithm>
#include <boost/math/distributions.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <random>
#include <vector>

namespace kde1d {

//! statistical functions
namespace stats {

//! standard normal density
//! @param x evaluation points.
//! @return matrix of pdf values.
inline Eigen::MatrixXd
dnorm(const Eigen::MatrixXd& x)
{
  boost::math::normal dist;
  return x.unaryExpr(
    [&dist](const double& y) { return boost::math::pdf(dist, y); });
};

//! standard normal density
//! @param x evaluation points.
//! @param drv order of the derivative
//! @return matrix of pdf values.
inline Eigen::MatrixXd
dnorm_drv(const Eigen::MatrixXd& x, unsigned drv)
{
  boost::math::normal dist;
  double rt2 = std::sqrt(2);
  return x.unaryExpr([&dist, &drv, &rt2](const double& y) {
    double res = boost::math::pdf(dist, y);
    // boost implementes phsyicist's hermite poly; rescale to probabilist's.
    res *= boost::math::hermite(drv, y / rt2);
    res *= std::pow(0.5, drv * 0.5);
    if (drv % 2)
      res = -res;
    return res;
  });
};

//! standard normal cdf
//! @param x evaluation points.
//! @return matrix of cdf values.
inline Eigen::MatrixXd
pnorm(const Eigen::MatrixXd& x)
{
  boost::math::normal dist;
  return x.unaryExpr(
    [&dist](const double& y) { return boost::math::cdf(dist, y); });
};

//! standard normal quantiles
//! @param x evaluation points.
//! @return matrix of quantiles.
inline Eigen::MatrixXd
qnorm(const Eigen::MatrixXd& x)
{
  boost::math::normal dist;
  return x.unaryExpr(
    [&dist](const double& y) { return boost::math::quantile(dist, y); });
};

//! empirical quantiles
//! @param x data.
//! @param q evaluation points.
//! @return vector of quantiles.
inline Eigen::VectorXd
quantile(const Eigen::VectorXd& x, const Eigen::VectorXd& q)
{
  double n = static_cast<double>(x.size() - 1);
  size_t m = q.size();
  Eigen::VectorXd res(m);

  // map to std::vector and sort
  std::vector<double> x2(x.data(), x.data() + x.size());
  std::sort(x2.begin(), x2.end());

  // linear interpolation (quantile of type 7 in R)
  for (size_t i = 0; i < m; ++i) {
    size_t k = std::floor(n * q(i));
    double p = static_cast<double>(k) / n;
    res(i) = x2[k];
    if (k < n)
      res(i) += (x2[k + 1] - x2[k]) * (q(i) - p) * n;
  }
  return res;
}

//! empirical quantiles
//! @param x data.
//! @param q evaluation points.
//! @param w vector of weights.
//! @return vector of quantiles.
inline Eigen::VectorXd
quantile(const Eigen::VectorXd& x,
         const Eigen::VectorXd& q,
         const Eigen::VectorXd& w)
{
  if (w.size() == 0)
    return quantile(x, q);
  if (w.size() != x.size())
    throw std::runtime_error("x and w must have the same size");
  double n = static_cast<double>(x.size());
  size_t m = q.size();
  Eigen::VectorXd res(m);

  // map to std::vector and sort
  std::vector<size_t> ind(n);
  for (size_t i = 0; i < n; ++i)
    ind[i] = i;
  std::sort(
    ind.begin(), ind.end(), [&x](size_t i, size_t j) { return x(i) < x(j); });

  auto x2 = x;
  auto wcum = w;
  double wacc = 0.0;
  for (size_t i = 0; i < n; ++i) {
    x2(i) = x(ind[i]);
    wcum(i) = wacc;
    wacc += w(ind[i]);
  }

  double wsum = w.sum() - w(ind[n - 1]);
  ;
  for (size_t j = 0; j < m; ++j) {
    size_t i = 1;
    while ((wcum(i) < q(j) * wsum) & (i < n))
      i++;
    res(j) = x2(i - 1);
    if (w(ind[i - 1]) > 1e-30) {
      res(j) +=
        (x2(i) - x2(i - 1)) * (q(j) - wcum(i - 1) / wsum) / w(ind[i - 1]);
    }
  }

  return res;
}

// conditionally equidistant jittering; equivalent to the R implementation:
//   tab <- table(x)
//   noise <- unname(unlist(lapply(tab, function(l) -0.5 + 1:l / (l + 1))))
//   s <- sort(x, index.return = TRUE)
//   return((s$x + noise)[rank(x, ties.method = "first", na.last = "keep")])
inline Eigen::VectorXd
equi_jitter(const Eigen::VectorXd& x)
{
  size_t n = x.size();

  // first compute the corresponding permutation that sorts x (required later)
  auto perm = tools::get_order(x);
  // actually sort x
  Eigen::VectorXd srt(n);
  for (size_t i = 0; i < n; ++i)
    srt(i) = x(perm(i));

  // compute contingency table
  Eigen::MatrixXd tab(n, 2);
  size_t lev = 0;
  size_t cnt = 1;
  for (size_t k = 1; k < n; ++k) {
    if (srt(k - 1) != srt(k)) {
      tab(lev, 0) = srt(k - 1);
      tab(lev++, 1) = cnt;
      cnt = 1;
    } else {
      cnt++;
      if (k == n - 1) {
        tab(lev, 0) = srt(k);
        tab(lev++, 1) = cnt;
      }
    }
  }
  tab.conservativeResize(lev, 2);

  // add deterministic, conditionally uniorm noise
  Eigen::VectorXd noise = Eigen::VectorXd::Zero(n);
  size_t i = 0;
  for (long k = 0; k < tab.rows(); ++k) {
    for (size_t cnt = 1; cnt <= tab(k, 1); ++cnt)
      noise(i++) = -0.5 + cnt / (tab(k, 1) + 1.0);
    cnt = 1;
  }
  Eigen::VectorXd jtr = srt + noise;

  // invert the permutation to return jittered x in original order
  for (long i = 0; i < perm.size(); ++i)
    srt(perm(i)) = jtr(i);

  return srt;
}

//! @brief simulates from the standard uniform distribution.
//!
//! @param n number of observations.
//! @param seeds seeds of the random number generator; if empty (default),
//!   the random number generator is seeded randomly.
//!
//! @return An size n vector of independent \f$ \mathrm{U}[0, 1] \f$ random
//!   variables.
inline Eigen::VectorXd
simulate_uniform(size_t n, std::vector<int> seeds)
{
  if (n < 1)
    throw std::runtime_error("n  must be at least 1.");

  if (seeds.size() == 0) { // no seeds provided, seed randomly
    std::random_device rd{};
    seeds = std::vector<int>(5);
    for (auto& s : seeds)
      s = static_cast<int>(rd());
  }

  // initialize random engine and uniform distribution
  std::seed_seq seq(seeds.begin(), seeds.end());
  std::mt19937 generator(seq);
  std::uniform_real_distribution<double> distribution(0.0, 1.0);

  Eigen::VectorXd U(n);
  return U.unaryExpr([&](double) { return distribution(generator); });
}

} // end kde1d::stats

} // end kde1d


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 optimize toolbox - quadl"

#include <iostream>
#include <nt2/include/functions/quadl.hpp>
#include <nt2/toolbox/optimization/output.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/expm1.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/table.hpp>
struct f
{
  template < class X > inline
  X operator()(const X & x ) const
  {
    return x;
  }
};

NT2_TEST_CASE_TPL( quadl_functor, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5));
  NT2_DISPLAY(x);
  BOOST_AUTO_TPL(res, quadl<T>(f(), x));
   std::cout << "Integrals:" << res.integrals << ") = " << res.errors
             << " after " << res.eval_count <<  " evaluations\n";

   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::sqr(x)*nt2::Half<T>())), nt2::Sqrteps<T>());

}

NT2_TEST_CASE_TPL( quadl_tag, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, quadl<T>(nt2::functor<nt2::tag::exp_>(), x));
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::expm1(x))), nt2::Sqrteps<T>());

}
NT2_TEST_CASE_TPL( quadl_tag_reverse, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(5), T(-1), T(0));
  NT2_DISPLAY(x);
  //output<tab_t,T>
  BOOST_AUTO_TPL(res, quadl<T>(nt2::functor<nt2::tag::exp_>(), x));
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, nt2::exp(x)-nt2::exp(T(5)))), nt2::Sqrteps<T>());

}

NT2_TEST_CASE_TPL( quadl_2, NT2_REAL_TYPES )
{
  using nt2::quadl;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  tab_t x = nt2::_(T(0), T(5), T(5));
  NT2_DISPLAY(x);

  BOOST_AUTO_TPL(res, quadl<T>(nt2::functor<nt2::tag::exp_>(), T(0), T(5)));
  std::cout << "Integrals: " << res.integrals << " with " << res.errors
            << " after " << res.eval_count <<  " evaluations\n";

  NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::dist(res.integrals, expm1(x))), nt2::Sqrteps<T>());

}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file TransformationWithCovariance.hpp
/// \brief Header file for a transformation matrix class with associated covariance.
/// \details Light weight transformation class with added covariance propagation, intended to
///          be fast, and not to provide unnecessary functionality, but still much slower than
///          the base Transformation class due to extra matrix multiplications associated with
///          covariance propagation.  Only use this class if you need covariance.
///
/// \author Kai van Es
//////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////
///
/// A note on EIGEN_MAKE_ALIGNED_OPERATOR_NEW (Sean Anderson, as of May 23, 2013)
/// (also see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
///
/// Fortunately, Eigen::Matrix3d and Eigen::Vector3d are NOT 16-byte vectorizable,
/// therefore this class should not require alignment, and can be used normally in STL.
///
/// To inform others of the issue, classes that include *fixed-size vectorizable Eigen types*,
/// see http://eigen.tuxfamily.org/dox-devel/group__TopicFixedSizeVectorizable.html,
/// must include the above macro! Furthermore, special considerations must be taken if
/// you want to use them in STL containers, such as std::vector or std::map.
/// The macro overloads the dynamic "new" operator so that it generates
/// 16-byte-aligned pointers, this MUST be in the public section of the header!
///
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGMATH_TRANSFORMATIONWITHCOVARIANCE_HPP
#define LGMATH_TRANSFORMATIONWITHCOVARIANCE_HPP

#include <lgmath/se3/Transformation.hpp>
#include <Eigen/Core>

namespace lgmath {
namespace se3 {

class TransformationWithCovariance: public Transformation
{
 public:
  // Eigen::Matrix<double,6,6> is 16-byte vectorizable
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(bool initCovarianceToZero = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const TransformationWithCovariance&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(TransformationWithCovariance&& T) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor from basic Transformation
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Transformation& T, bool initCovarianceToZero = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move constructor from basic Transformation
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(Transformation&& T, bool initCovarianceToZero = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor from basic Transformation, with covariance
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Transformation& T,
                               const Eigen::Matrix<double,6,6>& covariance);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::Matrix4d& T);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor with covariance
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::Matrix4d& T,
                               const Eigen::Matrix<double,6,6>& covariance);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = [C_ba, -C_ba*r_ba_ina; 0 0 0 1]
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::Matrix3d& C_ba, const Eigen::Vector3d& r_ba_ina);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor with covariance. The transformation will be
  /// T_ba = [C_ba, -C_ba*r_ba_ina; 0 0 0 1]
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::Matrix3d& C_ba, const Eigen::Vector3d& r_ba_ina,
                               const Eigen::Matrix<double,6,6>& covariance);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab)
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::Matrix<double,6,1>& xi_ab,
                               unsigned int numTerms = 0);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor with covariance. The transformation will be T_ba = vec2tran(xi_ab)
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::Matrix<double,6,1>& xi_ab,
                               const Eigen::Matrix<double,6,6>& covariance,
                               unsigned int numTerms = 0);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab), xi_ab must be 6x1
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::VectorXd& xi_ab);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab), xi_ab must be 6x1
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance(const Eigen::VectorXd& xi_ab,
                               const Eigen::Matrix<double,6,6>& covariance);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Destructor. Default implementation.
  //////////////////////////////////////////////////////////////////////////////////////////////
  ~TransformationWithCovariance() = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy assignment operator.
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance& operator=(const TransformationWithCovariance&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move assignment operator.
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance& operator=(TransformationWithCovariance&& T) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy assignment operator from basic Transform.
  /// \details This assignment resets the covariance to the uninitialized state.  You must
  ///          manually call setZeroCovariance() or setCovariance(const Eigen::Matrix6d&)
  ///          before querying it with the public method cov(), or an exception will be
  ///          thrown.
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual TransformationWithCovariance& operator=(const Transformation& T);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move assignment operator from basic Transform.
  /// \details This assignment resets the covariance to the uninitialized state.  You must
  ///          manually call setZeroCovariance() or setCovariance(const Eigen::Matrix6d&)
  ///          before querying it with the public method cov(), or an exception will be
  ///          thrown.
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual TransformationWithCovariance& operator=(Transformation&& T);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the underlying covariance matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  const Eigen::Matrix<double,6,6>& cov() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns whether or not a covariance has been set. If it is unset, then querying it
  ///        with the public method cov() will throw an exception.
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool covarianceSet() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Sets the underlying rotation matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  void setCovariance(const Eigen::Matrix<double,6,6>& covariance);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Sets the underlying rotation matrix to the 6x6 zero matrix (perfect certainty)
  //////////////////////////////////////////////////////////////////////////////////////////////
  void setZeroCovariance();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the inverse matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance inverse() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply T_rhs.
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance& operator*=(const TransformationWithCovariance& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply basic (certain) T_rhs
  ///
  /// Note: Assumes that the Transformation matrix has perfect certainty
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance& operator*=(const Transformation& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply this matrix by the inverse of T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance& operator/=(const TransformationWithCovariance& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply this matrix by
  ///        the inverse of a basic (certain) T_rhs
  ///
  /// Note: Assumes that the Transformation matrix has perfect certainty
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformationWithCovariance& operator/=(const Transformation& T_rhs);

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Covariance
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double,6,6> covariance_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Covariance flag.  Set to true when the covariance is manually set or initialized.
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool covarianceSet_;
};

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Multiplication of TransformWithCovariance by TransformWithCovariance
//////////////////////////////////////////////////////////////////////////////////////////////
TransformationWithCovariance operator*(TransformationWithCovariance T_lhs,
                                       const TransformationWithCovariance& T_rhs);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Multiplication of TransformWithCovariance by Transform
///
/// Note: Assumes that the Transformation matrix has perfect certainty
//////////////////////////////////////////////////////////////////////////////////////////////
TransformationWithCovariance operator*(TransformationWithCovariance T_lhs,
                                       const Transformation& T_rhs);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Multiplication of Transform by TransformWithCovariance
///
/// Note: Assumes that the Transformation matrix has perfect certainty
//////////////////////////////////////////////////////////////////////////////////////////////
TransformationWithCovariance operator*(const Transformation& T_lhs,
                                       const TransformationWithCovariance& T_rhs);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Multiplication of TransformWithCovariance by inverse TransformWithCovariance
//////////////////////////////////////////////////////////////////////////////////////////////
TransformationWithCovariance operator/(TransformationWithCovariance T_lhs,
                                       const TransformationWithCovariance& T_rhs);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Multiplication of TransformWithCovariance by inverse Transform
///
/// Note: Assumes that the Transformation matrix has perfect certainty
//////////////////////////////////////////////////////////////////////////////////////////////
TransformationWithCovariance operator/(TransformationWithCovariance T_lhs,
                                       const Transformation& T_rhs);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Multiplication of Transform by inverse TransformWithCovariance
///
/// Note: Assumes that the Transformation matrix has perfect certainty
//////////////////////////////////////////////////////////////////////////////////////////////
TransformationWithCovariance operator/(const Transformation& T_lhs,
                                       const TransformationWithCovariance& T_rhs);

} // se3
} // lgmath

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief print transformation
//////////////////////////////////////////////////////////////////////////////////////////////
std::ostream& operator<<(std::ostream& out, const lgmath::se3::TransformationWithCovariance& T);

#endif //LGMATH_TRANSFORMATIONWITHCOVARIANCE_HPP