#include "injectionNeutrons.h"

#include "messages.h"
#include "globalVariables.h"
#include "adafFunctions.h"
//#include <fmath/fbisection.h>
#include <fmath/RungeKutta.h>
//#include <fmath/physics.h>
//#include <boost/property_tree/ptree.hpp>
#include <iostream>
#include <finjection/pgammaPionInj.h>
#include <flosses/lossesPhotoHadronic.h>
#include <finjection/neutronPp.h>
#include <finjection/neutronPgamma.h>
#include <fparameters/SpaceIterator.h>
#include <fparameters/Dimension.h>
#include <fparameters/parameters.h>
#include <fparticle/Particle.h>
#include "write.h"

void injectionNeutrons(State& st)
{
	show_message(msgStart,Module_neutronInjection);

    Particle &p = st.ntProton;
    Particle &n = st.ntNeutron;

	std::ofstream file;
	file.open("neutronInjection.dat",std::ios::out);
	file 	<< "log(E/eV)"
			<< '\t' << "r/rS"
			<< '\t' << "pp"
			<< "\t" << "pY"
			<< std::endl;

	double sumQ = 0.0;
	n.ps.iterate([&](const SpaceIterator& iR) {
		double r = iR.val(DIM_R);
		double dens = st.denf_i.get(iR);
		const double vol = volume(r);
		double height = height_fun(r);
		double tcross = height / cLight; //corregir porque deberia ser (Rmax(theta)-r)/cLight;
		n.ps.iterate([&](const SpaceIterator& iRE) {
			const double En = iRE.val(DIM_E);
			//double n_pg = neutronPgamma(En,tcross,n,p,st.photon.distribution,iR,
			//				st.photon.emin(),st.photon.emax());
			/*double Ep = 4.0/3.0 * En;
			double t_pg = t_pion_PHsimple(Ep,st.ntProton,st.photon.distribution,iR,
					st.photon.emin(),st.photon.emax());
			t_pg = Ep/lossesPhotoMeson(Ep,st.ntProton,st.photon.distribution,iR,st.photon.emin(),
									st.photon.emax());
			double n_pg = 4.0/3.0 * st.ntProton.distribution.interpolate({{DIM_E,Ep}},&iR.coord) / t_pg;*/
			double n_pg = neutronPgamma(En,n,p,st.photon.distribution,iR,st.photon.emin(),st.photon.emax());
			double n_pp = neutronPp(En,p,dens,iR);
			double total = n_pp + n_pg;
			file << safeLog10(En/EV_TO_ERG) << "\t"
				 << iR.val(DIM_R)/schwRadius << "\t"
				 << safeLog10(n_pp*vol)  << "\t"
				 << safeLog10(n_pg*vol) << endl;
			n.injection.set(iRE,total); //en unidades de erg^-1 s^-1 cm^{-3}
		},{-1,iR.coord[DIM_R],0});
		sumQ += integSimpsonLog(n.emin(), n.emax(), [&] (double En)
				{
					return En * n.injection.interpolate({{DIM_E,En}},&iR.coord);
				},30) * vol;
	},{0,-1,0});
	cout << "Total power injected in " << n.id << " = " << sumQ << endl;

	file.close();
	show_message(msgEnd,Module_neutronInjection);
}


#include <boost_adaptbx/graph/graph_type.hpp>
#include <boost_adaptbx/graph/graph_export_adaptor.hpp>
#include <boost_adaptbx/graph/vertex_map.hpp>

#include <boost_adaptbx/exporting.hpp>

#include <boost/python/module.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/def.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/bc_clustering.hpp>

namespace boost_adaptbx
{
namespace
{

template< typename Graph >
struct betweenness_centrality_clustering_export
{
  typedef boost::graph_traits< Graph > graph_traits;
  typedef typename graph_traits::edge_iterator edge_iterator;
  typedef edge_map::generic_edge_map< Graph, double > edge_centrality_map_type;
  typedef vertex_map::index_map< Graph > index_map_type;

  static boost::python::dict betweenness_centrality_clustering(
    Graph& graph,
    double const& threshold
    )
  {
    using namespace boost;
    index_map_type index_map( graph );
    edge_centrality_map_type edge_centrality_map( graph );

    boost::betweenness_centrality_clustering(
      graph,
      boost::bc_clustering_threshold< double >( threshold, graph, false ),
      edge_centrality_map.get(),
      index_map.get()
      );

    boost::python::dict edge_centrality_dict;
    edge_iterator ei, ej;

    for( boost::tie( ei, ej ) = boost::edges( graph ); ei != ej; ++ei )
    {
      edge_centrality_dict[ *ei ] = boost::get( edge_centrality_map.get(), *ei );
    }

    return edge_centrality_dict;
  }

  static void process()
  {
    using namespace boost::python;

    def(
      "betweenness_centrality_clustering",
      betweenness_centrality_clustering,
      ( arg( "graph" ), arg( "threshold" ) )
      );
  }
};

struct clustering_algorithm_exporter
{
  template< typename Export >
  void operator ()(boost::mpl::identity< Export > myexport) const
  {
    typedef typename Export::first graph_type;
    typedef typename Export::second name_type;

    betweenness_centrality_clustering_export< graph_type >::process();
  }
};

} // namespace <anonymous>
} // namespace boost_adaptbx

BOOST_PYTHON_MODULE(boost_adaptbx_graph_clustering_algorithm_ext)
{
  boost_adaptbx::exporting::class_list< boost_adaptbx::graph_type::exports >::process(
    boost_adaptbx::clustering_algorithm_exporter()
    );
}


