/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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
//==============================================================================

*/

#ifndef __OPTIMISATION2_ALGO_LM_MANY_CLASSES_LEVMAR_HPP__
#define __OPTIMISATION2_ALGO_LM_MANY_CLASSES_LEVMAR_HPP__

#include <boost/fusion/include/for_each.hpp>
#include <boost/mpl/copy_if.hpp>
#include <libv/lma/lm/ba/bas.hpp>
#include <libv/lma/lm/ba/initialize.hpp>
#include <libv/lma/lm/ba/fill_hessien.hpp>
#include <libv/lma/lm/bundle/cost_and_save.hpp>
#include <libv/lma/lm/container/map_container.hpp>
#include <libv/lma/lm/container/container.hpp>
#include <libv/lma/lm/function/function.hpp>
#include <libv/lma/lm/trait/use_estimator.hpp>
#include <libv/lma/time/tictoc.hpp>

namespace lma
{
  template<class F> struct MakeResidualVector : bf::pair<F,AlignVector<std::pair<typename Function<F>::ErreurType,bool>>> {};

  template<> struct MakeResidualVector<mpl::_1>{};

  template<class A, class A_, class B, class B_, class C, class C_>
  void prod_v_v_h(VectorColumn<A,A_>& a, const VectorColumn<B,B_>& b, const DiagonalTable<C,C_>& c)
  {
    for(auto i = c.first() ; i < c.size() ; ++i)
      a(i) += b(i) * c(i);
  }

  template<class A, class A_, class B, class B_, class _>
  void prod_v_v_h(VectorColumn<B,B_>& r, const VectorColumn<A,A_>& delta, const Table<A,B,_>& h)
  {
    for(auto i = h.first() ; i < h.size() ; ++i)
      for(auto j = h.first(i) ; j < h.size(i) ; ++j)
        r(h.indice(i,j)) += delta(i) * h(i,j);
  }

  template<class A, class A_, class B, class B_, class _>
  void prod_v_v_h_t(VectorColumn<A,A_>& r, const VectorColumn<B,B_>& delta, const Table<A,B,_>& h)
  {
    for(auto i = h.first() ; i < h.size() ; ++i)
      for(auto j = h.first(i) ; j < h.size(i) ; ++j)
        r(i) += delta(h.indice(i,j)) * transpose(h(i,j));
  }

  template<class A, class B, class C, class C_>
  void prod_v_v_h_dispatch(A& r, const B& delta, const DiagonalTable<C,C_>& c)
  {
    prod_v_v_h(bf::at_key<C>(r),bf::at_key<C>(delta),c);
  }

  template<class A, class B, class C1, class C2, class _>
  void prod_v_v_h_dispatch(A& r, const B& delta, const Table<C1,C2,_>& c)
  {
    prod_v_v_h(bf::at_key<C2>(r),bf::at_key<C1>(delta),c);
    prod_v_v_h_t(bf::at_key<C1>(r),bf::at_key<C2>(delta),c);
  }

  struct GetMax
  {
    double& lambda;
    GetMax(double& d):lambda(d){}
    template<class Table> void operator()(const Table& table){ lambda = std::max(lambda,table.max_lambda());}
  };

  struct ComputeScale
  {
    double &scale;
    double λ
    ComputeScale(double& scale_, double& lambda_):scale(scale_),lambda(lambda_){}
    template<class Delta, class Jte> void operator() (const Delta& delta, const Jte& jte)
    {
      for(auto i = delta.first() ; i < delta.size() ; ++i)
        for(size_t k = 0 ; k < delta.I; ++k)
          scale += (delta(i)[k] * (lambda * delta(i)[k] + jte(i)[k]));
    };
  };

  template<class Policy> struct LevMar : Policy
  {
    typedef LevMar<Policy> type;

    typedef utils::Tic<false> Tic;
    typedef typename Policy::Bundle Bundle;
    typedef typename Policy::Ba Ba;
    typedef typename Ba::Keys Keys;

    Ba ba_;
    const Ba& ba() const { return ba_; }

    double residual_evaluations;
    double jacobian_evaluations;
    double norm_eq_;
    double preprocess;

    double prev_lambda;
    double erreur_,previous_erreur_;
    double nb_used_fonctor;

    typedef MultiContainer<typename Bundle::ListFunction,MakeResidualVector<mpl::_1>> ContainerMapErreur;

    utils::Tic<true> free_tic;

    ContainerMapErreur map_erreur;

    static std::string name(){ return ttt::name<LevMar<Policy>>(); }

    template<class Config>
    LevMar(Config config):Policy(config),prev_lambda(-1.0),erreur_(-1.0),previous_erreur_(-1.0)
    {
      residual_evaluations = 0;
      preprocess = norm_eq_ = jacobian_evaluations = 0;
    }

    typedef typename
    mpl::copy_if<
                  typename Bundle::ListFunction,
                  detail::IsMEstimator<mpl::_1>
                >::type MEstimatorList;

    typedef typename
      br::as_map<
                  typename mpl::transform<
                                          MEstimatorList,
                                          bf::pair<mpl::_1,double>
                                         >::type
                >::type Meds;
    Meds meds;//bf::tuple< pair<F1,double>, pair<F2,double> ...>

    void init(Bundle& bundle_)
    {
      free_tic.tic();
      bundle_.update();
      initialize(bundle_,ba_);
      Policy::init(bundle_,ba_);
      cost_and_save_mad<Meds>(bundle_,meds);
      preprocess = free_tic.toc();
    }

    void restore_erreur()
    {
      assert(erreur_!=-1.0);//! l'erreur a déjà été calculé au moins une fois
      erreur_ = previous_erreur_;
    }

    double get_erreur() const
    {
      assert(erreur_!=-1.0);
      return erreur_;
    }

    std::pair<double,int> compute_erreur(const Bundle& bundle_)
    {
      free_tic.tic();
      if (erreur_!=-1.0)
        previous_erreur_ = erreur_;
      std::tie(erreur_,nb_used_fonctor) =  cost_and_save(bundle_,map_erreur,meds);
      if (erreur_==-1)
	     std::cerr << " LMA::compute_erreur " << erreur_ << " " << previous_erreur_ << std::endl;
      assert(erreur_!=-1.0);
      residual_evaluations += free_tic.toc();
      return {erreur_,nb_used_fonctor};
    }

    double compute_scale(double lambda) const
    {
      double scale = 0.0;
      ComputeScale cs(scale,lambda);
      for_each<MetaBinary<typename Ba::Keys>>(std::tie(ba_.delta,ba_.jte),cs);
      return scale;
    }

    double init_lambda()
    {
      double lambda = 0;
      GetMax gm(lambda);
//       for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),[&lambda](auto& table){ lambda = std::max(lambda,table.max_lambda());});
      for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),gm);
      return lambda * 1e-5;
    }

//     double prediction(const Bundle& bundle_)
//     {
//       double pred=0;
//       double alpha=0;
//       typename Ba::Vectors v = ba_.jte;
//
//       bf::for_each(v,[](auto& pair){ pair.second.set_zero(); });
//       bf::for_each(ba_.h,[&](auto& pair){prod_v_v_h_dispatch(v,ba_.delta,pair.second);});
//       for_each<MetaBinary<typename Ba::Keys>>(std::tie(v,ba_.delta),[&alpha](auto& a, auto& b){
//         for(auto i = a.first() ; i < a.size() ; ++i)
//           alpha += a(i)*b(i);
//       });
//       alpha /= 2.0;
//       std::cout << " alpha : " << alpha << std::endl;
// //       abort();
//       bf::for_each(ba_.jacob,
//         [&](auto& pair)
//         {
//           typedef typename std::decay<decltype(pair)>::type Pair;
//           typedef typename Pair::first_type Obs;
//           auto& jacobs = pair.second;
//           TooN::Vector<2,double> vec = TooN::Ones;
//           vec *= alpha;
//
//           for(size_t iobs = 0 ; iobs < jacobs.size() ; ++iobs)
//           {
//             bf::for_each(jacobs.at(iobs),
//               [&]
//               (auto& pair_block_jacob)
//               {
//                 typedef typename std::decay<decltype(pair_block_jacob)>::type PairBlockJacob;
//                 typedef typename PairBlockJacob::first_type ParamType;
//                 auto& block = pair_block_jacob.second;
//                 auto& container_delta = bf::at_key<ParamType>(ba_.delta);
//                 typedef typename std::decay<decltype(container_delta)>::type::MatrixTag MatrixTag;
//                 const auto& map_indice = bundle_.spi2.indices(ttt::Indice<Obs>(iobs));// vector<Indice,...>
//                 auto& indice = *bf::find<ttt::Indice<ParamType>>(map_indice);
//
//                 pred +=
//                         squared_norm(make_view(map_erreur.template at_key<Obs>().at(iobs).first,ttt::wrap<MatrixTag>())
//                         +
//                         block * container_delta(indice)
//                         + vec);
//               });
//           }
//         });
//       // now I'm damned
//       return pred;
//     }

//     void compute(Bundle& bundle_, double& lambda, bool recalcul, double& pred)
//     {
//       set_zero_(ba_);
//       Policy::init_zero();
//
//       if (recalcul)
//       {
//         bf::for_each(ba_.h,detail::SetZero());
//         bf::for_each(ba_.jte,detail::SetZero());
//         // compute H = JtJ & Jte
//         detail::fill_hessien_residu33<Derivator<typename Policy::MatrixTag>>(bundle_,ba_,map_erreur);
//         Policy::save_h(ba_.h);
//       }
//       else
//       {
//         Policy::reload_h(ba_.h,-prev_lambda);
//       }
//
//       prev_lambda = lambda;
//       for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),LambdaDiag(lambda));
//       Policy::solve(ba_,bundle_);
//
//       pred = prediction(bundle_);
//
//       for_each<MetaBinary<typename Ba::Keys>>(std::tie(bundle_.opt_container.map(),ba_.delta),Correct());
//     }

    void compute(Bundle& bundle_, double& lambda, bool recalcul)
    {
      Tic tic("compute all");
      assert(bundle_.nb_obs()!=0);
      try
      {
        set_zero_(ba_);
        Policy::init_zero();

        free_tic.tic();
        Tic tic_h("compute H");
        if (recalcul)
        {
          bf::for_each(ba_.h,detail::SetZero());
          bf::for_each(ba_.jte,detail::SetZero());
          // compute H = JtJ & Jte
          detail::fill_hessien<typename Policy::MatrixTag>(bundle_,ba_,map_erreur,meds);
          Policy::save_h(ba_.h);
        }
        else
        {
          Policy::reload_h(ba_.h,-prev_lambda);
        }
        tic_h.disp();
        jacobian_evaluations += free_tic.toc();

        free_tic.tic();

        // H += Eye(H)*lambda
        if (lambda==-1)
        {
          lambda = init_lambda();
        }

        prev_lambda = lambda;
        for_each<MetaUnary<typename Ba::ListeDiag>>(std::tie(ba_.h),LambdaDiag(lambda));
        Tic tic_schur("SolveDelta");

//         clement(ba_.h);
//         std::cout << ba_.h << std::endl;
//           auto a = to_mat<typename Ba::Keys,typename Ba::ListeHessien>(ba_.h,size_tuple<mpl::size<typename Ba::Keys>::value>(ba_.delta));

//         auto u = Blocker<2,2>::view(a,0,0);
//         auto v = Blocker<2,2>::view(a,2,2);
//         auto w = Blocker<2,2>::view(a,0,2);
//
//         std::cout << " A :\n" << a << std::endl << std::endl << std::endl;
//         std::cout << " V :\n" << v << std::endl;
//         std::cout << " W :\n" << w << std::endl;
//
//         auto b = to_matv(ba_.jte());
//         auto ea = Blocker<2,1>::view(b,0,0);
//         auto eb = Blocker<2,1>::view(b,2,0);
//         std::cout << " jte : " << b.transpose() << std::endl;
//         std::cout << " ea  : " << ea.transpose() << std::endl;
//         std::cout << " eb  : " << eb.transpose() << std::endl;
//
//         auto y = w*v.inverse();
//         auto s = u - y * w.transpose();
//         auto e = ea - y * eb;
//
//         auto da = (s.template selfadjointView<Eigen::Upper>().llt().solve(e)).eval();
//         auto db = v.inverse()*(eb - w.transpose() * da);
//
//         std::cout << " S : \n" << s << std::endl;
//         std::cout << std::endl;
//
//         std::cout << da.transpose() << " " << db.transpose() << std::endl;

        Policy::solve(ba_,bundle_);

//         std::cout << to_matv(ba_.delta()).transpose() << std::endl;
//         std::cout << std::endl;
        tic_schur.disp();

        for_each<MetaBinary<typename Ba::Keys>>(std::tie(bundle_.opt_container.map(),ba_.delta),Correct());

        norm_eq_ += free_tic.toc();
      }
      catch(NAN_ERROR& e)
      {
        std::cout << " Optimization failure : " <<  e.what() << std::endl;
      }
      tic.disp();
    }
  };
}

namespace ttt
{
  template<class A> struct Name< lma::LevMar<A> > { static std::string name(){ return "LevMar<" + ttt::name<A>() + ">";} };
}

#endif


#ifndef G2O_TARGET_TYPES_3D_HPP_
#define G2O_TARGET_TYPES_3D_HPP_

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>

#include <Eigen/Core>

// This header file specifies a set of types for the different
// tracking examples; note that

class VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexPosition3D() {}

  virtual void setToOriginImpl() { _estimate.setZero(); }

  virtual void oplusImpl(const double* update) {
    _estimate[0] += update[0];
    _estimate[1] += update[1];
    _estimate[2] += update[2];
  }

  virtual bool read(std::istream& /*is*/) { return false; }

  virtual bool write(std::ostream& /*os*/) const { return false; }
};

// Store velocity separately from position?
class VertexVelocity3D : public g2o::BaseVertex<3, Eigen::Vector3d> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexVelocity3D() {}

  virtual void setToOriginImpl() { _estimate.setZero(); }

  virtual void oplusImpl(const double* update) {
    _estimate[0] += update[0];
    _estimate[1] += update[1];
    _estimate[2] += update[2];
  }

  virtual bool read(std::istream& /*is*/) { return false; }

  virtual bool write(std::ostream& /*os*/) const { return false; }
};

// The idealised GPS measurement; this is 3D and linear
class GPSObservationPosition3DEdge
    : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPosition3D> {
 public:
  GPSObservationPosition3DEdge() {}

  void computeError() {
    const VertexPosition3D* v =
        static_cast<const VertexPosition3D*>(_vertices[0]);
    _error = v->estimate() - _measurement;
  }

  virtual bool read(std::istream& /*is*/) { return false; }

  virtual bool write(std::ostream& /*os*/) const { return false; }
};

#endif  //  __TARGET_TYPES_3D_HPP__


#pragma once

#include <iostream>
#include <experimental/filesystem>
#include <fstream>
#include <random>

#define EIGEN_DONT_PARALLELIZE 1

#include <Eigen/Dense>

// Function: read_mnist_label
inline auto read_mnist_label(const std::experimental::filesystem::path& path) {

  // Helper lambda.
  auto reverse_int = [](int i) {
    unsigned char c1, c2, c3, c4;
    c1 = i         & 255;
    c2 = (i >> 8)  & 255;
    c3 = (i >> 16) & 255;
    c4 = (i >> 24) & 255;
    return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
  };

  // Read the image.
  std::ifstream ifs(path, std::ios::binary);

  if(!ifs) {
    assert(false);
  }

  int magic_number = 0;
  int num_imgs = 0;

  ifs.read((char*)&magic_number, sizeof(magic_number));
  magic_number = reverse_int(magic_number);

  ifs.read((char*)&num_imgs, sizeof(num_imgs));
  num_imgs = reverse_int(num_imgs);

  Eigen::VectorXi labels(num_imgs);
  for (int i = 0; i<num_imgs; ++i) {
    unsigned char temp = 0;  // must use unsigned
    ifs.read((char*)&temp, sizeof(temp));
    labels[i] = static_cast<int>(temp);
  }
  return labels;
}

inline auto read_mnist_image(const std::experimental::filesystem::path& path) {

  // Helper lambda.
  auto reverse_int = [] (int i) {
    unsigned char c1, c2, c3, c4;
    c1 = i         & 255;
    c2 = (i >> 8)  & 255;
    c3 = (i >> 16) & 255;
    c4 = (i >> 24) & 255;
    return ((int)c1 << 24) + ((int)c2 << 16) + ((int)c3 << 8) + c4;
  };

  // Read the image.
  std::ifstream ifs(path, std::ios::binary);

  if(!ifs) {
    assert(false);
  }

  int magic_number = 0;
  int num_imgs = 0;
  int num_rows = 0;
  int num_cols = 0;

  ifs.read((char*)&magic_number, sizeof(magic_number));
  magic_number = reverse_int(magic_number);

  ifs.read((char*)&num_imgs, sizeof(num_imgs));
  num_imgs = reverse_int(num_imgs);

  ifs.read((char*)&num_rows, sizeof(num_rows));
  num_rows = reverse_int(num_rows);

  ifs.read((char*)&num_cols, sizeof(num_cols));
  num_cols = reverse_int(num_cols);

  Eigen::MatrixXf images(num_imgs, num_rows*num_cols);

  for(int i = 0; i < num_imgs; ++i) {
    for(int r = 0; r < num_rows; ++r) {
      for(int c = 0; c < num_cols; ++c) {
        unsigned char p = 0;  // must use unsigned
        ifs.read((char*)&p, sizeof(p));
        images(i, r*num_cols + c) = static_cast<float>(p);
      }
    }
  }

  for(int i=0; i<images.rows(); i++) {
    for(int j=0; j<images.cols(); j++) {
      images(i, j) /= 255.0;
    }
  }
  return images;
}

inline auto time_diff(
  std::chrono::time_point<std::chrono::high_resolution_clock> &t1,
  std::chrono::time_point<std::chrono::high_resolution_clock> &t2
) {
  return std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count();
}

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

enum class Activation {
  NONE,
  RELU,
  SIGMOID
};

// Procedure: sigmoid
inline void sigmoid(Eigen::MatrixXf& x) {
  x = ((1.0f + (-x).array().exp()).inverse()).matrix();
}

// Procedure: relu
inline void relu(Eigen::MatrixXf& x) {
  for(int j=0; j<x.cols(); ++j) {
    for(int i=0; i<x.rows(); ++i) {
      if(x(i, j) <= 0.0f) {
        x(i, j) = 0.0f;
      }
    }
  }
}

inline void activate(Eigen::MatrixXf& mat, Activation act) {
  switch(act) {
    case Activation::NONE:  return;
    case Activation::SIGMOID: sigmoid(mat); return;
    case Activation::RELU: relu(mat); return;
  };
}

// Function: drelu
inline void drelu(Eigen::MatrixXf& x) {
  for(int j=0; j<x.cols(); ++j) {
    for(int i=0; i<x.rows(); ++i) {
      x(i, j) = x(i, j) > 0.0f ? 1.0f : 0.0f;
    }
  }
}

// Function: dsigmoid
inline void dsigmoid(Eigen::MatrixXf& x) {
  x = x.array() * (1 - x.array());
}

inline void deactivate(Eigen::MatrixXf& mat, Activation act) {
  switch(act) {
    case Activation::NONE:    mat = Eigen::MatrixXf::Ones(mat.rows(), mat.cols()); return;
    case Activation::SIGMOID: dsigmoid(mat); return ;
    case Activation::RELU:    drelu(mat); return;
  };
}

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

struct MNIST_DNN {
  // Ctor
  MNIST_DNN() = default;

  void add_layer(size_t in_degree, size_t out_degree, Activation act) {
    acts.emplace_back(act);
    Ys.emplace_back().resize(batch_size, out_degree);
    Ws.push_back(Eigen::MatrixXf::Random(in_degree, out_degree));
    Bs.push_back(Eigen::MatrixXf::Random(1, out_degree));

    dW.emplace_back().resize(in_degree, out_degree);
    dB.emplace_back().resize(1, out_degree);
  }

  void forward(size_t layer, const Eigen::MatrixXf& mat) {
    Ys[layer] = mat * Ws[layer] + Bs[layer].replicate(mat.rows(), 1);
    activate(Ys[layer], acts[layer]);
  }