/*
 * Copyright 2020 California  Institute  of Technology (“Caltech”)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <x/vio/msckf_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

MsckfUpdate::MsckfUpdate(const x::TrackList& trks,
                         const x::AttitudeList& quats,
                         const x::TranslationList& pos,
                         const Triangulation& triangulator,
                         const MatrixXd& cov_s,
                         const int n_poses_max,
                         const double sigma_img)
{
  // Number of features
  const size_t n_trks = trks.size();

  // Number of feature observations
  size_t n_obs = 0;
  for(size_t i=0; i < n_trks; i++)
    n_obs += trks[i].size();

  // Initialize Kalman update matrices
  const size_t rows = 2 * n_obs - n_trks * 3;
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(rows, cols);
  cov_m_diag_ = VectorXd::Ones(rows);
  res_ = MatrixXd::Zero(rows, 1);

  // For each track, compute residual, Jacobian and covariance block
  const double var_img = sigma_img * sigma_img;
  for (size_t i = 0, row_h = 0; i < n_trks; ++i) {
    processOneTrack(trks[i],
                    quats,
                    pos,
                    triangulator,
                    cov_s,
                    n_poses_max,
                    var_img,
                    i,
                    row_h);
  }
}

void MsckfUpdate::processOneTrack(const x::Track& track,
                                  const x::AttitudeList& C_q_G,
                                  const x::TranslationList& G_p_C,
                                  const Triangulation& triangulator,
                                  const MatrixXd& P,
                                  const int n_poses_max,
                                  const double var_img,
                                  const size_t& j,
                                  size_t& row_h)
{
  // Initialization
  const size_t track_size = track.size();
  unsigned int rows_track_j = track_size * 2;
  const size_t cols = P.cols();
  MatrixXd jac_j(MatrixXd::Zero(rows_track_j, cols));
  MatrixXd Hf_j(MatrixXd::Zero(jac_j.rows(), kJacCols));
  MatrixXd res_j(MatrixXd::Zero(rows_track_j, 1));

  // Triangulate feature j
  // Inverse-depth parameters in last observation frame
  Vector3d feature; // inverse-depth parameters in last observation frame
  triangulator.triangulateGN(track, feature);
  const double alpha = feature(0);
  const double beta  = feature(1);
  const double rho   = feature(2);

  // Coordinate of feature in global frame
  x::Quaternion Cn_q_G;
  Cn_q_G.x() = C_q_G.back().ax;
  Cn_q_G.y() = C_q_G.back().ay;
  Cn_q_G.z() = C_q_G.back().az;
  Cn_q_G.w() = C_q_G.back().aw;

  Vector3d G_p_Cn(G_p_C.back().tx, G_p_C.back().ty, G_p_C.back().tz);

  Vector3d G_p_fj = 1 / (rho)*Cn_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Cn;

  x::Quatern attitude_to_quaternion;

  // LOOP OVER ALL FEATURE OBSERVATIONS
  for (size_t i = 0; i < track_size; ++i)
  {
    const unsigned int pos = C_q_G.size() - track_size + i;

    Quaterniond Ci_q_G_ = attitude_to_quaternion(C_q_G[pos]);
    Vector3d G_p_Ci_(G_p_C[pos].tx, G_p_C[pos].ty, G_p_C[pos].tz);

    // Feature position expressed in camera frame.
    Vector3d Ci_p_fj;
    Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

    // eq. 20(a)
    Vector2d z;
    z(0) = track[i].getX();
    z(1) = track[i].getY();

    Vector2d z_hat(z);
    assert(Ci_p_fj(2));
    z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2);
    z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2);

    // eq. 20(b)
    res_j(i * 2, 0) = z(0) - z_hat(0);
    res_j(i * 2 + 1, 0) = z(1) - z_hat(1);

    // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23)
    VisJacBlock J_i = VisJacBlock::Zero();
    // first row
    J_i(0, 0) = 1.0 / Ci_p_fj(2);
    J_i(0, 1) = 0.0;
    J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2);
    // second row
    J_i(1, 0) = 0.0;
    J_i(1, 1) = 1.0 / Ci_p_fj(2);
    J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2);

    unsigned int row = i * kVisJacRows;

    // Measurement Jacobians wrt attitude, position and feature
    // Position
    VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose();

    Vector3d dP = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

    // Attitude
    VisJacBlock J_attitude = J_i * x::Skew(dP(0), dP(1), dP(2)).matrix;

    // Feature
    Hf_j.block<kVisJacRows, kJacCols>(row, 0) = -J_position;

    // Update stacked Jacobian matrix associated to the current feature
    unsigned int col = pos * kJacCols;
    jac_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_position;

    col += n_poses_max * kJacCols;

    jac_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_attitude;
  }  // LOOP OVER ALL FEATURE OBSERVATIONS

  //========================================================================
  // Left nullspace projection
  //========================================================================
  // Nullspace computation
  MatrixXd q = Hf_j.householderQr().householderQ();
  MatrixXd A = x::MatrixBlock(q, 0, 3);

  // Projections
  MatrixXd res0_j = A.transpose() * res_j;
  MatrixXd jac0_j = A.transpose() * jac_j;

  // Noise measurement matrix
  const VectorXd r0_j_diag = var_img * VectorXd::Ones(rows_track_j - 3);
  const MatrixXd r0_j = r0_j_diag.asDiagonal();

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  MatrixXd S_inv = (jac0_j * P * jac0_j.transpose() + r0_j).inverse();
  MatrixXd gamma = res0_j.transpose() * S_inv * res0_j;
  boost::math::chi_squared_distribution<> my_chisqr(2 * track_size - 3); // 2*Mj-3 DOFs
  double chi = quantile(my_chisqr, 0.95); // 95-th percentile

  if (gamma(0, 0) < chi)  // Inlier
  {
#ifdef VERBOSE
    inliers_.push_back(G_p_fj);
#endif

    jac_.block(row_h,            // startRow
             0,                 // startCol
             rows_track_j - 3,  // numRows
             cols) = jac0_j;    // numCols

    // Residual vector (for this track)
    res_.block(row_h, 0, rows_track_j - 3, 1) = res0_j;

    // Measurement covariance matrix diagonal
    cov_m_diag_.segment(row_h, rows_track_j - 3) = r0_j_diag;

    row_h += rows_track_j - 3;
  }
  else  // outlier
  {
#ifdef VERBOSE
    outliers_.push_back(G_p_fj);
#endif
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SCALAR_BITWISE_SELECT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SCALAR_BITWISE_SELECT_HPP_INCLUDED
#include <boost/simd/toolbox/bitwise/functions/bitwise_select.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/simd/include/functions/scalar/bitwise_andnot.hpp>
#include <boost/simd/include/functions/scalar/bitwise_or.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::bitwise_select_, tag::cpu_, (A0)(A1),
                                        (boost::mpl::bool_<sizeof(A0) == sizeof(A1)>)
                                      , (scalar_< arithmetic_<A0> >)
                                        (scalar_< arithmetic_<A1> >)
                                        (scalar_< arithmetic_<A1> >)
                                      )
  {
    typedef A1 result_type;
    inline A1 operator()(const A0 & a0,const A1 & a1,const A1 & a2) const
    {
      return b_or(b_and(a1, a0), b_andnot(a2, a0));
    }
  };
} } }

#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2018.
//  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)

//! \file
//!\brief Tests for the two_component_divide() of (fixed_point::detail).

#include <iomanip>
#include <iostream>
#include <random>

#define BOOST_FIXED_POINT_DISABLE_WIDE_INTEGER_MATH
#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION
#define BOOST_FIXED_POINT_DISABLE_IOSTREAM

#define BOOST_TEST_MODULE test_fixed_point_detail_two_component_divide
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/detail/fixed_point_detail.hpp>
#include <boost/fixed_point/detail/parallel_for.h>
#include <boost/fixed_point/detail/random_unsigned_variate_generator.h>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<const std::uint64_t number_of_trials>
  bool test_divide_64_by_32();

  template<const std::uint64_t      number_of_trials,
           const std::uint_fast16_t number_of_bits_left,
           const std::uint_fast16_t number_of_bits_right>
  bool test_divide_xx_by_yy();
}

template<const std::uint64_t number_of_trials>
bool local::test_divide_64_by_32()
{
  bool test_result_is_ok = true;

  // Create a 64-bit random number generator.
  std::mt19937_64 random_generator(std::uint64_t(std::time(nullptr)));

  // Create a 64-bit random distribution.
  std::uniform_int_distribution<std::uint64_t> uint64_distribution(0U, (std::numeric_limits<std::uint64_t>::max)());

  my_concurrency::parallel_for
  (
    std::uint64_t(0U),
    number_of_trials,
    [&test_result_is_ok, &random_generator, &uint64_distribution](std::uint64_t count)
    {
      std::uint64_t a;
      std::uint32_t b;

      for(;;)
      {
        a = uint64_distribution(random_generator);

        if(a != 0U)
        {
          break;
        }
      }

      for(;;)
      {
        b = random_unsigned_variate_generator<std::uint32_t, 32>();

        if(b != 0U)
        {
          break;
        }
      }

      std::uint32_t result_lo;
      std::uint32_t result_hi;

      boost::fixed_point::detail::two_component_divide<std::uint32_t>(std::uint32_t(a),
                                                                      std::uint32_t(a >> 32),
                                                                      b,
                                                                      result_lo,
                                                                      result_hi);

      const std::uint64_t control           = a / b;

      const std::uint64_t result_as_uint64  = result_lo | (std::uint64_t(result_hi) << 32);

      test_result_is_ok &= (control == result_as_uint64);
    }
  );

  return test_result_is_ok;
}

template<const std::uint64_t      number_of_trials,
         const std::uint_fast16_t number_of_bits_left,
         const std::uint_fast16_t number_of_bits_right>
bool local::test_divide_xx_by_yy()
{
  static_assert(number_of_bits_left <= 64U,
                "The number of bits on the left hand side of the division can not exceed 64.");

  static_assert(number_of_bits_right <= 32U,
                "The number of bits on the right hand side of the division can not exceed 32.");

  bool test_result_is_ok = true;

  my_concurrency::parallel_for
  (
    std::uint64_t(0U),
    number_of_trials,
    [&test_result_is_ok](std::uint64_t count)
    {
      std::uint64_t a;
      std::uint32_t b;

      for(;;)
      {
        a = random_unsigned_variate_generator<std::uint64_t, number_of_bits_left>();

        if(a != 0U)
        {
          break;
        }
      }

      for(;;)
      {
        b = random_unsigned_variate_generator<std::uint32_t, number_of_bits_right>();

        if(b != 0U)
        {
          break;
        }
      }

      std::uint32_t result_lo;
      std::uint32_t result_hi;

      boost::fixed_point::detail::two_component_divide<std::uint32_t>(std::uint32_t(a),
                                                                      std::uint32_t(a >> 32),
                                                                      b,
                                                                      result_lo,
                                                                      result_hi);

      const std::uint64_t control           = a / b;

      const std::uint64_t result_as_uint64  = result_lo | (std::uint64_t(result_hi) << 32);

      test_result_is_ok &= (control == result_as_uint64);
    }
  );

  return test_result_is_ok;
}

BOOST_AUTO_TEST_CASE(test_fixed_point_detail_two_component_divide)
{
  const bool test_divide_64_by_32_is_ok = local::test_divide_64_by_32<25000000ULL>();
  BOOST_CHECK_EQUAL(test_divide_64_by_32_is_ok, true);
  std::cout << "test_divide_64_by_32_is_ok: " << std::boolalpha << test_divide_64_by_32_is_ok << std::endl;

  const bool test_divide_64_by_31_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 31U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_31_is_ok, true);
  std::cout << "test_divide_64_by_31_is_ok: " << std::boolalpha << test_divide_64_by_31_is_ok << std::endl;

  const bool test_divide_64_by_17_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 17U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_17_is_ok, true);
  std::cout << "test_divide_64_by_17_is_ok: " << std::boolalpha << test_divide_64_by_17_is_ok << std::endl;

  const bool test_divide_64_by_16_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 16U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_16_is_ok, true);
  std::cout << "test_divide_64_by_16_is_ok: " << std::boolalpha << test_divide_64_by_16_is_ok << std::endl;

  const bool test_divide_64_by_15_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 15U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_15_is_ok, true);
  std::cout << "test_divide_64_by_15_is_ok: " << std::boolalpha << test_divide_64_by_15_is_ok << std::endl;

  const bool test_divide_64_by_09_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 9U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_09_is_ok, true);
  std::cout << "test_divide_64_by_09_is_ok: " << std::boolalpha << test_divide_64_by_09_is_ok << std::endl;

  const bool test_divide_64_by_08_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 8U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_08_is_ok, true);
  std::cout << "test_divide_64_by_08_is_ok: " << std::boolalpha << test_divide_64_by_08_is_ok << std::endl;

  const bool test_divide_64_by_07_is_ok = local::test_divide_xx_by_yy<25000000ULL, 64U, 7U>();
  BOOST_CHECK_EQUAL(test_divide_64_by_07_is_ok, true);
  std::cout << "test_divide_64_by_07_is_ok: " << std::boolalpha << test_divide_64_by_07_is_ok << std::endl;

  const bool test_divide_49_by_32_is_ok = local::test_divide_xx_by_yy<25000000ULL, 49U, 32U>();
  BOOST_CHECK_EQUAL(test_divide_49_by_32_is_ok, true);
  std::cout << "test_divide_49_by_32_is_ok: " << std::boolalpha << test_divide_49_by_32_is_ok << std::endl;

  const bool test_divide_49_by_31_is_ok = local::test_divide_xx_by_yy<25000000ULL, 49U, 31U>();
  BOOST_CHECK_EQUAL(test_divide_49_by_31_is_ok, true);
  std::cout << "test_divide_49_by_31_is_ok: " << std::boolalpha << test_divide_49_by_31_is_ok << std::endl;

  const bool test_divide_48_by_32_is_ok = local::test_divide_xx_by_yy<25000000ULL, 48U, 32U>();
  BOOST_CHECK_EQUAL(test_divide_48_by_32_is_ok, true);
  std::cout << "test_divide_48_by_32_is_ok: " << std::boolalpha << test_divide_48_by_32_is_ok << std::endl;

  const bool test_divide_48_by_31_is_ok = local::test_divide_xx_by_yy<25000000ULL, 48U, 31U>();
  BOOST_CHECK_EQUAL(test_divide_48_by_31_is_ok, true);
  std::cout << "test_divide_48_by_31_is_ok: " << std::boolalpha << test_divide_48_by_31_is_ok << std::endl;

  const bool test_divide_48_by_17_is_ok = local::test_divide_xx_by_yy<25000000ULL, 48U, 17U>();
  BOOST_CHECK_EQUAL(test_divide_48_by_17_is_ok, true);
  std::cout << "test_divide_48_by_17_is_ok: " << std::boolalpha << test_divide_48_by_17_is_ok << std::endl;

  const bool test_divide_48_by_16_is_ok = local::test_divide_xx_by_yy<25000000ULL, 48U, 16U>();
  BOOST_CHECK_EQUAL(test_divide_48_by_16_is_ok, true);
  std::cout << "test_divide_48_by_16_is_ok: " << std::boolalpha << test_divide_48_by_16_is_ok << std::endl;

  const bool test_divide_48_by_15_is_ok = local::test_divide_xx_by_yy<25000000ULL, 48U, 15U>();
  BOOST_CHECK_EQUAL(test_divide_48_by_15_is_ok, true);
  std::cout << "test_divide_48_by_15_is_ok: " << std::boolalpha << test_divide_48_by_15_is_ok << std::endl;

  const bool test_divide_33_by_32_is_ok = local::test_divide_xx_by_yy<25000000ULL, 33U, 32U>();
  BOOST_CHECK_EQUAL(test_divide_33_by_32_is_ok, true);
  std::cout << "test_divide_33_by_32_is_ok: " << std::boolalpha << test_divide_33_by_32_is_ok << std::endl;

  const bool test_divide_33_by_31_is_ok = local::test_divide_xx_by_yy<25000000ULL, 33U, 31U>();
  BOOST_CHECK_EQUAL(test_divide_33_by_31_is_ok, true);
  std::cout << "test_divide_33_by_31_is_ok: " << std::boolalpha << test_divide_33_by_31_is_ok << std::endl;

  const bool test_divide_32_by_32_is_ok = local::test_divide_xx_by_yy<25000000ULL, 32U, 32U>();
  BOOST_CHECK_EQUAL(test_divide_32_by_32_is_ok, true);
  std::cout << "test_divide_32_by_32_is_ok: " << std::boolalpha << test_divide_32_by_32_is_ok << std::endl;

  const bool test_divide_32_by_31_is_ok = local::test_divide_xx_by_yy<25000000ULL, 32U, 31U>();
  BOOST_CHECK_EQUAL(test_divide_32_by_31_is_ok, true);
  std::cout << "test_divide_32_by_31_is_ok: " << std::boolalpha << test_divide_32_by_31_is_ok << std::endl;

  const bool test_divide_32_by_17_is_ok = local::test_divide_xx_by_yy<25000000ULL, 32U, 17U>();
  BOOST_CHECK_EQUAL(test_divide_32_by_17_is_ok, true);
  std::cout << "test_divide_32_by_17_is_ok: " << std::boolalpha << test_divide_32_by_17_is_ok << std::endl;

  const bool test_divide_32_by_16_is_ok = local::test_divide_xx_by_yy<25000000ULL, 32U, 16U>();
  BOOST_CHECK_EQUAL(test_divide_32_by_16_is_ok, true);
  std::cout << "test_divide_32_by_16_is_ok: " << std::boolalpha << test_divide_32_by_16_is_ok << std::endl;

  const bool test_divide_32_by_15_is_ok = local::test_divide_xx_by_yy<25000000ULL, 32U, 15U>();
  BOOST_CHECK_EQUAL(test_divide_32_by_15_is_ok, true);
  std::cout << "test_divide_32_by_15_is_ok: " << std::boolalpha << test_divide_32_by_15_is_ok << std::endl;

  const bool test_divide_16_by_09_is_ok = local::test_divide_xx_by_yy<25000000ULL, 16U, 9U>();
  BOOST_CHECK_EQUAL(test_divide_16_by_09_is_ok, true);
  std::cout << "test_divide_32_by_09_is_ok: " << std::boolalpha << test_divide_16_by_09_is_ok << std::endl;

  const bool test_divide_16_by_08_is_ok = local::test_divide_xx_by_yy<25000000ULL, 16U, 8U>();
  BOOST_CHECK_EQUAL(test_divide_16_by_08_is_ok, true);
  std::cout << "test_divide_32_by_08_is_ok: " << std::boolalpha << test_divide_16_by_08_is_ok << std::endl;

  const bool test_divide_16_by_07_is_ok = local::test_divide_xx_by_yy<25000000ULL, 16U, 7U>();
  BOOST_CHECK_EQUAL(test_divide_16_by_07_is_ok, true);
  std::cout << "test_divide_32_by_07_is_ok: " << std::boolalpha << test_divide_16_by_07_is_ok << std::endl;

  const bool test_divide_16_by_16_is_ok = local::test_divide_xx_by_yy<25000000ULL, 16U, 16U>();
  BOOST_CHECK_EQUAL(test_divide_16_by_16_is_ok, true);
  std::cout << "test_divide_16_by_16_is_ok: " << std::boolalpha << test_divide_16_by_16_is_ok << std::endl;

  const bool test_divide_16_by_15_is_ok = local::test_divide_xx_by_yy<25000000ULL, 16U, 15U>();
  BOOST_CHECK_EQUAL(test_divide_16_by_15_is_ok, true);
  std::cout << "test_divide_16_by_15_is_ok: " << std::boolalpha << test_divide_16_by_15_is_ok << std::endl;
}


#include "camera_model/camera_models/ScaramuzzaCamera.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SVD>
#include <iomanip>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camera_model/gpl/gpl.h"

Eigen::VectorXd
polyfit( Eigen::VectorXd& xVec, Eigen::VectorXd& yVec, int poly_order )
{
    assert( poly_order > 0 );
    assert( xVec.size( ) > poly_order );
    assert( xVec.size( ) == yVec.size( ) );

    Eigen::MatrixXd A( xVec.size( ), poly_order + 1 );
    Eigen::VectorXd B( xVec.size( ) );

    for ( int i = 0; i < xVec.size( ); ++i )
    {
        const double x = xVec( i );
        const double y = yVec( i );

        double x_pow_k = 1.0;

        for ( int k = 0; k <= poly_order; ++k )
        {
            A( i, k ) = x_pow_k;
            x_pow_k *= x;
        }

        B( i ) = y;
    }

    Eigen::JacobiSVD< Eigen::MatrixXd > svd( A, Eigen::ComputeThinU | Eigen::ComputeThinV );
    Eigen::VectorXd x = svd.solve( B );

    return x;
}

namespace camera_model
{

OCAMCamera::Parameters::Parameters( )
: Camera::Parameters( SCARAMUZZA )
, m_C( 0.0 )
, m_D( 0.0 )
, m_E( 0.0 )
, m_center_x( 0.0 )
, m_center_y( 0.0 )
{
    memset( m_poly, 0, sizeof( double ) * SCARAMUZZA_POLY_SIZE );
    memset( m_inv_poly, 0, sizeof( double ) * SCARAMUZZA_INV_POLY_SIZE );
}

bool
OCAMCamera::Parameters::readFromYamlFile( const std::string& filename )
{
    cv::FileStorage fs( filename, cv::FileStorage::READ );

    if ( !fs.isOpened( ) )
    {
        return false;
    }

    if ( !fs["model_type"].isNone( ) )
    {
        std::string sModelType;
        fs["model_type"] >> sModelType;

        if ( !boost::iequals( sModelType, "scaramuzza" ) )
        {
            return false;
        }
    }

    m_modelType = SCARAMUZZA;
    fs["camera_name"] >> m_cameraName;
    m_imageWidth  = static_cast< int >( fs["image_width"] );
    m_imageHeight = static_cast< int >( fs["image_height"] );

    cv::FileNode n = fs["poly_parameters"];
    for ( int i   = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        m_poly[i] = static_cast< double >( n[std::string( "p" ) + boost::lexical_cast< std::string >( i )] );

    n = fs["inv_poly_parameters"];
    for ( int i       = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        m_inv_poly[i] = static_cast< double >( n[std::string( "p" ) + boost::lexical_cast< std::string >( i )] );

    n   = fs["affine_parameters"];
    m_C = static_cast< double >( n["ac"] );
    m_D = static_cast< double >( n["ad"] );
    m_E = static_cast< double >( n["ae"] );

    m_center_x = static_cast< double >( n["cx"] );
    m_center_y = static_cast< double >( n["cy"] );

    return true;
}

void
OCAMCamera::Parameters::writeToYamlFile( const std::string& filename ) const
{
    cv::FileStorage fs( filename, cv::FileStorage::WRITE );

    fs << "model_type"
       << "scaramuzza";
    fs << "camera_name" << m_cameraName;
    fs << "image_width" << m_imageWidth;
    fs << "image_height" << m_imageHeight;

    fs << "poly_parameters";
    fs << "{";
    for ( int i = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        fs << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << m_poly[i];
    fs << "}";

    fs << "inv_poly_parameters";
    fs << "{";
    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        fs << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << m_inv_poly[i];
    fs << "}";

    fs << "affine_parameters";
    fs << "{"
       << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy" << m_center_y << "}";

    fs.release( );
}

OCAMCamera::Parameters&
OCAMCamera::Parameters::operator=( const OCAMCamera::Parameters& other )
{
    if ( this != &other )
    {
        m_modelType   = other.m_modelType;
        m_cameraName  = other.m_cameraName;
        m_imageWidth  = other.m_imageWidth;
        m_imageHeight = other.m_imageHeight;
        m_C           = other.m_C;
        m_D           = other.m_D;
        m_E           = other.m_E;
        m_center_x    = other.m_center_x;
        m_center_y    = other.m_center_y;

        memcpy( m_poly, other.m_poly, sizeof( double ) * SCARAMUZZA_POLY_SIZE );
        memcpy( m_inv_poly, other.m_inv_poly, sizeof( double ) * SCARAMUZZA_INV_POLY_SIZE );
    }

    return *this;
}

std::ostream&
operator<<( std::ostream& out, const OCAMCamera::Parameters& params )
{
    out << "Camera Parameters:" << std::endl;
    out << "    model_type "
        << "scaramuzza" << std::endl;
    out << "   camera_name " << params.m_cameraName << std::endl;
    out << "   image_width " << params.m_imageWidth << std::endl;
    out << "  image_height " << params.m_imageHeight << std::endl;

    out << std::fixed << std::setprecision( 10 );

    out << "Poly Parameters" << std::endl;
    for ( int i = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        out << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << ": " << params.m_poly[i] << std::endl;

    out << "Inverse Poly Parameters" << std::endl;
    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        out << std::string( "p" ) + boost::lexical_cast< std::string >( i ) << ": " << params.m_inv_poly[i] << std::endl;

    out << "Affine Parameters" << std::endl;
    out << "            ac " << params.m_C << std::endl
        << "            ad " << params.m_D << std::endl
        << "            ae " << params.m_E << std::endl;
    out << "            cx " << params.m_center_x << std::endl
        << "            cy " << params.m_center_y << std::endl;

    return out;
}

OCAMCamera::OCAMCamera( )
: m_inv_scale( 0.0 )
{
}

OCAMCamera::OCAMCamera( const OCAMCamera::Parameters& params )
: mParameters( params )
{
    m_inv_scale = 1.0 / ( params.C( ) - params.D( ) * params.E( ) );
}

Camera::ModelType
OCAMCamera::modelType( void ) const
{
    return mParameters.modelType( );
}

const std::string&
OCAMCamera::cameraName( void ) const
{
    return mParameters.cameraName( );
}

int
OCAMCamera::imageWidth( void ) const
{
    return mParameters.imageWidth( );
}

int
OCAMCamera::imageHeight( void ) const
{
    return mParameters.imageHeight( );
}

void
OCAMCamera::estimateIntrinsics( const cv::Size& boardSize,
                                const std::vector< std::vector< cv::Point3f > >& objectPoints,
                                const std::vector< std::vector< cv::Point2f > >& imagePoints )
{
    // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" <<
    // std::endl;
    // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED");

    // Reference: Page 30 of
    // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion
    // Estimation, ETH Zurich. Thesis no. 17635."
    // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf
    // Matlab code: calibrate.m

    // First, estimate every image's extrinsics parameters
    std::vector< Eigen::Matrix3d > RList;
    std::vector< Eigen::Vector3d > TList;

    RList.reserve( imagePoints.size( ) );
    TList.reserve( imagePoints.size( ) );

    // i-th image
    for ( size_t image_index = 0; image_index < imagePoints.size( ); ++image_index )
    {
        const std::vector< cv::Point3f >& objPts = objectPoints.at( image_index );
        const std::vector< cv::Point2f >& imgPts = imagePoints.at( image_index );

        assert( objPts.size( ) == imgPts.size( ) );
        assert( objPts.size( ) == static_cast< unsigned int >( boardSize.width * boardSize.height ) );

        Eigen::MatrixXd M( objPts.size( ), 6 );

        for ( size_t corner_index = 0; corner_index < objPts.size( ); ++corner_index )
        {
            double X = objPts.at( corner_index ).x;
            double Y = objPts.at( corner_index ).y;
            assert( objPts.at( corner_index ).z == 0.0 );

            double u = imgPts.at( corner_index ).x;
            double v = imgPts.at( corner_index ).y;

            M( corner_index, 0 ) = -v * X;
            M( corner_index, 1 ) = -v * Y;
            M( corner_index, 2 ) = u * X;
            M( corner_index, 3 ) = u * Y;
            M( corner_index, 4 ) = -v;
            M( corner_index, 5 ) = u;
        }

        Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeFullU | Eigen::ComputeFullV );
        assert( svd.matrixV( ).cols( ) == 6 );
        Eigen::VectorXd h = -svd.matrixV( ).col( 5 );

        // scaled version of R and T
        const double sr11 = h( 0 );
        const double sr12 = h( 1 );
        const double sr21 = h( 2 );
        const double sr22 = h( 3 );
        const double st1  = h( 4 );
        const double st2  = h( 5 );

        const double AA = square( sr11 * sr12 + sr21 * sr22 );
        const double BB = square( sr11 ) + square( sr21 );
        const double CC = square( sr12 ) + square( sr22 );

        const double sr32_squared_1 = ( -( CC - BB ) + sqrt( square( CC - BB ) + 4.0 * AA ) ) / 2.0;
        const double sr32_squared_2 = ( -( CC - BB ) - sqrt( square( CC - BB ) + 4.0 * AA ) ) / 2.0;

        // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 +
        // (CC-BB)*sr32_squared_1 - AA);

        std::vector< double > sr32_squared_values;
        if ( sr32_squared_1 > 0 )
            sr32_squared_values.push_back( sr32_squared_1 );
        if ( sr32_squared_2 > 0 )
            sr32_squared_values.push_back( sr32_squared_2 );
        assert( !sr32_squared_values.empty( ) );

        std::vector< double > sr32_values;
        std::vector< double > sr31_values;
        for ( auto sr32_squared : sr32_squared_values )
        {
            for ( int sign = -1; sign <= 1; sign += 2 )
            {
                const double sr32 = static_cast< double >( sign ) * std::sqrt( sr32_squared );
                sr32_values.push_back( sr32 );
                if ( sr32_squared == 0.0 )
                {
                    // sr31 can be calculated through norm equality,
                    // but it has positive and negative posibilities
                    // positive one
                    sr31_values.push_back( std::sqrt( CC - BB ) );
                    // negative one
                    sr32_values.push_back( sr32 );
                    sr31_values.push_back( -std::sqrt( CC - BB ) );

                    break; // skip the same situation
                }
                else
                {
                    // sr31 can be calculated throught dot product == 0
                    sr31_values.push_back( -( sr11 * sr12 + sr21 * sr22 ) / sr32 );
                }
            }
        }

        // std::cout << "h= " << std::setprecision(12) << h.transpose() <<
        // std::endl;
        // std::cout << "length: " << sr32_values.size() << " & " <<
        // sr31_values.size() << std::endl;

        assert( !sr31_values.empty( ) );
        assert( sr31_values.size( ) == sr32_values.size( ) );

        std::vector< Eigen::Matrix3d > H_values;
        for ( size_t i = 0; i < sr31_values.size( ); ++i )
        {
            const double sr31   = sr31_values.at( i );
            const double sr32   = sr32_values.at( i );
            const double lambda = 1.0 / sqrt( sr11 * sr11 + sr21 * sr21 + sr31 * sr31 );
            Eigen::Matrix3d H;
            H.setZero( );
            H( 0, 0 ) = sr11;
            H( 0, 1 ) = sr12;
            H( 0, 2 ) = st1;
            H( 1, 0 ) = sr21;
            H( 1, 1 ) = sr22;
            H( 1, 2 ) = st2;
            H( 2, 0 ) = sr31;
            H( 2, 1 ) = sr32;
            H( 2, 2 ) = 0;

            H_values.push_back( lambda * H );
            H_values.push_back( -lambda * H );
        }

        for ( auto& H : H_values )
        {
            // std::cout << "H=\n" << H << std::endl;
            Eigen::Matrix3d R;
            R.col( 0 ) = H.col( 0 );
            R.col( 1 ) = H.col( 1 );
            R.col( 2 ) = H.col( 0 ).cross( H.col( 1 ) );
            // std::cout << "R33 = " << R(2,2) << std::endl;
        }

        std::vector< Eigen::Matrix3d > H_candidates;

        for ( auto& H : H_values )
        {
            Eigen::MatrixXd A_mat( 2 * imagePoints.at( image_index ).size( ), 4 );
            Eigen::VectorXd B_vec( 2 * imagePoints.at( image_index ).size( ) );
            A_mat.setZero( );
            B_vec.setZero( );

            size_t line_index = 0;

            // iterate images
            const double& r11 = H( 0, 0 );
            const double& r12 = H( 0, 1 );
            // const double& r13 = H(0,2);
            const double& r21 = H( 1, 0 );
            const double& r22 = H( 1, 1 );
            // const double& r23 = H(1,2);
            const double& r31 = H( 2, 0 );
            const double& r32 = H( 2, 1 );
            // const double& r33 = H(2,2);
            const double& t1 = H( 0 );
            const double& t2 = H( 1 );

            // iterate chessboard corners in the image
            for ( size_t j = 0; j < imagePoints.at( image_index ).size( ); ++j )
            {
                assert( line_index == 2 * j );

                const double& X = objectPoints.at( image_index ).at( j ).x;
                const double& Y = objectPoints.at( image_index ).at( j ).y;
                const double& u = imagePoints.at( image_index ).at( j ).x;
                const double& v = imagePoints.at( image_index ).at( j ).y;

                double A   = r21 * X + r22 * Y + t2;
                double B   = v * ( r31 * X + r32 * Y );
                double C   = r11 * X + r12 * Y + t1;
                double D   = u * ( r31 * X + r32 * Y );
                double rou = std::sqrt( u * u + v * v );

                A_mat( line_index + 0, 0 ) = A;
                A_mat( line_index + 1, 0 ) = C;
                A_mat( line_index + 0, 1 ) = A * rou;
                A_mat( line_index + 1, 1 ) = C * rou;
                A_mat( line_index + 0, 2 ) = A * rou * rou;
                A_mat( line_index + 1, 2 ) = C * rou * rou;

                A_mat( line_index + 0, 3 ) = -v;
                A_mat( line_index + 1, 3 ) = -u;
                B_vec( line_index + 0 ) = B;
                B_vec( line_index + 1 ) = D;

                line_index += 2;
            }

            assert( line_index == static_cast< unsigned int >( A_mat.rows( ) ) );

            // pseudo-inverse for polynomial parameters and all t3s
            {
                Eigen::JacobiSVD< Eigen::MatrixXd > svd( A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV );

                Eigen::VectorXd x = svd.solve( B_vec );

                // std::cout << "x(poly and t3) = " << x << std::endl;

                if ( x( 2 ) > 0 && x( 3 ) > 0 )
                {
                    H_candidates.push_back( H );
                }
            }
        }

        // printf("H_candidates.size()=%zu\n", H_candidates.size());
        assert( H_candidates.size( ) == 1 );

        Eigen::Matrix3d& H = H_candidates.front( );

        Eigen::Matrix3d R;
        R.col( 0 ) = H.col( 0 );
        R.col( 1 ) = H.col( 1 );
        R.col( 2 ) = H.col( 0 ).cross( H.col( 1 ) );

        Eigen::Vector3d T = H.col( 2 );
        RList.push_back( R );
        TList.push_back( T );

        // std::cout << "#" << image_index << " frame" << " R =" << R << " \nT =
        // " << T.transpose() << std::endl;
    }

    // Second, estimate camera intrinsic parameters and all t3
    Eigen::MatrixXd A_mat( 2 * imagePoints.size( ) * imagePoints.at( 0 ).size( ),
                           SCARAMUZZA_POLY_SIZE - 1 + imagePoints.size( ) );
    Eigen::VectorXd B_vec( 2 * imagePoints.size( ) * imagePoints.at( 0 ).size( ) );
    A_mat.setZero( );
    B_vec.setZero( );

    size_t line_index = 0;

    // iterate images
    for ( size_t i = 0; i < imagePoints.size( ); ++i )
    {
        const double& r11 = RList.at( i )( 0, 0 );
        const double& r12 = RList.at( i )( 0, 1 );
        // const double& r13 = RList.at(i)(0,2);
        const double& r21 = RList.at( i )( 1, 0 );
        const double& r22 = RList.at( i )( 1, 1 );
        // const double& r23 = RList.at(i)(1,2);
        const double& r31 = RList.at( i )( 2, 0 );
        const double& r32 = RList.at( i )( 2, 1 );
        // const double& r33 = RList.at(i)(2,2);
        const double& t1 = TList.at( i )( 0 );
        const double& t2 = TList.at( i )( 1 );

        // iterate chessboard corners in the image
        for ( size_t j = 0; j < imagePoints.at( i ).size( ); ++j )
        {
            assert( line_index == 2 * ( i * imagePoints.at( 0 ).size( ) + j ) );

            const double& X = objectPoints.at( i ).at( j ).x;
            const double& Y = objectPoints.at( i ).at( j ).y;
            const double& u = imagePoints.at( i ).at( j ).x;
            const double& v = imagePoints.at( i ).at( j ).y;

            double A   = r21 * X + r22 * Y + t2;
            double B   = v * ( r31 * X + r32 * Y );
            double C   = r11 * X + r12 * Y + t1;
            double D   = u * ( r31 * X + r32 * Y );
            double rou = std::sqrt( u * u + v * v );

            for ( int k = 1; k <= SCARAMUZZA_POLY_SIZE - 1; ++k )
            {
                double pow_rou = 0.0;
                if ( k == 1 )
                {
                    pow_rou = 1.0;
                }
                else
                {
                    pow_rou = std::pow( rou, k );
                }

                A_mat( line_index + 0, k - 1 ) = A * pow_rou;
                A_mat( line_index + 1, k - 1 ) = C * pow_rou;
            }

            A_mat( line_index + 0, SCARAMUZZA_POLY_SIZE - 1 + i ) = -v;
            A_mat( line_index + 1, SCARAMUZZA_POLY_SIZE - 1 + i ) = -u;
            B_vec( line_index + 0 ) = B;
            B_vec( line_index + 1 ) = D;

            line_index += 2;
        }
    }

    assert( line_index == static_cast< unsigned int >( A_mat.rows( ) ) );

    Eigen::Matrix< double, SCARAMUZZA_POLY_SIZE, 1 > poly_coeff;
    // pseudo-inverse for polynomial parameters and all t3s
    {
        Eigen::JacobiSVD< Eigen::MatrixXd > svd( A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV );

        Eigen::VectorXd x = svd.solve( B_vec );

        poly_coeff[0] = x( 0 );
        poly_coeff[1] = 0.0;
        for ( int i = 1; i < poly_coeff.size( ) - 1; ++i )
        {
            poly_coeff[i + 1] = x( i );
        }
        assert( x.size( ) == static_cast< unsigned int >( SCARAMUZZA_POLY_SIZE - 1 + TList.size( ) ) );
    }

    Parameters params = getParameters( );

    // Affine matrix A is constructed as [C D; E 1]
    params.C( ) = 1.0;
    params.D( ) = 0.0;
    params.E( ) = 0.0;

    params.center_x( ) = params.imageWidth( ) / 2.0;
    params.center_y( ) = params.imageHeight( ) / 2.0;

    for ( size_t i = 0; i < SCARAMUZZA_POLY_SIZE; ++i )
    {
        params.poly( i ) = poly_coeff[i];
    }

    // params.poly(0) = -216.9657476318;
    // params.poly(1) = 0.0;
    // params.poly(2) = 0.0017866911;
    // params.poly(3) = -0.0000019866;
    // params.poly(4) =  0.0000000077;

    // inv_poly
    {
        std::vector< double > rou_vec;
        std::vector< double > z_vec;
        for ( double rou = 0.0; rou <= ( params.imageWidth( ) + params.imageHeight( ) ) / 2; rou += 0.1 )
        {
            double rou_pow_k = 1.0;
            double z         = 0.0;

            for ( int k = 0; k < SCARAMUZZA_POLY_SIZE; k++ )
            {
                z += rou_pow_k * params.poly( k );
                rou_pow_k *= rou;
            }

            rou_vec.push_back( rou );
            z_vec.push_back( z );
        }

        assert( rou_vec.size( ) == z_vec.size( ) );
        Eigen::VectorXd xVec( rou_vec.size( ) );
        Eigen::VectorXd yVec( rou_vec.size( ) );

        for ( size_t i = 0; i < rou_vec.size( ); ++i )
        {
            xVec( i ) = std::atan2( -z_vec.at( i ), rou_vec.at( i ) );
            yVec( i ) = rou_vec.at( i );
        }

        // use lower order poly to eliminate over-fitting cause by
        // noisy/inaccurate data
        const int poly_fit_order       = 4;
        Eigen::VectorXd inv_poly_coeff = polyfit( xVec, yVec, poly_fit_order );

        for ( int i = 0; i <= poly_fit_order; ++i )
        {
            params.inv_poly( i ) = inv_poly_coeff( i );
        }
    }

    setParameters( params );

    std::cout << "initial params:\n" << params << std::endl;
}

/**
 * \brief Lifts a point from the image plane to the unit sphere
 *
 * \param p image coordinates
 * \param P coordinates of the point on the sphere
 */
void
OCAMCamera::liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const
{
    liftProjective( p, P );
    P.normalize( );
}

/**
 * \brief Lifts a point from the image plane to its projective ray
 *
 * \param p image coordinates
 * \param P coordinates of the projective ray
 */
void
OCAMCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const
{
    // Relative to Center
    Eigen::Vector2d xc( p[0] - mParameters.center_x( ), p[1] - mParameters.center_y( ) );

    // Affine Transformation
    // xc_a = inv(A) * xc;
    Eigen::Vector2d xc_a( m_inv_scale * ( xc[0] - mParameters.D( ) * xc[1] ),
                          m_inv_scale * ( -mParameters.E( ) * xc[0] + mParameters.C( ) * xc[1] ) );

    double phi   = std::sqrt( xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1] );
    double phi_i = 1.0;
    double z     = 0.0;

    for ( int i = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
    {
        z += phi_i * mParameters.poly( i );
        phi_i *= phi;
    }

    P << xc[0], xc[1], -z;
}

void
OCAMCamera::liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const
{
    Eigen::Vector2d p_tmp = p / image_scale; // p_tmp is without resize, p is with resize
    liftProjective( p_tmp, P );              // p_tmp is without resize
}

/**
 * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *
 * \param P 3D point coordinates
 * \param p return value, contains the image point coordinates
 */