  void loss(const Eigen::VectorXi& labels) {
    delta = Ys.back();
    delta = (delta - delta.rowwise().maxCoeff().replicate(1, delta.cols())).array().exp().matrix();
    delta = delta.cwiseQuotient(delta.rowwise().sum().replicate(1, delta.cols()));
    for(size_t i=beg_row, j=0; j<batch_size; i++, j++) {
      delta(j, labels[i]) -= 1.0;
    }
  }

  void backward(size_t layer, const Eigen::MatrixXf& Xin) {
    deactivate(Ys[layer], acts[layer]);
    delta = delta.cwiseProduct(Ys[layer]);
    dB[layer] = delta.colwise().sum();
    dW[layer] = Xin.transpose() * delta;

    if(layer > 0) {
      delta = delta * Ws[layer].transpose();
    }
  }

  void update(size_t layer) {
    Ws[layer] -= lrate*(dW[layer] + decay*Ws[layer]);
    Bs[layer] -= lrate*(dB[layer] + decay*Bs[layer]);
  }

  // Testing images # = 10000 x 784 (28 x 28)
  void validate(Eigen::MatrixXf& test_images, Eigen::VectorXi& test_labels) {
    Eigen::MatrixXf res = test_images;
    //auto t1 = std::chrono::high_resolution_clock::now();
    for(size_t i=0; i<acts.size(); i++) {
      res = res * Ws[i] + Bs[i].replicate(res.rows(), 1);
      if(acts[i] == Activation::RELU) {
        relu(res);
      }
      else if(acts[i] == Activation::SIGMOID) {
        sigmoid(res);
      }
    }
    //auto t2 = std::chrono::high_resolution_clock::now();
    //std::cout << "Infer runtime: " << time_diff(t1, t2) << " ms\n";

    size_t correct_num {0};
    for(int k=0; k<res.rows(); k++) {
      int pred ;
      res.row(k).maxCoeff(&pred);
      if(pred == test_labels[k]) {
        correct_num ++;
      }
    }
    //std::cout << "Accuracy: " << correct_num << '/' << res.rows() << '\n';
  }

  // Parameter functions ------------------------------------------------------
  auto& epoch_num(unsigned e) {
    epoch = e;
    return *this;
  }
  auto& batch(size_t b) {
    batch_size = b;
    //assert(images->rows()%batch_size == 0);
    return *this;
  }
  auto& learning_rate(float l) {
    lrate = l;
    return *this;
  }

  auto num_layers() const {
    return acts.size();
  }

  std::vector<Eigen::MatrixXf> Ys;
  std::vector<Eigen::MatrixXf> Ws;
  std::vector<Eigen::MatrixXf> Bs;
  std::vector<Eigen::MatrixXf> dW;
  std::vector<Eigen::MatrixXf> dB;

  std::vector<Activation> acts;

  // Training images # = 60000 x 784 (28 x 28)
  //Eigen::MatrixXf* images;
  //Eigen::VectorXi* labels;
  Eigen::MatrixXf delta;

  int beg_row {0};

  float lrate {0.01f};
  float decay {0.01f};

  unsigned epoch {0};
  size_t batch_size {1};
};

inline Eigen::MatrixXf IMAGES;
inline Eigen::VectorXi LABELS;
inline Eigen::MatrixXf TEST_IMAGES;
inline Eigen::VectorXi TEST_LABELS;

// A workable DNN setting:
//   BATCH = 100 (or 50~100)
//   NUM_ITERATIONS = 60000/BATCH
//   dnn.add_layer(784, 100, Activation::RELU);
//   dnn.add_layer(100, 30, Activation::RELU);
//   dnn.add_layer(30, 10, Activation::NONE);

// The total number of training data = 60000
inline constexpr size_t BATCH {60};
inline constexpr size_t NUM_ITERATIONS {60000/BATCH};
inline constexpr size_t NUM_DNNS {10};

inline void init_dnn(MNIST_DNN& dnn, float rate) {
  //dnn.batch(BATCH).learning_rate(0.001);
  //dnn.add_layer(784, 100, Activation::RELU);
  //dnn.add_layer(100, 30, Activation::RELU);
  //dnn.add_layer(30, 10, Activation::NONE);

  dnn.batch(BATCH).learning_rate(rate);
  dnn.add_layer(784, 64, Activation::RELU);
  dnn.add_layer(64, 32, Activation::RELU);
  dnn.add_layer(32, 16, Activation::RELU);
  dnn.add_layer(16, 8, Activation::RELU);
  dnn.add_layer(8, 10, Activation::NONE);
}

inline void forward_task(MNIST_DNN& D,
  Eigen::MatrixXf& mat,
  Eigen::VectorXi& vec) {

  for(size_t i=0; i<D.acts.size(); i++) {
    if(i == 0){
      D.forward(i, mat.middleRows(D.beg_row, D.batch_size));
    }
    else {
      D.forward(i, D.Ys[i-1]);
    }
  }

  D.loss(vec);
}

inline void backward_task(MNIST_DNN& D, size_t i, Eigen::MatrixXf& mat) {
  if(i > 0) {
    //D.backward(i, D.Ys[i-1].transpose());
    D.backward(i, D.Ys[i-1]);
  }
  else {
    //D.backward(i, mat.middleRows(D.beg_row, D.batch_size).transpose());
    D.backward(i, mat.middleRows(D.beg_row, D.batch_size));

    D.beg_row += D.batch_size;
    if(D.beg_row >= IMAGES.rows()) {
      D.beg_row = 0;
    }
  }
}

inline void shuffle(Eigen::MatrixXf& mat, Eigen::VectorXi& vec) {

  static thread_local std::mt19937 gen(0);

  Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> p(mat.rows());
  p.setIdentity();
  std::shuffle(p.indices().data(), p.indices().data() + p.indices().size(), gen);

  mat = p * mat;
  vec = p * vec;
}

inline void report_runtime(std::chrono::time_point<std::chrono::high_resolution_clock>& t1) {
  auto t2 = std::chrono::high_resolution_clock::now();
  std::cout << "One iteration runtime: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count() << " ms\n";
  t1 = std::chrono::high_resolution_clock::now();
}

inline float rand_rate() {
  return static_cast<float>(rand())/static_cast<float>(RAND_MAX);
}

void run_tbb(const unsigned, const unsigned);
void run_taskflow(const unsigned, const unsigned);
void run_omp(const unsigned, const unsigned);
void run_sequential(unsigned, unsigned);


#include <iostream>
#include <random>

#include <Eigen/Dense>

#include <h5.hpp>

namespace h5
{
    // Adapt Eigen::Matrix as a two-dimensional buffer.
    template<typename T>
    struct buffer_traits<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
    {
        using buffer_type = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
        using value_type = T;

        static constexpr int rank = 2;

        static h5::shape<rank> shape(buffer_type const& buffer)
        {
            auto const rows = static_cast<std::size_t>(buffer.rows());
            auto const cols = static_cast<std::size_t>(buffer.cols());
            return {rows, cols};
        }

        static value_type const* data(buffer_type const& buffer)
        {
            return &buffer(0, 0);
        }

        static value_type* data(buffer_type& buffer)
        {
            return &buffer(0, 0);
        }

        static void reshape(buffer_type& buffer, h5::shape<rank> const& shape)
        {
            using index_type = typename buffer_type::Index;
            buffer.resize(
                static_cast<index_type>(shape.dims[0]),
                static_cast<index_type>(shape.dims[1])
            );
        }
    };
}