void
OCAMCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const
{
    double norm    = std::sqrt( P[0] * P[0] + P[1] * P[1] );
    double theta   = std::atan2( -P[2], norm );
    double rho     = 0.0;
    double theta_i = 1.0;

    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
    {
        rho += theta_i * mParameters.inv_poly( i );
        theta_i *= theta;
    }

    double invNorm = 1.0 / norm;
    Eigen::Vector2d xn( P[0] * invNorm * rho, P[1] * invNorm * rho );

    p << xn[0] * mParameters.C( ) + xn[1] * mParameters.D( ) + mParameters.center_x( ),
    xn[0] * mParameters.E( ) + xn[1] + mParameters.center_y( );
}

void
OCAMCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, float image_scalse ) const
{
    Eigen::Vector2d p_tmp;
    spaceToPlane( P, p_tmp );
    p = p_tmp * image_scalse;
}

void
OCAMCamera::spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix< double, 2, 3 >& J ) const
{
    double norm    = std::sqrt( P[0] * P[0] + P[1] * P[1] );
    double theta   = std::atan2( -P[2], norm );
    double rho     = 0.0;
    double theta_i = 1.0;

    for ( int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
    {
        rho += theta_i * mParameters.inv_poly( i );
        theta_i *= theta;
    }

    double invNorm = 1.0 / norm;
    Eigen::Vector2d xn( P[0] * invNorm * rho, P[1] * invNorm * rho );

    p << xn[0] * mParameters.C( ) + xn[1] * mParameters.D( ) + mParameters.center_x( ),
    xn[0] * mParameters.E( ) + xn[1] + mParameters.center_y( );
}
/**
 * \brief Projects an undistorted 2D point p_u to the image plane
 *
 * \param p_u 2D point coordinates
 * \return image point coordinates
 */
void
OCAMCamera::undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const
{
    Eigen::Vector3d P( p_u[0], p_u[1], 1.0 );
    spaceToPlane( P, p );
}

void
OCAMCamera::initUndistortMap( cv::Mat& map1, cv::Mat& map2, double fScale ) const
{
    //    cv::Size imageSize(mParameters.imageWidth(),
    //    mParameters.imageHeight());

    //    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
    //    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);

    //    for (int v = 0; v < imageSize.height; ++v)
    //    {
    //        for (int u = 0; u < imageSize.width; ++u)
    //        {
    //            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
    //            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;

    //            double xi = mParameters.xi();
    //            double d2 = mx_u * mx_u + my_u * my_u;

    //            Eigen::Vector3d P;
    //            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0
    //            - xi * xi) * d2));

    //            Eigen::Vector2d p;
    //            spaceToPlane(P, p);

    //            mapX.at<float>(v,u) = p(0);
    //            mapY.at<float>(v,u) = p(1);
    //        }
    //    }

    //    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
}

cv::Mat
OCAMCamera::initUndistortRectifyMap(
cv::Mat& map1, cv::Mat& map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat ) const
{
    if ( imageSize == cv::Size( 0, 0 ) )
    {
        imageSize = cv::Size( mParameters.imageWidth( ), mParameters.imageHeight( ) );
    }

    cv::Mat mapX = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F );
    cv::Mat mapY = cv::Mat::zeros( imageSize.height, imageSize.width, CV_32F );

    Eigen::Matrix3f K_rect;

    K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy, cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1;

    if ( fx < 0 || fy < 0 )
    {
        throw std::string( std::string( __FUNCTION__ ) + ": Focal length must be specified" );
    }

    Eigen::Matrix3f K_rect_inv = K_rect.inverse( );

    Eigen::Matrix3f R, R_inv;
    cv::cv2eigen( rmat, R );
    R_inv = R.inverse( );

    for ( int v = 0; v < imageSize.height; ++v )
    {
        for ( int u = 0; u < imageSize.width; ++u )
        {
            Eigen::Vector3f xo;
            xo << u, v, 1;

            Eigen::Vector3f uo = R_inv * K_rect_inv * xo;

            Eigen::Vector2d p;
            spaceToPlane( uo.cast< double >( ), p );

            mapX.at< float >( v, u ) = p( 0 );
            mapY.at< float >( v, u ) = p( 1 );
        }
    }

    cv::convertMaps( mapX, mapY, map1, map2, CV_32FC1, false );

    cv::Mat K_rect_cv;
    cv::eigen2cv( K_rect, K_rect_cv );
    return K_rect_cv;
}

int
OCAMCamera::parameterCount( void ) const
{
    return SCARAMUZZA_CAMERA_NUM_PARAMS;
}

const OCAMCamera::Parameters&
OCAMCamera::getParameters( void ) const
{
    return mParameters;
}

void
OCAMCamera::setParameters( const OCAMCamera::Parameters& parameters )
{
    mParameters = parameters;

    m_inv_scale = 1.0 / ( parameters.C( ) - parameters.D( ) * parameters.E( ) );
}

void
OCAMCamera::readParameters( const std::vector< double >& parameterVec )
{
    if ( ( int )parameterVec.size( ) != parameterCount( ) )
    {
        return;
    }

    Parameters params = getParameters( );

    params.C( )        = parameterVec.at( 0 );
    params.D( )        = parameterVec.at( 1 );
    params.E( )        = parameterVec.at( 2 );
    params.center_x( ) = parameterVec.at( 3 );
    params.center_y( ) = parameterVec.at( 4 );
    for ( int i          = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        params.poly( i ) = parameterVec.at( 5 + i );
    for ( int i              = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        params.inv_poly( i ) = parameterVec.at( 5 + SCARAMUZZA_POLY_SIZE + i );

    setParameters( params );
}

void
OCAMCamera::writeParameters( std::vector< double >& parameterVec ) const
{
    parameterVec.resize( parameterCount( ) );
    parameterVec.at( 0 ) = mParameters.C( );
    parameterVec.at( 1 ) = mParameters.D( );
    parameterVec.at( 2 ) = mParameters.E( );
    parameterVec.at( 3 ) = mParameters.center_x( );
    parameterVec.at( 4 ) = mParameters.center_y( );
    for ( int i                  = 0; i < SCARAMUZZA_POLY_SIZE; i++ )
        parameterVec.at( 5 + i ) = mParameters.poly( i );
    for ( int i                                         = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++ )
        parameterVec.at( 5 + SCARAMUZZA_POLY_SIZE + i ) = mParameters.inv_poly( i );
}

void
OCAMCamera::writeParametersToYamlFile( const std::string& filename ) const
{
    mParameters.writeToYamlFile( filename );
}

std::string
OCAMCamera::parametersToString( void ) const
{
    std::ostringstream oss;
    oss << mParameters;

    return oss.str( );
}
}


// practice 3.1

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

struct convert123to234
{
	typedef mpl::vector_c<int,1,2,3> seq;

	struct add_1
	{
		template <class T>
		struct apply// : boost::add_pointer< T >
		{
			//typedef  type;
			//static const int value = T+1;
			//typedef typename mpl::int_<0> type;
			//typedef typename boost::add_pointer<T>::type type;
			//typedef mpl::int_<0> type;
			typedef mpl::vector_c<int,2,3,4> type;
			//BOOST_STATIC_ASSERT( 0 );
		};

		template<>
		struct apply<int>
		{
//			BOOST_STATIC_ASSERT( 0 );
//			typedef mpl::int_<2>::type type;
			typedef float type;
		};

		template<>
		struct apply< mpl::int_<4> >
		{
			typedef mpl::int_<4>::type type;
		};
	};

	typedef mpl::transform<
		seq,
		add_1 > convert;
};

template <class T>
struct test
{
	T a;
	BOOST_STATIC_ASSERT( 0 );
};

void main()
{
	//mpl::transform
	bool b1 = mpl::equal<
		convert123to234::convert::type,
		mpl::vector<int, mpl::int_<1>, mpl::int_<2>, mpl::int_<3>>
		//mpl::vector<int*,int*, float*, char*>
		//mpl::vector<mpl::int_<0>,mpl::int_<0>,mpl::int_<0>,mpl::int_<0>>
	>::value;

	bool b2 = mpl::equal<
			mpl::vector_c<int, 1,2,3>
			,	mpl::vector<int, mpl::int_<1>, mpl::int_<2>, mpl::int_<3> >
	>::value;

	bool b3 = mpl::equal<
				convert123to234::convert::type
			,	mpl::vector_c<int, 2,3,4>
	>::value;

	typedef mpl::transform<
		mpl::vector_c< int, 1, 2, 3 >::type,
		mpl::vector_c< int, 1, 1, 1 >::type,
		mpl::plus<_,_> >::type newType;

	bool b4 = mpl::equal<
			newType
		,	mpl::vector_c<int, 2,3,4>
	>::value;

}


#include <catch2/catch.hpp>
#include <mitama/mana/data/type_list.hpp>
#include <mitama/mana/algorithm/sliding.hpp>
#include <type_traits>
#include <iostream>
#include <boost/type_index.hpp>
namespace mana = mitama::mana;

TEST_CASE("sliding<2>", "[algorithm][chunk]") {
    REQUIRE(std::is_same_v<
        decltype(mana::sliding<2>(mana::type_list_c<int, int, double, double>)),
        mana::type_list<mana::type_list<int, int>,
                        mana::type_list<int, double>,
                        mana::type_list<double, double>
                        >>);
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <iostream>
#include <thread>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"

#include "point_to_point_tait_bryan_wc_jacobian.h"
#include "point_to_point_source_to_target_tait_bryan_wc_jacobian.h"

#include "rgd.h"

struct ScanPose{
	Eigen::Affine3d m;
	pcl::PointCloud<pcl::PointXYZ> pc;
};

pcl::PointCloud<pcl::PointXYZ> pc_ground_truth;
std::vector<ScanPose> scan_poses;
int current_scan_index = 0;

float sradius = 1.0;
bool show_ground_truth = true;

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 = 0.0, rotate_y = 0.0;
float translate_z = -50.0;
float translate_x, translate_y = 0.0;

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();
void set_initial_guess(std::vector<ScanPose>& scan_poses);

void split(std::string &str, char delim, std::vector<std::string> &out)
{
	size_t start;
	size_t end = 0;

	while ((start = str.find_first_not_of(delim, end)) != std::string::npos)
	{
		end = str.find(delim, start);
		out.push_back(str.substr(start, end - start));
	}
}

std::vector<std::pair<int,int>> nns(ScanPose &sp1, ScanPose &sp2, float radius);

std::vector<std::pair<int,int>> pairs_temp;
std::vector<Bucket> buckets_render;
bool show_ndt_covariances = true;

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

	std::vector<std::string> pcd_file_names;
	pcd_file_names.push_back("../data/pcd/scan000.pcd");
	pcd_file_names.push_back("../data/pcd/scan001.pcd");
	pcd_file_names.push_back("../data/pcd/scan002.pcd");
	pcd_file_names.push_back("../data/pcd/scan003.pcd");
	pcd_file_names.push_back("../data/pcd/scan004.pcd");
	pcd_file_names.push_back("../data/pcd/scan005.pcd");
	pcd_file_names.push_back("../data/pcd/scan006.pcd");
	pcd_file_names.push_back("../data/pcd/scan007.pcd");
	pcd_file_names.push_back("../data/pcd/scan008.pcd");
	pcd_file_names.push_back("../data/pcd/scan009.pcd");
	pcd_file_names.push_back("../data/pcd/scan010.pcd");
	pcd_file_names.push_back("../data/pcd/scan011.pcd");
	pcd_file_names.push_back("../data/pcd/scan012.pcd");

	for(size_t i = 0; i < pcd_file_names.size(); i++){
		std::cout << "loading file: " << pcd_file_names[i] << std::endl;
		ScanPose sp;
		sp.m = Eigen::Affine3d::Identity();
		pcl::PointCloud<pcl::PointXYZ> pc;
		if (pcl::io::loadPCDFile(pcd_file_names[i], pc) == -1) {
			std::cout << "PROBLEM WITH LODAING pcd: " << pcd_file_names[i] << std::endl;
			return 1;
		}else{
			sp.pc = pc;
			scan_poses.push_back(sp);
		}
	}

	if (pcl::io::loadPCDFile("../data/pcd/ground_truth.pcd", pc_ground_truth) == -1) {
		std::cout << "PROBLEM WITH LODAING pcd: ../data/pcd/ground_truth.pcd" << std::endl;
		return 1;
	}

	set_initial_guess(scan_poses);

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

	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("point_cloud_registration");
	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;
}

void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd  = 3)
{

    Eigen::LLT<Eigen::Matrix<double,3,3> > cholSolver(covar);
    Eigen::Matrix3d transform = cholSolver.matrixL();

    const double pi = 3.141592;
    const double di =0.02;
    const double dj =0.04;
    const double du =di*2*pi;
    const double dv =dj*pi;
    glColor3f(color.x(), color.y(),color.z());

    for (double i = 0; i < 1.0; i+=di)  //horizonal
    {
        for (double j = 0; j < 1.0; j+=dj)  //vertical
        {
            double u = i*2*pi;      //0     to  2pi
            double v = (j-0.5)*pi;  //-pi/2 to pi/2

            const Eigen::Vector3d pp0( cos(v)* cos(u),cos(v) * sin(u),sin(v));
            const Eigen::Vector3d pp1(cos(v) * cos(u + du) ,cos(v) * sin(u + du) ,sin(v));
            const Eigen::Vector3d pp2(cos(v + dv)* cos(u + du) ,cos(v + dv)* sin(u + du) ,sin(v + dv));
            const Eigen::Vector3d pp3( cos(v + dv)* cos(u),cos(v + dv)* sin(u),sin(v + dv));
            Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
            Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
            Eigen::Vector3d tp2 = transform * (nstd*pp2) + mean;
            Eigen::Vector3d tp3 = transform * (nstd*pp3) + mean;

            glBegin(GL_LINE_LOOP);
            glVertex3dv(tp0.data());
            glVertex3dv(tp1.data());
            glVertex3dv(tp2.data());
            glVertex3dv(tp3.data());
            glEnd();
        }
    }
}

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);

	glBegin(GL_LINES);
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(1.0f, 0.0f, 0.0f);

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 1.0f, 0.0f);

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	glBegin(GL_POINTS);
	glColor3f(1.0, 0.0, 0.0);
	for(size_t i = 0; i < scan_poses.size(); i++){
		if(i == current_scan_index){
			glColor3f(0.0, 1.0, 0.0);
		}else{
			glColor3f(1.0, 0.0, 0.0);
		}
		if(i+1 == current_scan_index){
			glColor3f(0.0, 0.0, 1.0);
		}
		for(size_t j = 0; j < scan_poses[i].pc.size(); j++){
			Eigen::Vector3d v(scan_poses[i].pc[j].x, scan_poses[i].pc[j].y, scan_poses[i].pc[j].z);
			Eigen::Vector3d vt = scan_poses[i].m * v;
			glVertex3f(vt.x(), vt.y(), vt.z());
		}
	}
	glEnd();

	glColor3f(0,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0 ; i < pairs_temp.size(); i++){
		pcl::PointXYZ p1 = scan_poses[0].pc[pairs_temp[i].first];
		pcl::PointXYZ p2 = scan_poses[1].pc[pairs_temp[i].second];

		Eigen::Vector3d v1(p1.x, p1.y, p1.z);
		Eigen::Vector3d v1t = scan_poses[0].m * v1;

		glVertex3f(v1t.x(), v1t.y(), v1t.z());

		Eigen::Vector3d v2(p2.x, p2.y, p2.z);
		Eigen::Vector3d v2t = scan_poses[1].m * v2;

		glVertex3f(v2t.x(), v2t.y(), v2t.z());

	}
	glEnd();

	if(show_ground_truth){
		glColor3f(0.7, 0.7, 0.7);
		glBegin(GL_POINTS);
		for(size_t i = 0; i < pc_ground_truth.size(); i++){
			glVertex3f(pc_ground_truth[i].x, pc_ground_truth[i].y, pc_ground_truth[i].z);
		}
		glEnd();
	}

	if(show_ndt_covariances){
		for(size_t i = 0 ; i < buckets_render.size(); i++){
			if(buckets_render[i].number_of_points > 10){
				draw_ellipse(buckets_render[i].cov, buckets_render[i].mean, Eigen::Vector3f(0.0, 0.0, 1.0), 1);
			}
		}
	}

	glutSwapBuffers();
}