int main()
{
    // Create some random matrix.
    using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    Matrix matrix(1000, 1000);

    std::mt19937 random;
    std::normal_distribution<double> normal;
    for (int i = 0; i < matrix.rows(); i++) {
        for (int j = 0; j < matrix.cols(); j++) {
            matrix(i, j) = normal(random);
        }
    }

    matrix = Matrix(matrix.transpose() * matrix);

    // Store the matrix in an HDF5 file.
    h5::file file("dump.h5", "w");

    file.dataset<h5::f32, 2>("eigen/matrix").write(matrix);

    std::clog << "Matrix is written to dump.h5\n";
    std::clog << "sum = " << matrix.sum() << '\n';

    // Read back into another matrix.
    Matrix buffer;

    file.dataset<h5::f32, 2>("eigen/matrix").read_fit(buffer);

    std::clog << "Matrix is read back from the file\n";
    std::clog << "sum = " << buffer.sum() << '\n';
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_EXP_MOD_NORMAL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_EXP_MOD_NORMAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/prob/exponential_rng.hpp>
#include <stan/math/prim/scal/prob/normal_rng.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/**
 * Return an exponentially modified normal random variate for the
 * given location, scale, and inverse scale using the specified random
 * number generator.
 *
 * mu, sigma, and lambda can each be a scalar or a one-dimensional container.
 * Any non-scalar inputs must be the same size.
 *
 * @tparam T_loc Type of location parameter
 * @tparam T_scale Type of scale parameter
 * @tparam T_inv_scale Type of inverse scale parameter
 * @tparam RNG type of random number generator
 * @param mu (Sequence of) location parameter(s)
 * @param sigma (Sequence of) scale parameter(s)
 * @param lambda (Sequence of) inverse scale parameter(s)
 * @param rng random number generator
 * @return (Sequence of) Exponentially modified normal random variate(s)
 * @throw std::domain_error if mu is infinite, sigma is nonpositive,
 * or lambda is nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_scale, typename T_inv_scale, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_scale, T_inv_scale>::type
exp_mod_normal_rng(const T_loc& mu, const T_scale& sigma,
                   const T_inv_scale& lambda, RNG& rng) {
  static const char* function = "exp_mod_normal_rng";

  check_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Scale parameter", sigma);
  check_positive_finite(function, "Inv_scale parameter", lambda);
  check_consistent_sizes(function, "Location parameter", mu, "Scale Parameter",
                         sigma, "Inv_scale Parameter", lambda);

  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_scale> sigma_vec(sigma);
  scalar_seq_view<T_inv_scale> lambda_vec(lambda);
  size_t N = max_size(mu, sigma, lambda);
  VectorBuilder<true, double, T_loc, T_scale, T_inv_scale> output(N);

  for (size_t n = 0; n < N; ++n)
    output[n] = normal_rng(mu_vec[n], sigma_vec[n], rng)
                + exponential_rng(lambda_vec[n], rng);

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


//  (C) Copyright Gennadiy Rozental 2011-2015.
//  Distributed under 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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/bind.hpp>
using namespace boost::unit_test;
namespace tt=boost::test_tools;
using namespace boost;

class test_class {
public:
  void test_method( double d )
  {
    BOOST_TEST( d * 100 == (double)(int)(d*100), tt::tolerance(0.0001) );
  }
} tester;

bool init_unit_test()
{
  double params[] = { 1., 1.1, 1.01, 1.001, 1.0001 };

  boost::function<void (double)> test_method = bind( &test_class::test_method, &tester, _1);

  framework::master_test_suite().
    add( BOOST_PARAM_TEST_CASE( test_method, params, params+5 ) );

  return true;
}
//]


// Copyright (c) 2006 Johan Rade
// Copyright (c) 2011 Paul A. Bristow  To incorporate into Boost.Math

// Distributed under 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)

#ifdef _MSC_VER
#   pragma warning(disable : 4702) // Unreachable code.
#   pragma warning(disable : 4127) // expression is constant.
#endif

#define BOOST_TEST_MAIN

#include <iomanip>
#include <locale>
#include <sstream>
#include <limits>

#include <boost/test/auto_unit_test.hpp>
#include <boost/math/special_functions/nonfinite_num_facets.hpp>
#include "almost_equal.ipp"
#include "s_.ipp"

namespace {

// the anonymous namespace resolves ambiguities on platforms
// with fpclassify etc functions at global scope

using namespace boost::math;
using boost::math::signbit;
using boost::math::changesign;
using boost::math::isnan;

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

template<class CharType, class ValType> void signed_zero_test_impl();

BOOST_AUTO_TEST_CASE(signed_zero_test)
{
    signed_zero_test_impl<char, float>();
    signed_zero_test_impl<char, double>();
    signed_zero_test_impl<char, long double>();
    signed_zero_test_impl<wchar_t, float>();
    signed_zero_test_impl<wchar_t, double>();
    signed_zero_test_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void signed_zero_test_impl()
{

  if (signbit(static_cast<CharType>(-1e-6f)/(std::numeric_limits<CharType>::max)()) != -0)
  {
    BOOST_TEST_MESSAGE("Signed zero is not supported on this platform.");
    return;
  }

    std::locale old_locale;
    std::locale tmp_locale(
        old_locale, new nonfinite_num_put<CharType>(signed_zero));
    std::locale new_locale(tmp_locale, new nonfinite_num_get<CharType>);

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a1 = static_cast<ValType>(0);
    ValType a2 = (changesign)(static_cast<ValType>(0));
    BOOST_CHECK(!(signbit)(a1));
    BOOST_CHECK((signbit)(a2));

    ss << a1 << ' ' << a2;

    std::basic_string<CharType> s = S_("0 -0");
    BOOST_CHECK(ss.str() == s);

    ValType b1, b2;
    ss >> b1 >> b2;

    BOOST_CHECK(b1 == a1);
    BOOST_CHECK(b2 == a2);
    BOOST_CHECK(!(signbit)(b1));
    BOOST_CHECK((signbit)(b2));
    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit);
}

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

}   // anonymous namespace


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    typedef typename mtl::Collection<Matrix>::size_type     size_type;
    cout << "\n" << name << "\n";
    typename mtl::Collection<Matrix>::value_type four(4.0), one(1.0), zero(0.0);

    diagonal_setup(A, 1.0);
    cout << "Diagonal matrix:\n" << A << "\n";
    for (size_type r= 0; r < num_rows(A); ++r)
	for (size_type c= 0; c < num_cols(A); ++c)
	    if (r == c && A[r][c] != one) {throw "wrong diagonal";}
	    else MTL_THROW_IF(r != c && A[0][1] != zero, mtl::runtime_error("wrong off-diagonal"));

    A= 4.0;
    cout << "Diagonal matrix:\n" << A << "\n";
    for (size_type r= 0; r < num_rows(A); ++r)
	for (size_type c= 0; c < num_cols(A); ++c)
	    if (r == c && A[r][c] != four) {throw "wrong diagonal";}
	    else MTL_THROW_IF(r != c && A[0][1] != zero, mtl::runtime_error("wrong off-diagonal"));
}

int main(int, char**)
{
    using namespace mtl;
    unsigned size= 7;

    dense2D<double>                                      dr(size, size), dr2(size, size+1), dr3(size+1, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dr2, "Dense row major (one column more)");
    test(dr3, "Dense row major (one row more)");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    return 0;
}


/*
 * Copyright (c) 2018-2019, Eberty Alves, Pedro Raimundo
 */

// OpenMP
#include <omp.h>

// C++ standard library
#include <bits/stdc++.h>

// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>

// Boost
#include <boost/program_options.hpp>

// Adapted from https://stackoverflow.com/questions/7616511/
float mean(std::vector<float> &v) {
  double sum = std::accumulate(v.begin(), v.end(), 0.0);
  double mean = sum / v.size();
  return mean;
}

// From https://stackoverflow.com/questions/1719070/1719155#1719155
// Test median fusion instead of average
float median(std::vector<float> &v) {
  size_t n = v.size() / 2;
  std::nth_element(v.begin(), v.begin() + n, v.end());
  return v[n];
}

int save_ply(cv::Mat depth_mat, std::string filename, float min_value = std::numeric_limits<float>::min(),
             float max_value = std::numeric_limits<float>::max()) {
  FILE *fp;

  if ((fp = fopen(filename.c_str(), "w")) == NULL) {
    printf("Error: while creating file %s\n", filename.c_str());
    return 0;
  }

  // Normals via cross-product
  cv::Mat normal_mat = cv::Mat(depth_mat.size(), CV_32FC3);
  for (int i = 0; i < depth_mat.size().height; i++) {
    for (int j = 0; j < depth_mat.size().width; j++) {
      uint16_t right = 0, left = 0, down = 0, up = 0;

      if (i > 0 && j > 0 && i < depth_mat.size().height - 1 && j < depth_mat.size().width - 1) {
        right = depth_mat.at<uint16_t>(i + 1, j);
        left = depth_mat.at<uint16_t>(i - 1, j);
        down = depth_mat.at<uint16_t>(i, j + 1);
        up = depth_mat.at<uint16_t>(i, j - 1);
      }

      // Threshold (as per Hinterstoisser et al. (2011))
      if (abs(depth_mat.at<uint16_t>(i, j) - right) > 20) {
        right = 0;
      }
      if (abs(depth_mat.at<uint16_t>(i, j) - left) > 20) {
        left = 0;
      }
      if (abs(depth_mat.at<uint16_t>(i, j) - down) > 20) {
        down = 0;
      }
      if (abs(depth_mat.at<uint16_t>(i, j) - up) > 20) {
        up = 0;
      }

      double dzdx = (right - left) / 2.0;
      double dzdy = (down - up) / 2.0;

      cv::Vec3d d(-dzdx, -dzdy, 1.0);
      cv::Vec3d n = normalize(d);

      normal_mat.at<cv::Vec3f>(i, j) = n;
    }
  }

  uint32_t num_vertices = 0;
  // Iterate to count the number of vertices
  for (int i = 0; i < depth_mat.size().height; i++) {
    for (int j = 0; j < depth_mat.size().width; j++) {
      uint16_t z_in_mm = depth_mat.at<uint16_t>(i, j);
      if (z_in_mm != 0 && z_in_mm >= min_value && z_in_mm <= max_value) {
        num_vertices++;
      }
    }
  }

  // Then a second time to write the .ply file
  fprintf(fp, "ply\n");
  fprintf(fp, "format ascii 1.0\n");
  fprintf(fp, "element vertex %d\n", num_vertices);
  fprintf(fp, "property float x\n");
  fprintf(fp, "property float y\n");
  fprintf(fp, "property float z\n");
  fprintf(fp, "property float nx\n");
  fprintf(fp, "property float ny\n");
  fprintf(fp, "property float nz\n");
  fprintf(fp, "property uchar red\n");
  fprintf(fp, "property uchar green\n");
  fprintf(fp, "property uchar blue\n");
  fprintf(fp, "end_header\n");

  for (int i = 0; i < depth_mat.size().height; i++) {
    for (int j = 0; j < depth_mat.size().width; j++) {
      uint16_t z_in_mm = depth_mat.at<uint16_t>(i, j);
      double fx = 572.882768, fy = 542.739980, cx = 314.649173, cy = 240.160459;
      if (z_in_mm != 0 && z_in_mm >= min_value && z_in_mm <= max_value) {
        double vx = z_in_mm * (j - cx) * (1 / fx);
        double vy = -z_in_mm * (i - cy) * (1 / fy);
        cv::Vec3f normal = normal_mat.at<cv::Vec3f>(i, j);
        fprintf(fp, "%.6lf %.6lf %.6lf %.6lf %.6lf %.6lf %d %d %d\n", (double)vx, (double)vy, (double)-z_in_mm,
                (double)normal[0], (double)normal[1], (double)normal[2], 128, 128, 128);
      }
    }
  }

  fclose(fp);
  fflush(fp);

  printf("%s saved!\n", filename.c_str());
  fflush(stdout);

  return 1;
}

int main(int argc, char **argv) {
  try {
    // Declaration of variables
    std::string capture_name;
    uint capture_step;
    uint num_captures;
    uint sr_size;
    int resample_factor;

    uint view_angle;
    std::string view_name;
    bool single_view;

    // Parse command-line options
    namespace po = boost::program_options;

    // Define command-line options
    po::options_description desc("Options");
    desc.add_options()
    ("help,h", "Print help message")
    ("capture_name,n", po::value<std::string>(&capture_name)->default_value("artefact"), "Prefix of saved files")
    ("capture_step,s", po::value<uint>(&capture_step)->default_value(20), "Angles (in degrees) for each capture")
    ("num_captures,c", po::value<uint>(&num_captures)->default_value(18), "Number of view points (anlges)")
    ("sr_size,z", po::value<uint>(&sr_size)->default_value(16), "Number of sr images for each capture")
    ("resample_factor,f", po::value<int>(&resample_factor)->default_value(4), "Resample factor")
    ("view_angle,a", po::value<uint>(&view_angle), "Make SR just for an unique view angle")
    ("top,t", "Make SR for top view")
    ("bottom,b", "Make SR for bottom view");

    // Use a parser to evaluate the command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    // Store the command-line options evaluated by the parser
    if (vm.count("help")) {
      std::cout << "Super-resolution tool to reconstruct detail and obtain high-resolution "
                << "depth images from the acquired image bursts." << std::endl
                << std::endl;
      std::cout << desc << std::endl;
      return 0;
    }

    capture_name = vm["capture_name"].as<std::string>();
    capture_step = vm["capture_step"].as<uint>();
    num_captures = vm["num_captures"].as<uint>();
    sr_size = vm["sr_size"].as<uint>();
    resample_factor = vm["resample_factor"].as<int>();

    if (resample_factor <= 0) {
      std::cout << "The command option 'resample_factor', must be bigger than zero" << std::endl;
      return -1;
    }

    if (vm.count("view_angle") || vm.count("top") || vm.count("bottom")) {
      if ((vm.count("view_angle") && vm.count("top")) || (vm.count("view_angle") && vm.count("bottom")) ||
          (vm.count("top") && vm.count("bottom"))) {
        std::cout << "The command options: 'view_angle', 'top' and 'bottom'"
                  << " can not be used together. Choose only one of them" << std::endl;
        return -1;
      }

      std::cout << "When using the command options: 'view_angle', 'top' or 'bottom'"
                << ", the options 'capture_step' and 'num_captures' are ignored" << std::endl;
      capture_step = 0;
      num_captures = 1;

      if (vm.count("view_angle")) {
        view_name = std::to_string(vm["view_angle"].as<uint>() % 360);
      } else if (vm.count("top")) {
        view_name = "top";
      } else if (vm.count("bottom")) {
        view_name = "bottom";
      }

      single_view = true;
    } else {
      single_view = false;
    }

    std::cout << "Capture name: " << capture_name << std::endl;
    if (single_view) {
      std::cout << "View angle: " << view_name << std::endl;
    } else {
      std::cout << "Step: " << capture_step << std::endl;
      std::cout << "N. of captures: " << num_captures << std::endl;
    }
    std::cout << std::endl;

    for (size_t captures = 0; captures < num_captures; captures++) {
      if (!single_view) {
        view_name = std::to_string(captures * capture_step);
      }

      std::vector<cv::Mat> lr_images(sr_size);            // stored as float
      std::vector<cv::Mat> lr_images_upsampled(sr_size);  // stored as float
      std::vector<cv::Mat> alignment_matrices(sr_size);   // affine transform matrices
      cv::Mat hr_image;

      float global_min = std::numeric_limits<float>::max(), global_max = std::numeric_limits<float>::min();

      // Pre-processing Phase - get minimum and maximum values from the LR images
      // we use this information to remove extreme values from the HR image because
      // some non-linear noise might be added on the resampling and fusion phases
      for (size_t i = 0; i < sr_size; i++) {
        std::ostringstream oss_in;
        oss_in << capture_name << "_burst_" << view_name << "_" << i << ".png";
        lr_images[i] = imread(oss_in.str(), cv::IMREAD_ANYDEPTH);
        if (lr_images[i].empty()) {
          std::cout << "Image can not be read: " << oss_in.str() << std::endl;
          return -1;
        }
        lr_images[i].convertTo(lr_images[i], CV_32FC1);
        alignment_matrices[i] = cv::Mat::eye(2, 3, CV_32F);

        double local_min, local_max;
        cv::Mat mask = lr_images[i] > 0;

        minMaxLoc(lr_images[i], &local_min, &local_max, NULL, NULL, mask);

        global_min = local_min < global_min ? local_min : global_min;
        global_max = local_max > global_max ? local_max : global_max;
      }

      printf("Observed Global Min: %.0lf\n", global_min);
      printf("Observed Global Max: %.0lf\n", global_max);

// Registration Phase - Subpixel registration of all LR images to the first
#pragma omp parallel for
      for (size_t i = 0; i < sr_size; i++) {
        cv::Mat template_image = lr_images[0];  // first LR image
        findTransformECC(template_image, lr_images[i], alignment_matrices[i], cv::MOTION_AFFINE,
                         cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, 1E-12));
        std::cout << "View(" << view_name << ") Alignment " << i << "-> 0 = " << alignment_matrices[i] << std::endl;
      }

      // Upsample + Warp Phase - Upsample all LR images and use registration
      // information technically the resize operation should happen before the
      // warpAffine, but this is not working in this version of the program, whilst
      // it worked on an older version, ignoring this for now since the results are
      // good
      for (size_t i = 0; i < sr_size; i++) {
        // Warp - Simplified to rigid transform because we aim to have as little
        // translation and rotation between the LR images as possible while still
        // modifying the intrinsics enough to have complementary data
        if (i > 0) {
          warpAffine(lr_images[i], lr_images[i], alignment_matrices[i], lr_images[i].size(), cv::WARP_INVERSE_MAP);
        }

        // Upsample - can use pyramids or perform a simple scale operation
        // results were exactly the same, still gotta find out why
        cv::resize(lr_images[i], lr_images_upsampled[i], lr_images[i].size() * resample_factor, 0, 0,
                   cv::INTER_NEAREST);
      }

      // Reconstruction Phase - For now, simply averaging the images, this reduces
      // the impact of the additive noise from the kinect sensor on the HR image
      // (Richardt). Nonlinear noise generated during the other phases will be
      // removed using the previously established limits of the depth info; i.e., we
      // can not generate or reconstruct information outside of the observed volume.
      // This implementation performs poorly in case the images are not very well
      // aligned. In fact, even changing the ECC epsilon from 1E-12 to 1E-06
      // adversely affected the results
      /*hr_image = cv::Mat(lr_images_upsampled[0].size(),CV_32FC1);
      for(size_t i = 0; i < sr_size; i++) {
        addWeighted(hr_image, 1.0, lr_images_upsampled[i], (1.0 / sr_size), 0, hr_image);
      }*/

      // New approach -- zero-elimination (ZE) averaging
      hr_image = cv::Mat(lr_images_upsampled[0].size(), CV_32FC1);
      for (int i = 0; i < hr_image.size().height; i++) {
        for (int j = 0; j < hr_image.size().width; j++) {
          std::vector<float> candidate_pixels;
          for (size_t k = 0; k < sr_size; k++) {
            float depth = lr_images_upsampled[k].at<float>(i, j);
            if (depth > 0) {
              candidate_pixels.push_back(depth);
            }
          }
          float &pix = hr_image.at<float>(i, j);
          pix = mean(candidate_pixels);
        }
      }

      // Downsample the HR image back to original size to keep it valid within the
      // coordinate system of the Microsoft Kinect sensor - lanczos removes some
      // artifacts with no apparent downsides
      cv::resize(hr_image, hr_image, hr_image.size() / resample_factor, 0, 0, cv::INTER_LANCZOS4);
      hr_image.convertTo(hr_image, CV_16UC1);
      std::ostringstream oss_out;
      oss_out << capture_name << "_srdepth_" << view_name << ".png";
      imwrite(oss_out.str(), hr_image);
      oss_out.str(std::string());
      oss_out << capture_name << "_srmesh_" << view_name << ".ply";
      save_ply(hr_image, oss_out.str(), global_min, global_max);
      std::cout << std::endl;
    }

    return 0;
  } catch (boost::program_options::error &msg) {
    std::cout << "ERROR: " << msg.what() << std::endl;
  } catch (cv::Exception &msg) {
    std::cout << "ERROR: " << msg.what() << std::endl;
  } catch (...) {
    std::cout << "Some error has occurred." << std::endl;
  }
  return -1;
}