void ndt_job(int i, Job* job, std::vector<Bucket>* buckets, Eigen::SparseMatrix<double > *AtPA,
		Eigen::SparseMatrix<double > *AtPB, std::vector<PointBucketIndexPair> *index_pair_internal, std::vector<Point3D> *pp,
		std::vector<TaitBryanPose> *poses, std::vector<Eigen::Affine3d> *mposes_inv, size_t trajectory_size) {

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

	for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) {
		Bucket& b = (*buckets)[ii];
		if (b.number_of_points < 5)continue;

		Eigen::Vector3d mean(0, 0, 0);
		Eigen::Matrix3d cov;
		cov.setZero();

		for (int index = b.index_begin; index < b.index_end; index++) {
			const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point];
			mean += Eigen::Vector3d(p.x, p.y, p.z);
		}
		mean /= b.number_of_points;

		for (int index = b.index_begin; index < b.index_end; index++) {
			const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point];
			cov(0, 0) += (mean.x() - p.x) * (mean.x() - p.x);
			cov(0, 1) += (mean.x() - p.x) * (mean.y() - p.y);
			cov(0, 2) += (mean.x() - p.x) * (mean.z() - p.z);
			cov(1, 0) += (mean.y() - p.y) * (mean.x() - p.x);
			cov(1, 1) += (mean.y() - p.y) * (mean.y() - p.y);
			cov(1, 2) += (mean.y() - p.y) * (mean.z() - p.z);
			cov(2, 0) += (mean.z() - p.z) * (mean.x() - p.x);
			cov(2, 1) += (mean.z() - p.z) * (mean.y() - p.y);
			cov(2, 2) += (mean.z() - p.z) * (mean.z() - p.z);
		}
		cov /= b.number_of_points;

		(*buckets)[ii].mean = mean;
		(*buckets)[ii].cov = cov;

		Eigen::Matrix3d infm = cov.inverse();

		if (!(infm(0, 0) == infm(0, 0)))continue;
		if (!(infm(0, 1) == infm(0, 1)))continue;
		if (!(infm(0, 2) == infm(0, 2)))continue;

		if (!(infm(1, 0) == infm(1, 0)))continue;
		if (!(infm(1, 1) == infm(1, 1)))continue;
		if (!(infm(1, 2) == infm(1, 2)))continue;

		if (!(infm(2, 0) == infm(2, 0)))continue;
		if (!(infm(2, 1) == infm(2, 1)))continue;
		if (!(infm(2, 2) == infm(2, 2)))continue;

		for (int index = b.index_begin; index < b.index_end; index++) {
			const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point];

			Eigen::Vector3d point_local(p.x, p.y, p.z);
			point_local = (*mposes_inv)[p.index_pose] * point_local;

			TaitBryanPose pose_s = (*poses)[p.index_pose];
			double delta_x;
			double delta_y;
			double delta_z;

			point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z,
				pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka,
				point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z());

			Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
			point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian,
				pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka,
				point_local.x(), point_local.y(), point_local.z());

			int ir = tripletListB.size();
			int c = p.index_pose * 6;

			for (int row = 0; row < 3; row++) {
				for (int col = 0; col < 6; col++) {
					if (jacobian(row, col) != 0.0) {
						tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col));
					}
				}
			}

			tripletListP.emplace_back(ir, ir, infm(0, 0));
			tripletListP.emplace_back(ir, ir + 1, infm(0, 1));
			tripletListP.emplace_back(ir, ir + 2, infm(0, 2));
			tripletListP.emplace_back(ir + 1, ir, infm(1, 0));
			tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1));
			tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2));
			tripletListP.emplace_back(ir + 2, ir, infm(2, 0));
			tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1));
			tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2));

			tripletListB.emplace_back(ir, 0, delta_x);
			tripletListB.emplace_back(ir + 1, 0, delta_y);
			tripletListB.emplace_back(ir + 2, 0, delta_z);
		}
	}

	Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory_size * 6);
	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> AtPAt(trajectory_size * 6, trajectory_size * 6);
	Eigen::SparseMatrix<double> AtPBt(trajectory_size * 6, 1);

	{
		Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
		AtPAt = AtP * matA;
		AtPBt = AtP * matB;

		(*AtPA) = AtPAt;
		(*AtPB) = AtPBt;
	}
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case '-':{
			current_scan_index --;
			if(current_scan_index < 0)current_scan_index = 0;
			break;
		}
		case '=':{
			current_scan_index ++;
			if(current_scan_index >= scan_poses.size())current_scan_index = scan_poses.size() - 1;
			break;
		}
		case 'a':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = -0.1;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'd':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = 0.1;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'w':{
			TaitBryanPose pose;
			pose.px = 0.1;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 's':{
			TaitBryanPose pose;
			pose.px = -0.1;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'z':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = -0.01;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'x':{
			TaitBryanPose pose;
			pose.px = 0;
			pose.py = 0;
			pose.pz = 0;
			pose.om = 0;
			pose.fi = 0;
			pose.ka = 0.01;
			scan_poses[current_scan_index].m = scan_poses[current_scan_index].m * affine_matrix_from_pose_tait_bryan(pose);
			break;
		}
		case 'p':{
			for(size_t i = 0; i < scan_poses.size(); i++){
				std::cout << "scan: " << i << std::endl;
				std::cout << scan_poses[i].m.matrix() << std::endl;
			}
			break;
		}
		case 'n':{
			pairs_temp = nns(scan_poses[0], scan_poses[1], sradius);
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < scan_poses.size() ; i++){
				for(size_t j = i+1 ; j < scan_poses.size() ; j++){
					//if(i == j)continue;
					std::vector<std::pair<int,int>> nn = nns(scan_poses[i], scan_poses[j], sradius);
					std::cout << nn.size() << "," << scan_poses[i].pc.size() << "," << scan_poses[j].pc.size() << std::endl;

					TaitBryanPose pose_1 = pose_tait_bryan_from_affine_matrix(scan_poses[i].m);
					TaitBryanPose pose_2 = pose_tait_bryan_from_affine_matrix(scan_poses[j].m);

					for(size_t k = 0 ; k < nn.size(); k+=1){
						pcl::PointXYZ &p_1 = scan_poses[i].pc[nn[k].first];
						pcl::PointXYZ &p_2 = scan_poses[j].pc[nn[k].second];
						double delta_x;
						double delta_y;
						double delta_z;
						point_to_point_tait_bryan_wc(delta_x, delta_y, delta_z, pose_1.px, pose_1.py, pose_1.pz, pose_1.om, pose_1.fi, pose_1.ka, pose_2.px, pose_2.py, pose_2.pz, pose_2.om, pose_2.fi, pose_2.ka, p_1.x, p_1.y, p_1.z, p_2.x, p_2.y, p_2.z);

						Eigen::Matrix<double, 3, 12, Eigen::RowMajor> jacobian;
						point_to_point_tait_bryan_wc_jacobian(jacobian, pose_1.px, pose_1.py, pose_1.pz, pose_1.om, pose_1.fi, pose_1.ka, pose_2.px, pose_2.py, pose_2.pz, pose_2.om, pose_2.fi, pose_2.ka, p_1.x, p_1.y, p_1.z, p_2.x, p_2.y, p_2.z);

						int ir = tripletListB.size();
						int ic_1 = i * 6;
						int ic_2 = j * 6;

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

						tripletListA.emplace_back(ir     , ic_2    , -jacobian(0,6));
						tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,7));
						tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,8));
						tripletListA.emplace_back(ir     , ic_2 + 3, -jacobian(0,9));
						tripletListA.emplace_back(ir     , ic_2 + 4, -jacobian(0,10));
						tripletListA.emplace_back(ir     , ic_2 + 5, -jacobian(0,11));

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

						tripletListA.emplace_back(ir + 1 , ic_2    , -jacobian(1,6));
						tripletListA.emplace_back(ir + 1 , ic_2 + 1, -jacobian(1,7));
						tripletListA.emplace_back(ir + 1 , ic_2 + 2, -jacobian(1,8));
						tripletListA.emplace_back(ir + 1 , ic_2 + 3, -jacobian(1,9));
						tripletListA.emplace_back(ir + 1 , ic_2 + 4, -jacobian(1,10));
						tripletListA.emplace_back(ir + 1 , ic_2 + 5, -jacobian(1,11));

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

						tripletListA.emplace_back(ir + 2 , ic_2    , -jacobian(2,6));
						tripletListA.emplace_back(ir + 2 , ic_2 + 1, -jacobian(2,7));
						tripletListA.emplace_back(ir + 2 , ic_2 + 2, -jacobian(2,8));
						tripletListA.emplace_back(ir + 2 , ic_2 + 3, -jacobian(2,9));
						tripletListA.emplace_back(ir + 2 , ic_2 + 4, -jacobian(2,10));
						tripletListA.emplace_back(ir + 2 , ic_2 + 5, -jacobian(2,11));

						tripletListP.emplace_back(ir    , ir    ,  1);//cauchy(delta_x, 1));
						tripletListP.emplace_back(ir + 1, ir + 1,  1);//cauchy(delta_y, 1));
						tripletListP.emplace_back(ir + 2, ir + 2,  1);//cauchy(delta_z, 1));

						tripletListB.emplace_back(ir    , 0,  delta_x);
						tripletListB.emplace_back(ir + 1, 0,  delta_y);
						tripletListB.emplace_back(ir + 2, 0,  delta_z);
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), scan_poses.size() * 6);
			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(scan_poses.size() * 6, scan_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(scan_poses.size() * 6, 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() == scan_poses.size() * 6){
				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;

				for(size_t i = 0; i < scan_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(scan_poses[i].m);
					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++];

					scan_poses[i].m = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'l':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

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

				for(size_t j = 0 ; j < scan_poses.size() ; j++){
					if(i == j)continue;
					std::vector<std::pair<int,int>> nn = nns(scan_poses[i], scan_poses[j], sradius);
					std::cout << nn.size() << "," << scan_poses[i].pc.size() << "," << scan_poses[j].pc.size() << std::endl;

					for(size_t k = 0 ; k < nn.size(); k+=1){
						pcl::PointXYZ &p_1 = scan_poses[i].pc[nn[k].first];
						pcl::PointXYZ &p_2 = scan_poses[j].pc[nn[k].second];

						Eigen::Vector3d p_t(p_2.x, p_2.y, p_2.z);// = trajectory[j] * p_2;
						Eigen::Vector3d p_s(p_1.x, p_1.y, p_1.z);// = p_1;

						int ir = tripletListB.size();
						int ic_1 = i * 6;
						Eigen::Matrix3d px;
						px(0,0) = 0;
						px(0,1) = -p_s.z();
						px(0,2) =  p_s.y();
						px(1,0) = p_s.z();
						px(1,1) = 0;
						px(1,2) = -p_s.x();
						px(2,0) = -p_s.y();
						px(2,1) = p_s.x();
						px(2,2) = 0;

						Eigen::Matrix3d R = pose_source.inverse().rotation();
						Eigen::Matrix3d Rpx = R*px;

						tripletListA.emplace_back(ir     ,ic_1 + 0, R(0,0));
						tripletListA.emplace_back(ir     ,ic_1 + 1, R(0,1));
						tripletListA.emplace_back(ir     ,ic_1 + 2, R(0,2));
						tripletListA.emplace_back(ir     ,ic_1 + 3, -Rpx(0,0));
						tripletListA.emplace_back(ir     ,ic_1 + 4, -Rpx(0,1));
						tripletListA.emplace_back(ir     ,ic_1 + 5, -Rpx(0,2));

						tripletListA.emplace_back(ir + 1 ,ic_1 + 0, R(1,0));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 1, R(1,1));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 2, R(1,2));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 3, -Rpx(1,0));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 4, -Rpx(1,1));
						tripletListA.emplace_back(ir + 1 ,ic_1 + 5, -Rpx(1,2));

						tripletListA.emplace_back(ir + 2 ,ic_1 + 0, R(2,0));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 1, R(2,1));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 2, R(2,2));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 3, -Rpx(2,0));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 4, -Rpx(2,1));
						tripletListA.emplace_back(ir + 2 ,ic_1 + 5, -Rpx(2,2));

						tripletListP.emplace_back(ir    , ir    ,  1);
						tripletListP.emplace_back(ir + 1, ir + 1,  1);
						tripletListP.emplace_back(ir + 2, ir + 2,  1);

						Eigen::Vector3d target = scan_poses[i].m.inverse() * (scan_poses[j].m * p_t);

						tripletListB.emplace_back(ir    , 0,  -(target.x() - p_s.x()));
						tripletListB.emplace_back(ir + 1, 0,  -(target.y() - p_s.y()));
						tripletListB.emplace_back(ir + 2, 0,  -(target.z() - p_s.z()));
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), scan_poses.size() * 6);
			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(scan_poses.size() * 6, scan_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(scan_poses.size() * 6, 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() == scan_poses.size() * 6){
				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;

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

					scan_poses[i].m = (scan_poses[i].m.inverse() * affine_matrix_from_pose_rodrigues(pose_update)).inverse();
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case '1':{
			sradius -= 0.01;
			std::cout << "sradius: " << sradius << std::endl;
			if(sradius < 0)sradius = 0.01;
			break;
		}
		case '2':{
			sradius += 0.01;
			std::cout << "sradius: " << sradius << std::endl;
			break;
		}
		case '3':{
			for(size_t i = 0; i < scan_poses.size(); i++){
				pcl::PointCloud<pcl::PointXYZ> pc;
				for(size_t j = 0; j < scan_poses[i].pc.size(); j++){
					Eigen::Vector3d v(scan_poses[i].pc[j].x, scan_poses[i].pc[j].y, scan_poses[i].pc[j].z);
					Eigen::Vector3d vt = scan_poses[i].m * v;
					pcl::PointXYZ p;
					p.x = vt.x();
					p.y = vt.y();
					p.z = vt.z();
					pc.push_back(p);
				}
				pcl::io::savePCDFileBinary(std::to_string(i) + ".pcd", pc);
				std::cout << "file: " << std::to_string(i) + ".pcd" << std::endl;
 			}
			break;
		}
		case 'g':{
			show_ground_truth =! show_ground_truth;
			break;
		}
		case 'o':{
			GridParameters rgd_params;
			rgd_params.resolution_X = sradius;
			rgd_params.resolution_Y = sradius;
			rgd_params.resolution_Z = sradius;
			rgd_params.bounding_box_extension = sradius;

			std::vector<Point3D> points_global;
			for(size_t i = 0; i < scan_poses.size(); i++){
				for(size_t j = 0; j < scan_poses[i].pc.size(); j++){
					Eigen::Vector3d v(scan_poses[i].pc[j].x, scan_poses[i].pc[j].y, scan_poses[i].pc[j].z);
					Eigen::Vector3d vt = scan_poses[i].m * v;
					Point3D p;
					p.x = vt.x();
					p.y = vt.y();
					p.z = vt.z();
					p.index_pose = i;
					points_global.push_back(p);
				}
			}

			std::vector<PointBucketIndexPair> index_pair;
			std::vector<Bucket> buckets;

			grid_calculate_params(points_global, rgd_params);
			build_rgd(points_global, index_pair, buckets, rgd_params, 8);

			std::vector<Job> jobs = get_jobs(buckets.size(), 8);

			std::vector<std::thread> threads;

			std::vector<Eigen::SparseMatrix<double>> AtPAtmp(jobs.size());
			std::vector<Eigen::SparseMatrix<double>> AtPBtmp(jobs.size());

			for (size_t i = 0; i < jobs.size(); i++) {
				AtPAtmp[i] = Eigen::SparseMatrix<double>(scan_poses.size() * 6, scan_poses.size() * 6);
				AtPBtmp[i] = Eigen::SparseMatrix<double>(scan_poses.size() * 6, 1);
			}

			std::vector<TaitBryanPose> poses;
			std::vector<Eigen::Affine3d> mposes_inv;
			for(size_t i = 0; i < scan_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(scan_poses[i].m));
				mposes_inv.push_back(scan_poses[i].m.inverse());
			}

			for (size_t k = 0; k < jobs.size(); k++) {
				threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), &index_pair, &points_global, &poses, &mposes_inv, scan_poses.size()));
			}

			for (size_t j = 0; j < threads.size(); j++) {
				threads[j].join();
			}

			bool init = false;
			Eigen::SparseMatrix<double> AtPA_ndt(scan_poses.size() * 6, scan_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB_ndt(scan_poses.size() * 6, 1);

			for (size_t k = 0; k < jobs.size(); k++) {
				if (!init) {
					if (AtPBtmp[k].size() > 0) {
						AtPA_ndt = AtPAtmp[k];
						AtPB_ndt = AtPBtmp[k];
						init = true;
					}
				}
				else {
					if (AtPBtmp[k].size() > 0) {

						AtPA_ndt += AtPAtmp[k];
						AtPB_ndt += AtPBtmp[k];
					}
				}
			}
			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA_ndt);

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

			std::vector<double> h_x;

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

			if(h_x.size() == scan_poses.size() * 6){
				buckets_render = buckets;
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				int counter = 0;
				for (size_t i = 0; i < scan_poses.size(); i++) {

					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(scan_poses[i].m);

					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++];

					scan_poses[i].m = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}

			break;
		}
		case '4':{
			show_ndt_covariances = !show_ndt_covariances;
			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 << "-: current_scan_index--" << std::endl;
	std::cout << "=: current_scan_index++" << std::endl;
	std::cout << "awsdzx: move current_scan (green)" << std::endl;
	std::cout << "p: print poses" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "l: optimize (Lie algebra)" << std::endl;
	std::cout << "1: sradius -= 0.01" << std::endl;
	std::cout << "2: sradius += 0.01" << std::endl;
	std::cout << "3: save current point clouds" << std::endl;
	std::cout << "g: show_ground_truth =! show_ground_truth" << std::endl;
	std::cout << "o: optimize (NDT)" << std::endl;
	std::cout << "n: nns" << std::endl;
	std::cout << "4: show ndt covariances on/off" << std::endl;
}

void set_initial_guess(std::vector<ScanPose>& scan_poses){
	scan_poses[0].m(0,0) = 0.380925;
	scan_poses[0].m(0,1) = 0.924606;
	scan_poses[0].m(0,3) = -6.06303;

	scan_poses[0].m(1,0) = -0.924606;
	scan_poses[0].m(1,1) = 0.380925;
	scan_poses[0].m(1,3) = -8.2396;

	scan_poses[1].m(0,0) = 0.710913;
	scan_poses[1].m(0,1) = 0.70328;
	scan_poses[1].m(0,3) = -1.03452;

	scan_poses[1].m(1,0) = -0.70328;
	scan_poses[1].m(1,1) = 0.710913;
	scan_poses[1].m(1,3) = -7.77656;

	scan_poses[2].m(0,0) = 0.613745;
	scan_poses[2].m(0,1) = 0.789504;
	scan_poses[2].m(0,3) = 3.99851;

	scan_poses[2].m(1,0) = -0.789504;
	scan_poses[2].m(1,1) = 0.613745;
	scan_poses[2].m(1,3) = -8.18709;

	scan_poses[3].m(0,0) = 0.751805;
	scan_poses[3].m(0,1) = 0.659385;
	scan_poses[3].m(0,3) = 9.99224;

	scan_poses[3].m(1,0) = -0.659385;
	scan_poses[3].m(1,1) = 0.751805;
	scan_poses[3].m(1,3) = -8.54882;

	scan_poses[4].m(0,0) = -0.996673;
	scan_poses[4].m(0,1) = 0.0815022;
	scan_poses[4].m(0,3) = 12.6963;

	scan_poses[4].m(1,0) = -0.0815022;
	scan_poses[4].m(1,1) = -0.996673;
	scan_poses[4].m(1,3) = -8.73861;

	scan_poses[5].m(0,0) = 0.639602;
	scan_poses[5].m(0,1) = 0.768705;
	scan_poses[5].m(0,3) = 17.8592;

	scan_poses[5].m(1,0) = -0.768705;
	scan_poses[5].m(1,1) = 0.639602;
	scan_poses[5].m(1,3) = -2.78423;

	scan_poses[6].m(0,0) = 0.745174;
	scan_poses[6].m(0,1) = 0.66687;
	scan_poses[6].m(0,3) = 22.9195;

	scan_poses[6].m(1,0) = -0.66687;
	scan_poses[6].m(1,1) = 0.745174;
	scan_poses[6].m(1,3) = -1.98392;

	scan_poses[7].m(0,0) = -0.0491839;
	scan_poses[7].m(0,1) = 0.998789;
	scan_poses[7].m(0,3) = 31.7827;

	scan_poses[7].m(1,0) = -0.998789;
	scan_poses[7].m(1,1) = -0.0491839;
	scan_poses[7].m(1,3) = -2.2143;

	scan_poses[8].m(0,0) = -0.128844;
	scan_poses[8].m(0,1) = 0.991665;
	scan_poses[8].m(0,3) = 39.0272;

	scan_poses[8].m(1,0) = -0.991665;
	scan_poses[8].m(1,1) = -0.128844;
	scan_poses[8].m(1,3) = -2.29705;

	scan_poses[9].m(0,0) = -0.34215;
	scan_poses[9].m(0,1) = 0.939646;
	scan_poses[9].m(0,3) = 48.1018;

	scan_poses[9].m(1,0) = -0.939646;
	scan_poses[9].m(1,1) = -0.34215;
	scan_poses[9].m(1,3) = -1.94245;

	scan_poses[10].m(0,0) = -0.158532;
	scan_poses[10].m(0,1) = 0.987354;
	scan_poses[10].m(0,3) = 54.2044;

	scan_poses[10].m(1,0) = -0.987354;
	scan_poses[10].m(1,1) = -0.158532;
	scan_poses[10].m(1,3) = -7.96743;

	scan_poses[11].m(0,0) = -0.197888;
	scan_poses[11].m(0,1) = 0.980225;
	scan_poses[11].m(0,3) = 65.5777;

	scan_poses[11].m(1,0) = -0.980225;
	scan_poses[11].m(1,1) = -0.197888;
	scan_poses[11].m(1,3) = -8.39231;

	scan_poses[12].m(0,0) = -0.360872;
	scan_poses[12].m(0,1) = 0.932615;
	scan_poses[12].m(0,3) = 78.1712;

	scan_poses[12].m(1,0) = -0.932615;
	scan_poses[12].m(1,1) = -0.360872;
	scan_poses[12].m(1,3) = -7.76261;
}