/*=============================================================================
    Copyright (c) 2001-2011 Hartmut Kaiser
    http://spirit.sourceforge.net/

    Distributed under 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)
=============================================================================*/
//[porting_guide_classic_includes
#include <boost/spirit/include/classic.hpp>
#include <boost/spirit/include/phoenix1.hpp>
#include <iostream>
#include <string>
//]

//[porting_guide_classic_namespace
using namespace boost::spirit::classic;
//]

//[porting_guide_classic_grammar
struct roman : public grammar<roman>
{
    template <typename ScannerT>
    struct definition
    {
        definition(roman const& self)
        {
            hundreds.add
                ("C"  , 100)("CC"  , 200)("CCC"  , 300)("CD" , 400)("D" , 500)
                ("DC" , 600)("DCC" , 700)("DCCC" , 800)("CM" , 900) ;

            tens.add
                ("X"  , 10)("XX"  , 20)("XXX"  , 30)("XL" , 40)("L" , 50)
                ("LX" , 60)("LXX" , 70)("LXXX" , 80)("XC" , 90) ;

            ones.add
                ("I"  , 1)("II"  , 2)("III"  , 3)("IV" , 4)("V" , 5)
                ("VI" , 6)("VII" , 7)("VIII" , 8)("IX" , 9) ;

            first = eps_p         [phoenix::var(self.r) = phoenix::val(0)]
                >>  (  +ch_p('M') [phoenix::var(self.r) += phoenix::val(1000)]
                    ||  hundreds  [phoenix::var(self.r) += phoenix::_1]
                    ||  tens      [phoenix::var(self.r) += phoenix::_1]
                    ||  ones      [phoenix::var(self.r) += phoenix::_1]
                    ) ;
        }

        rule<ScannerT> first;
        symbols<unsigned> hundreds;
        symbols<unsigned> tens;
        symbols<unsigned> ones;

        rule<ScannerT> const& start() const { return first; }
    };

    roman(unsigned& r_) : r(r_) {}
    unsigned& r;
};
//]

int main()
{
    {
        //[porting_guide_classic_parse
        std::string input("1,1");
        parse_info<std::string::iterator> pi = parse(input.begin(), input.end(), int_p);

        if (pi.hit)
            std::cout << "successful match!\n";

        if (pi.full)
            std::cout << "full match!\n";
        else
            std::cout << "stopped at: " << std::string(pi.stop, input.end()) << "\n";

        std::cout << "matched length: " << pi.length << "\n";
        //]
    }

    {
        //[porting_guide_classic_phrase_parse
        std::string input(" 1, 1");
        parse_info<std::string::iterator> pi = parse(input.begin(), input.end(), int_p, space_p);

        if (pi.hit)
            std::cout << "successful match!\n";

        if (pi.full)
            std::cout << "full match!\n";
        else
            std::cout << "stopped at: " << std::string(pi.stop, input.end()) << "\n";

        std::cout << "matched length: " << pi.length << "\n";
        //]
    }

    {
        //[porting_guide_classic_use_grammar
        std::string input("MMIX");        // MMIX == 2009
        unsigned value = 0;
        roman r(value);
        parse_info<std::string::iterator> pi = parse(input.begin(), input.end(), r);
        if (pi.hit)
            std::cout << "successfully matched: " << value << "\n";
        //]
    }
    return 0;
}


/*! \file boxplot_full.cpp

  \brief More complex boxplot plot.

  \author Jacob Voytko

  \date 2007
*/

// Copyright Jacob Voytko 2007
// Distributed under the Boost Software License, Version 1.0.
// For more information, see http://www.boost.org

#include <vector>
#include <cmath>

#include <boost/svg_plot/svg_boxplot.hpp>

using std::vector;

// Functions we are using to simulate distributions.
double f(double x)
{
    return 50 / x;
}

double g(double x)
{
    return 40 + 25 * sin(x * 50);
}

int main()
{
    using namespace boost::svg;
    std::vector<double> data1, data2;

    // Fill our vectors;
    for(double i = .1; i < 10; i+=.1)
    {
        data1.push_back(f(i));
        data2.push_back(g(i));
    }

    // Initialize a new plot.
    svg_boxplot my_plot;

    // Color information.
    my_plot.background_border_color(black)
    .background_color(svg_color(67, 111, 69))
    .plot_background_color(svg_color(136, 188, 126))
    .title_color(white)
    .x_label_color(white)
    .y_label_color(white);

    // String information.
    my_plot.title("Boxplots of Common Functions")
           .x_label("Functions")
           .y_label("Population Size");

    // Axis information.
    my_plot.y_range(0, 100)
           .y_minor_tick_length(20)
           .y_major_interval(20);

    // Write data.
    my_plot.plot(data1, "[50 / x]");
    my_plot.plot(data2, "[40 + 25 * sin(x * 50)]");

    my_plot.write("boxplot_full.svg");
    return 0;
} // int main()


//------------------------------------------------------------------------------
/// \file Queue_tests.cpp
/// \date 20201101 04:56
//------------------------------------------------------------------------------
#include "DataStructures/Queues/Queues.h"

#include <boost/test/unit_test.hpp>

using DataStructures::Queues::CircularQueue;
using DataStructures::Queues::QueueAsResizeable;

BOOST_AUTO_TEST_SUITE(DataStructures)
BOOST_AUTO_TEST_SUITE(Queue_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(QueueDefaultConstructs)
{
  QueueAsResizeable<int> queue;

  BOOST_TEST(queue.size() == 0);
  BOOST_TEST(queue.capacity() == 8);
//  BOOST_TEST(queue.p_start() == 0);
//  BOOST_TEST(queue.is_empty());
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(EnqueueAddsOneElement)
{
  QueueAsResizeable<int> queue;

}

// cf. https://leetcode.com/explore/learn/card/queue-stack/228/first-in-first-out-data-structure/1337/
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CircularQueueIsAQueue)
{
  CircularQueue<int> queue {3};

  BOOST_TEST(queue.enqueue(1));
  BOOST_TEST(queue.enqueue(2));
  BOOST_TEST(queue.enqueue(3));
  BOOST_TEST(!queue.enqueue(4)); // Return false, queue is full.
  BOOST_TEST(queue.rear() == 3);
  BOOST_TEST(queue.is_full());
  BOOST_TEST(queue.dequeue()); // return true
  BOOST_TEST(queue.enqueue(4)); // return true
  BOOST_TEST(queue.rear() == 4);
}

BOOST_AUTO_TEST_SUITE_END() // Queue_tests
BOOST_AUTO_TEST_SUITE_END() // DataStructures


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <opencv2/opencv.hpp>

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"
#include "perspective_camera_with_intrinsics_tait_bryan_cw_jacobian.h"
#include "perspective_camera_with_intrinsics_rodrigues_cw_jacobian.h"
#include "perspective_camera_with_intrinsics_quaternion_cw_jacobian.h"
#include "quaternion_constraint_jacobian.h"
#include "perspective_camera_tait_bryan_wc_jacobian.h"

double k1 = 0.1686;
double k2 = -0.515827;
double k3 = 0.446983;
double k4 = 0.0;
double k5 = 0.0;
double k6 = 0.0;
double p1 = -0.00138148;
double p2 = 0.000127175;

double k1tmp;
double k2tmp;
double k3tmp;
double k4tmp;
double k5tmp;
double k6tmp;
double p1tmp;
double p2tmp;

struct KeyPoint{
	double u;
	double v;
	std::pair<int,int> index_to_tie_point;
};

struct Camera{
	Eigen::Affine3d pose;
	std::vector<std::vector<KeyPoint>> key_points;
};

std::vector<std::vector<Eigen::Vector3d>> tie_points;
std::vector<Camera> cameras;
PerspectiveCameraParams cam_params;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = -62, rotate_y = 21;
float translate_z = -12.4502;
float translate_x = -0.899999, translate_y = -1.49993;
bool show_only_one = true;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){
	k1tmp = k1;
	k2tmp = k2;
	k3tmp = k3;
	p1tmp = p1;
	p2tmp = p2;

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	cam_params.fx = 1727;
	cam_params.fy = 1727;
	cam_params.cx = 985;
	cam_params.cy = 522;

	for(size_t i = 0 ; i < 8; i++){
		std::vector<Eigen::Vector3d> tps;
		for(size_t j = 0 ; j < 8; j++){
		Eigen::Vector3d p;
		p.x() = i;
		p.y() = j;
		p.z() = 0;
		tps.push_back(p);
		}
		tie_points.push_back(tps);
	}

	for(float i = 3 ; i < 5; i+=0.5){
		Camera c;
		c.pose = Eigen::Affine3d::Identity();
		c.pose(0,3) = 4;
		c.pose(1,3) = i;
		c.pose(2,3) = -10;
		cameras.push_back(c);
	}

	for(size_t i = 0 ; i < cameras.size(); i++){
		for(size_t j = 0; j < tie_points.size(); j++){
			std::vector<KeyPoint> kps;
			for(size_t k = 0; k < tie_points[j].size(); k++){
				KeyPoint kp;
				kp.index_to_tie_point = std::pair<int,int>(j,k);

				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose.inverse());
				projection_perspective_camera_with_intrinsics_tait_bryan_cw(kp.u, kp.v, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
						tie_points[j][k].x(), tie_points[j][k].y(), tie_points[j][k].z(),  k1,  k2,  k3, p1, p2);
				kps.push_back(kp);
			}
			cameras[i].key_points.push_back(kps);
		}
	}

	k1 = 0.0;
	k2 = 0.0;
	p1 = 0.0;
	p2 = 0.0;
	k3 = 0.0;

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("perspective_camera_intrinsic_calibration");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