std::vector<std::pair<int,int>> nns(ScanPose &sp1, ScanPose &sp2, float radius)
{
	pcl::PointCloud<pcl::PointXYZ> pc1;
	pcl::PointCloud<pcl::PointXYZ> pc2;

	for(size_t i = 0; i < sp1.pc.size(); i++){
		Eigen::Vector3d v(sp1.pc[i].x, sp1.pc[i].y, sp1.pc[i].z);
		Eigen::Vector3d vt = sp1.m * v;
		pc1.push_back(pcl::PointXYZ(vt.x(), vt.y(), vt.z()));
	}

	for(size_t i = 0 ; i < sp2.pc.size(); i++){
		Eigen::Vector3d v(sp2.pc[i].x, sp2.pc[i].y, sp2.pc[i].z);
		Eigen::Vector3d vt = sp2.m * v;
		pc2.push_back(pcl::PointXYZ(vt.x(), vt.y(), vt.z()));
	}

	std::vector<std::pair<int,int>> result;

	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	for(size_t i = 0; i < pc1.size(); i++){
		cloud->push_back(pc1[i]);
	}
	int K = 1;

	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;

	kdtree.setInputCloud (cloud);
	for(size_t k = 0; k < pc2.size(); k++){
		if ( kdtree.radiusSearch (pc2[k], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
			for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
				result.emplace_back(pointIdxRadiusSearch[i], k);
				break;
			}
		}
	}

	return result;
}