Eigen::Vector3d get_intersection(float u, float v, Eigen::Affine3d camera_pose_wc){
	cv::Mat mat(1, 2, CV_32F);
	mat.at<float>(0, 0) = u;
	mat.at<float>(0, 1) = v;

	cv::Mat cv_cam_matrix = (cv::Mat_<float>(3, 3) << cam_params.fx, 0, cam_params.cx, 0, cam_params.fy, cam_params.cy, 0, 0, 1);
	cv::Mat cv_dist_params = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);

	mat = mat.reshape(2);
	cv::undistortPoints(mat, mat, cv_cam_matrix, cv_dist_params, cv::Mat(), cv_cam_matrix,
						cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::MAX_ITER, 20, 1e-6));
	mat = mat.reshape(1);

	double undistorted_u = mat.at<float>(0, 0);
	double undistorted_v = mat.at<float>(0, 1);

	Eigen::Matrix3d K;
	K(0,0) = cam_params.fx;
	K(0,1) = 0;
	K(0,2) = cam_params.cx;
	K(1,0) = 0;
	K(1,1) = cam_params.fy;
	K(1,2) = cam_params.cy;
	K(2,0) = 0;
	K(2,1) = 0;
	K(2,2) = 1;

	Eigen::Vector3d p(undistorted_u, undistorted_v, 1);
	Eigen::Vector3d r =  K.inverse() * p;
	Eigen::Matrix3d R = camera_pose_wc.rotation();
	Eigen::Vector3d T = camera_pose_wc.translation();
	Eigen::Vector3d rt = R*r + T;

	Eigen::Vector3d n(0,0,-1);
	Eigen::Vector3d a(camera_pose_wc(0,3), camera_pose_wc(1,3), camera_pose_wc(2,3));
	Eigen::Vector3d b = rt;

	Eigen::Vector3d ba = b-a;
	Eigen::Vector3d intersection = a + (((0 - n.dot(a))/n.dot(ba)) * ba);
	return intersection;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glLineWidth(2);
	glColor3f(1,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0 ; i < tie_points.size(); i++){
		for(size_t j = 0 ; j < tie_points[i].size(); j++){
			if(j+1 < tie_points[i].size()){
				glVertex3f(tie_points[i][j].x(), tie_points[i][j].y(), tie_points[i][j].z());
				glVertex3f(tie_points[i][j+1].x(), tie_points[i][j+1].y(), tie_points[i][j+1].z());
			}

			if(i+1 < tie_points.size()){
				glVertex3f(tie_points[i][j].x(), tie_points[i][j].y(), tie_points[i][j].z());
				glVertex3f(tie_points[i+1][j].x(), tie_points[i+1][j].y(), tie_points[i+1][j].z());
			}
		}
	}
	glEnd();

	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	int number_cameras = 1;
	if(!show_only_one){
		number_cameras = cameras.size();
	}
	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < number_cameras; i++){
		for(size_t j = 0; j < cameras[i].key_points.size(); j++){
			for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){
				if(k+1 < tie_points[j].size()){
					Eigen::Vector3d intersection = get_intersection(cameras[i].key_points[j][k].u, cameras[i].key_points[j][k].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());

					intersection = get_intersection(cameras[i].key_points[j][k+1].u, cameras[i].key_points[j][k+1].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());
				}

				if(j+1 < tie_points.size()){
					Eigen::Vector3d intersection = get_intersection(cameras[i].key_points[j][k].u, cameras[i].key_points[j][k].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());

					intersection = get_intersection(cameras[i].key_points[j+1][k].u, cameras[i].key_points[j+1][k].v, cameras[i].pose);
					glVertex3f(intersection.x(), intersection.y(), intersection.z());
				}
			}
		}
	}
	glEnd();
	glPointSize(1);

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'c':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose pose;
				pose.px = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.py = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.pz = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1;
				pose.om = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.fi = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;
				pose.ka = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.1;

				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose);
				cameras[i].pose = cameras[i].pose * m;
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			int number_intrinsic_paramters = 5;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].key_points.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){
						TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose.inverse());

						Eigen::Matrix<double, 2, 1> delta;
						observation_equation_perspective_camera_with_intrinsics_tait_bryan_cw(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy,
								pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								std::floor(cameras[i].key_points[j][k].u), std::floor(cameras[i].key_points[j][k].v), k1, k2, k3, p1, p2);

						Eigen::Matrix<double, 2, 14> jacobian;
						observation_equation_perspective_camera_with_intrinsics_tait_bryan_cw_jacobian(jacobian, cam_params.fx, cam_params.fy,
								pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								k1, k2, k3, p1, p2);

						int ir = tripletListB.size();
						int ic_camera = i * 6;

						for(size_t l = 0; l < number_intrinsic_paramters; l++){
							tripletListA.emplace_back(ir     , l, -jacobian(0,l));
						}
						for(size_t l = 0; l < number_intrinsic_paramters; l++){
							tripletListA.emplace_back(ir + 1  , l, -jacobian(1,l));
						}

						tripletListA.emplace_back(ir     , ic_camera     + number_intrinsic_paramters, -jacobian(0,0 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 1 + number_intrinsic_paramters, -jacobian(0,1 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 2 + number_intrinsic_paramters, -jacobian(0,2 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 3 + number_intrinsic_paramters, -jacobian(0,3 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 4 + number_intrinsic_paramters, -jacobian(0,4 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 5 + number_intrinsic_paramters, -jacobian(0,5 + number_intrinsic_paramters));

						tripletListA.emplace_back(ir + 1 , ic_camera     + number_intrinsic_paramters, -jacobian(1,0 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 1 + number_intrinsic_paramters, -jacobian(1,1 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 2 + number_intrinsic_paramters, -jacobian(1,2 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 3 + number_intrinsic_paramters, -jacobian(1,3 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 4 + number_intrinsic_paramters, -jacobian(1,4 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 5 + number_intrinsic_paramters, -jacobian(1,5 + number_intrinsic_paramters));

						tripletListP.emplace_back(ir    , ir    , cauchy(delta(0,0), 1));
						tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0), 1));

						tripletListB.emplace_back(ir    , 0,  delta(0,0));
						tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
					}
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 6 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 6 + number_intrinsic_paramters, cameras.size() * 6 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6 + number_intrinsic_paramters, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 6 + number_intrinsic_paramters){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				k1 += h_x[counter++];
				k2 += h_x[counter++];
				k3 += h_x[counter++];
				p1 += h_x[counter++];
				p2 += h_x[counter++];

				for(size_t i = 0; i < cameras.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose.inverse());
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_tait_bryan(pose).inverse();
				}

				std::cout << "desired intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1tmp << " k2: " << k2tmp << " k3: " << k3tmp << " k4: " << k4tmp << " k5: " << k5tmp << " k6: " << k6tmp << " p1: " << p1tmp << " p2: " << p2tmp << std::endl;
				std::cout << "computer intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1 << " k2: " << k2 << " k3: " << k3 << " k4: " << k4 << " k5: " << k5 << " k6: " << k6 << " p1: " << p1 << " p2: " << p2 << std::endl;

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.px += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.py += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.pz += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			int number_intrinsic_paramters = 5;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].key_points.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){

						RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose.inverse());

						Eigen::Matrix<double, 2, 1> delta;
						observation_equation_perspective_camera_with_intrinsics_rodrigues_cw(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy,
								pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								std::floor(cameras[i].key_points[j][k].u), std::floor(cameras[i].key_points[j][k].v), k1, k2, k3, p1, p2);

						Eigen::Matrix<double, 2, 14> jacobian;
						observation_equation_perspective_camera_with_intrinsics_rodrigues_cw_jacobian(jacobian, cam_params.fx, cam_params.fy,
								pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								k1, k2, k3, p1, p2);

						int ir = tripletListB.size();
						int ic_camera = i * 6;

						tripletListA.emplace_back(ir     , 0, -jacobian(0,0));
						tripletListA.emplace_back(ir     , 1, -jacobian(0,1));
						tripletListA.emplace_back(ir     , 2, -jacobian(0,2));
						tripletListA.emplace_back(ir     , 3, -jacobian(0,3));
						tripletListA.emplace_back(ir     , 4, -jacobian(0,4));

						tripletListA.emplace_back(ir + 1 , 0, -jacobian(1,0));
						tripletListA.emplace_back(ir + 1 , 1, -jacobian(1,1));
						tripletListA.emplace_back(ir + 1 , 2, -jacobian(1,2));
						tripletListA.emplace_back(ir + 1 , 3, -jacobian(1,3));
						tripletListA.emplace_back(ir + 1 , 4, -jacobian(1,4));

						tripletListA.emplace_back(ir     , ic_camera     + number_intrinsic_paramters, -jacobian(0,0 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 1 + number_intrinsic_paramters, -jacobian(0,1 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 2 + number_intrinsic_paramters, -jacobian(0,2 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 3 + number_intrinsic_paramters, -jacobian(0,3 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 4 + number_intrinsic_paramters, -jacobian(0,4 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 5 + number_intrinsic_paramters, -jacobian(0,5 + number_intrinsic_paramters));

						tripletListA.emplace_back(ir + 1 , ic_camera     + number_intrinsic_paramters, -jacobian(1,0 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 1 + number_intrinsic_paramters, -jacobian(1,1 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 2 + number_intrinsic_paramters, -jacobian(1,2 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 3 + number_intrinsic_paramters, -jacobian(1,3 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 4 + number_intrinsic_paramters, -jacobian(1,4 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 5 + number_intrinsic_paramters, -jacobian(1,5 + number_intrinsic_paramters));

						tripletListP.emplace_back(ir    , ir    , cauchy(delta(0,0), 1));
						tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0), 1));

						tripletListB.emplace_back(ir    , 0,  delta(0,0));
						tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
					}
				}
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 6 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 6 + number_intrinsic_paramters, cameras.size() * 6 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 6 + number_intrinsic_paramters, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 6 + number_intrinsic_paramters){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				k1 += h_x[counter++];
				k2 += h_x[counter++];
				k3 += h_x[counter++];
				p1 += h_x[counter++];
				p2 += h_x[counter++];

				for(size_t i = 0; i < cameras.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose.inverse());
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_rodrigues(pose).inverse();
				}

				std::cout << "desired intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1tmp << " k2: " << k2tmp << " k3: " << k3tmp << " p1: " << p1tmp << " p2: " << p2tmp << std::endl;
				std::cout << "computer intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1 << " k2: " << k2 << " k3: " << k3 << " p1: " << p1 << " p2: " << p2 << std::endl;

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.px += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.py += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.pz += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.000001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			int number_intrinsic_paramters = 5;

			for(size_t i = 0; i < cameras.size(); i++){
				for(size_t j = 0; j < cameras[i].key_points.size(); j++){
					for(size_t k = 0; k < cameras[i].key_points[j].size(); k++){
						QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose.inverse());

						Eigen::Matrix<double, 2, 1> delta;
						observation_equation_perspective_camera_with_intrinsics_quaternion_cw(delta, cam_params.fx, cam_params.fy, cam_params.cx, cam_params.cy,
								pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								std::floor(cameras[i].key_points[j][k].u), std::floor(cameras[i].key_points[j][k].v), k1, k2, k3, p1, p2);

						Eigen::Matrix<double, 2, 15> jacobian;
						observation_equation_perspective_camera_with_intrinsics_quaternion_cw_jacobian(jacobian, cam_params.fx, cam_params.fy,
								pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].x(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].y(),
								tie_points[cameras[i].key_points[j][k].index_to_tie_point.first][cameras[i].key_points[j][k].index_to_tie_point.second].z(),
								k1, k2, k3, p1, p2);

						int ir = tripletListB.size();
						int ic_camera = i * 7;

						tripletListA.emplace_back(ir     , 0, -jacobian(0,0));
						tripletListA.emplace_back(ir     , 1, -jacobian(0,1));
						tripletListA.emplace_back(ir     , 2, -jacobian(0,2));
						tripletListA.emplace_back(ir     , 3, -jacobian(0,3));
						tripletListA.emplace_back(ir     , 4, -jacobian(0,4));

						tripletListA.emplace_back(ir + 1 , 0, -jacobian(1,0));
						tripletListA.emplace_back(ir + 1 , 1, -jacobian(1,1));
						tripletListA.emplace_back(ir + 1 , 2, -jacobian(1,2));
						tripletListA.emplace_back(ir + 1 , 3, -jacobian(1,3));
						tripletListA.emplace_back(ir + 1 , 4, -jacobian(1,4));

						tripletListA.emplace_back(ir     , ic_camera     + number_intrinsic_paramters, -jacobian(0,0 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 1 + number_intrinsic_paramters, -jacobian(0,1 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 2 + number_intrinsic_paramters, -jacobian(0,2 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 3 + number_intrinsic_paramters, -jacobian(0,3 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 4 + number_intrinsic_paramters, -jacobian(0,4 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 5 + number_intrinsic_paramters, -jacobian(0,5 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir     , ic_camera + 6 + number_intrinsic_paramters, -jacobian(0,6 + number_intrinsic_paramters));

						tripletListA.emplace_back(ir + 1 , ic_camera     + number_intrinsic_paramters, -jacobian(1,0 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 1 + number_intrinsic_paramters, -jacobian(1,1 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 2 + number_intrinsic_paramters, -jacobian(1,2 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 3 + number_intrinsic_paramters, -jacobian(1,3 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 4 + number_intrinsic_paramters, -jacobian(1,4 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 5 + number_intrinsic_paramters, -jacobian(1,5 + number_intrinsic_paramters));
						tripletListA.emplace_back(ir + 1 , ic_camera + 6 + number_intrinsic_paramters, -jacobian(1,6 + number_intrinsic_paramters));

						tripletListP.emplace_back(ir    , ir    , cauchy(delta(0,0), 1));
						tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0), 1));

						tripletListB.emplace_back(ir    , 0,  delta(0,0));
						tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
					}
				}
			}
			std::cout << "joo2" << std::endl;
			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 7 + number_intrinsic_paramters;
				int ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 4 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 5 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 6 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), cameras.size() * 7 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(cameras.size() * 7 + number_intrinsic_paramters, cameras.size() * 7 + number_intrinsic_paramters);
			Eigen::SparseMatrix<double> AtPB(cameras.size() * 7 + number_intrinsic_paramters, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == cameras.size() * 7 + number_intrinsic_paramters){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				k1 += h_x[counter++];
				k2 += h_x[counter++];
				k3 += h_x[counter++];
				p1 += h_x[counter++];
				p2 += h_x[counter++];

				for(size_t i = 0; i < cameras.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose.inverse());
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];

					cameras[i].pose = affine_matrix_from_pose_quaternion(pose).inverse();
				}

				std::cout << "desired intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1tmp << " k2: " << k2tmp << " k3: " << k3tmp << " p1: " << p1tmp << " p2: " << p2tmp << std::endl;
				std::cout << "computer intrinsic parameters:" << std::endl;
				std::cout << "k1: " << k1 << " k2: " << k2 << " k3: " << k3 << " p1: " << p1 << " p2: " << p2 << std::endl;

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case '1':{
			k1 -= 0.01;
			std::cout << "k1: " << k1 << std::endl;
			break;
		}
		case '2':{
			k1 += 0.01;
			std::cout << "k1: " << k1 << std::endl;
			break;
		}
		case '3':{
			k2 -= 0.01;
			std::cout << "k2: " << k2 << std::endl;
			break;
		}
		case '4':{
			k2 += 0.01;
			std::cout << "k2: " << k2 << std::endl;
			break;
		}
		case '5':{
			k3 -= 0.01;
			std::cout << "k3: " << k3 << std::endl;
			break;
		}
		case '6':{
			k3 += 0.01;
			std::cout << "k3: " << k3 << std::endl;
			break;
		}
		case '7':{
			p1 -= 0.01;
			std::cout << "p1: " << p1 << std::endl;
			break;
		}
		case '8':{
			p1 += 0.01;
			std::cout << "p1: " << p1 << std::endl;
			break;
		}
		case '9':{
			p2 -= 0.01;
			std::cout << "p2: " << p2 << std::endl;
			break;
		}
		case '0':{
			p2 += 0.01;
			std::cout << "p2: " << p2 << std::endl;
			break;
		}
		case 'e':{
			show_only_one =! show_only_one;
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "t: optimize (Tait-Bryan wc)" << std::endl;
	std::cout << "r: optimize (Rodriguez cw)" << std::endl;
	std::cout << "q: optimize (Quaternion cw)" << std::endl;
	std::cout << "c: add noise to cameras" << std::endl;
	std::cout << "1: k1 -= 0.01" << std::endl;
	std::cout << "2: k1 += 0.01" << std::endl;
	std::cout << "3: k2 -= 0.01" << std::endl;
	std::cout << "4: k2 += 0.01" << std::endl;
	std::cout << "5: k3 -= 0.01" << std::endl;
	std::cout << "6: k3 += 0.01" << std::endl;
	std::cout << "7: p1 -= 0.01" << std::endl;
	std::cout << "8: p1 += 0.01" << std::endl;
	std::cout << "9: p2 -= 0.01" << std::endl;
	std::cout << "0: p2 += 0.01" << std::endl;
	std::cout << "e: show_only_one=!show_only_one" << std::endl;
}


/*--------------------------------------------------------------------

Search for LCG multiplier using the LLL-Spectral Test!  January 2000

Authors: Karl Entacher, Karl.Entacher@fh-sbg.ac.at

         Thomas Schell, Dept. of Scientific Computing, Univ. Salzburg

--------------------------------------------------------------------*/

/*-------------Libraries (needs Victor Shoups NTL-Lib----------------*/

#include <iostream>

#include <fstream>

#include <time.h>

#include <sys/resource.h>

#include <math.h>

#include <float.h>

#include <NTL/ZZ.h>

#include <NTL/RR.h>

#include <NTL/mat_ZZ.h>

#include <NTL/LLL.h>

using namespace std;
using namespace NTL;

/*------------------------ Definitions ------------------------------*/

ZZ lambda, modul, *m_primes;

struct {

  ZZ lambda;

  double min_norm_x_x;

} best;

double fact[8];

const int x_xmax = 8;       /* Test for dimensions <= 8 */

const ZZ n_min=to_ZZ(1);

//const ZZ n_max=to_ZZ(10000);
const ZZ n_max=to_ZZ(10000000);

/*-----------------------------------------------------------------------

   Call the function with:

        lll_search ["output-file"] [modulus] [prim-factors of modulus-1]

   Example:

        lll_search "output-file" 2147483647 2 3 7 11 31 151 331

  ----------------------------------------------------------------------*/

int main(int argc, char *argv[]) {

/*--------------------------    Input-Check        -----------------------*/

  if (!(argc >= 2)) {

    cout << "lll_search_rnd <output-file-name> <modul> <prime factors of modul-1>" << endl;

    exit(0);

  }

/*--------- reads the input-parameters: "file" modulus primefactors ------*/

  ofstream out_file(argv[1], ios::out);

  modul = to_ZZ(argv[2]);

  if (argc-5 > 0) {

    m_primes = new ZZ [argc-3];

    for (int i = 3; i < argc; i++)

      m_primes[i-3] = to_ZZ(argv[i]);

  }

/*------------- output of modulus and factors of modulus-1 ---------------*/

  out_file << modul;

  for (int i = 0; i < argc-3; i++)

    out_file << " " << m_primes[i];

  out_file << endl;

/*-----------------Search for the first primitiv root and its output-------*/

  int is_primitive_root;

  ZZ pr = to_ZZ(1);

  do {

    int k = 0;

    pr++;

    while (k < argc-3 && (is_primitive_root = (PowerMod(pr, (modul-1) / m_primes[k++], modul) != 1)));

  } while (!is_primitive_root);

  cout << "primitive root " << pr << endl;

/*------------- Constants for the normalized Spectral Test   -----------------*/

  fact[0] = 0.0;                                // intentionally left uninitialized -> not used

  fact[1] = to_double(to_RR(1.0) / pow(to_RR(4.0/3.0), to_RR(1.0/4.0)) / pow(to_RR(modul), to_RR(1.0/2.0)));

  fact[2] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/6.0)) / pow(to_RR(modul), to_RR(1.0/3.0)));

  fact[3] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/4.0)) / pow(to_RR(modul), to_RR(1.0/4.0)));

  fact[4] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(3.0/10.0)) / pow(to_RR(modul), to_RR(1.0/5.0)));

  fact[5] = to_double(to_RR(1.0) / pow(to_RR(64.0/3.0), to_RR(1.0/12.0)) / pow(to_RR(modul), to_RR(1.0/6.0)));

  fact[6] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(3.0/7.0)) / pow(to_RR(modul), to_RR(1.0/7.0)));

  fact[7] = to_double(to_RR(1.0) / pow(to_RR(2.0), to_RR(1.0/2.0)) / pow(to_RR(modul), to_RR(1.0/8.0)));

/*------------Search, Matrix (Basis) Input, LLL und Output -------------*/

  best.lambda = to_ZZ(0);

  best.min_norm_x_x = 0.0;

  ZZ t_h = to_ZZ(10);

  struct rusage cur_ru;

  for (ZZ n = n_min; n <= n_max; n++) {

    double min_norm_x_x = 0.0;

    if (GCD(n, modul - 1) == 1) {

      lambda = PowerMod(pr, n, modul);

      mat_ZZ x;

      x.SetDims(x_xmax, x_xmax);

      min_norm_x_x = 1.0;

      for (int j = 2; j <= x_xmax; j++) {

    x.SetDims(j,j);

    x[0][0] = modul;     // first index = rows, second index = columns

    for (int i = 1; i < j; i++)          // fill in the 1s

      x[i][i] = 1;

    for (int i = 1; i < j; i++)

      x[i][0] =-(power(lambda, i));

    ZZ det, rg;

    rg = LLL(det, x, 0);

    double min_x_x = to_double(x[0] * x[0]);

    for (int i = 1; i < j; i++) {

      double x_x = to_double(x[i] * x[i]);

      if (min_x_x > x_x)

        min_x_x = x_x;

    }

    double norm_x_x = fact[j-1] * sqrt(min_x_x);

    if (min_norm_x_x > norm_x_x)

      min_norm_x_x = norm_x_x;

      }

      if (min_norm_x_x > best.min_norm_x_x) {

    best.min_norm_x_x = min_norm_x_x;

    best.lambda = lambda;

    getrusage(RUSAGE_SELF, &cur_ru);

    out_file << "time\t" << cur_ru.ru_utime.tv_sec << "\tn\t" << n << "\tl\t" << lambda << "\t" << min_norm_x_x << endl;

      }

    }

    ZZ n_ = n - n_min;

    if (n_ >= t_h) {

      getrusage(RUSAGE_SELF, &cur_ru);

      cout << n_ << "\t" << cur_ru.ru_utime.tv_sec << endl;

      t_h *= 10;

    }

  }

  getrusage(RUSAGE_SELF, &cur_ru);

  cout << "total time elapsed\t" << cur_ru.ru_utime.tv_sec << endl;

  out_file << "time\t" << cur_ru.ru_utime.tv_sec << "\tl\t" << best.lambda << "\t" << best.min_norm_x_x << endl;

  out_file.close();

}