#include <NTL/ZZX.h>
#include <NTL/ZZ_pX.h>
#include <NTL/lzz_pX.h>
#include <NTL/GF2X.h>

#include <NTL/version.h>

NTL_CLIENT

#define make_string_aux(x) #x
#define make_string(x) make_string_aux(x)

int SmallModulusTest(long p, long n)
{
   zz_pBak bak;

   bak.save();

   zz_p::init(p);

   zz_pX a, b, c, cc;

   random(a, n);
   random(b, n);
   PlainMul(c, a, b);
   FFTMul(cc, a, b);

   int res;
   res = (c != cc);

   bak.restore();

   return res;
}

int GF2X_test()
{
   GF2X a, b, c, c1;

   long n;

#ifdef NTL_GF2X_LIB
   for (n = 32; n <= (1L << 18); n = n << 1) {
      random(a, n);
      random(b, n);
      OldMul(c, a, b);
      mul(c1, a, b);
      if (c1 != c) return 1;
   }
#endif

   return 0;
}

void GF2X_time()
{
   long iter;

   GF2X a, b, c;

   double t;
   long i;

   for (long n = 1000; n <= 1000000L; n *= 1000) {

      random(a, n);
      random(b, n);

      mul(c, a, b);

      iter = 0;
      do {
	 iter = iter ? (2*iter) : 1;
	 t = GetTime();
	 for (i = 0; i < iter; i++)
	    mul(c, a, b);
	 t = GetTime() - t;
      } while (t < 0.5);

      cerr << "time to multiply polynomials over GF(2) \n   of degree < " << n << " : "
	   << (t/iter) << "s";

#ifdef NTL_GF2X_LIB
      OldMul(c, a, b);

      iter = 0;
      do {
	 iter = iter ? (2*iter) : 1;
	 t = GetTime();
	 for (i = 0; i < iter; i++)
	    OldMul(c, a, b);
	 t = GetTime() - t;
      } while (t < 0.5);

      cerr << " [old code: "  << (t/iter) << "s]";
#endif
      cerr << "\n";
   }

}

ZZX KarMul(const ZZX& a, const ZZX& b)
{
   ZZX res;
   KarMul(res, a, b);
   return res;
}

int main()
{
   SetSeed(ZZ(0));

   cerr << "This is NTL version " << NTL_VERSION << "\n";

   cerr << "Hardware charactersitics:\n";
   cerr << "NTL_BITS_PER_LONG = " << NTL_BITS_PER_LONG << "\n";
   cerr << "NTL_ZZ_NBITS = " << NTL_ZZ_NBITS << "\n";
   cerr << "NTL_SP_NBITS = " << NTL_SP_NBITS << "\n";

#ifdef NTL_LONGDOUBLE_SP_MULMOD
   cerr << "NTL_LONGDOUBLE_SP_MULMOD\n";
#endif

#ifdef NTL_LONGLONG_SP_MULMOD
   cerr << "NTL_LONGLONG_SP_MULMOD\n";
#endif

#include <NTL/REPORT_ALL_FEATURES.h>

   cerr << "\n";

   cerr << "Basic Configuration Options:\n";

#ifdef NTL_LEGACY_NO_NAMESPACE
   cerr << "NTL_LEGACY_NO_NAMESPACE\n";
#endif

#ifdef NTL_LEGACY_INPUT_ERROR
   cerr << "NTL_LEGACY_INPUT_ERROR\n";
#endif

#ifdef NTL_THREADS
   cerr << "NTL_THREADS\n";
#endif

#ifdef NTL_TLS_HACK
   cerr << "NTL_TLS_HACK\n";
#endif

#ifdef NTL_EXCEPTIONS
   cerr << "NTL_EXCEPTIONS\n";
#endif

#ifdef NTL_THREAD_BOOST
   cerr << "NTL_THREAD_BOOST\n";
#endif

#ifdef NTL_LEGACY_SP_MULMOD
   cerr << "NTL_LEGACY_SP_MULMOD\n";
#endif

#ifdef NTL_DISABLE_LONGDOUBLE
   cerr << "NTL_DISABLE_LONGDOUBLE\n";
#endif

#ifdef NTL_DISABLE_LONGLONG
   cerr << "NTL_DISABLE_LONGLONG\n";
#endif

#ifdef NTL_DISABLE_LL_ASM
   cerr << "NTL_DISABLE_LL_ASM\n";
#endif

#ifdef NTL_MAXIMIZE_SP_NBITS
   cerr << "NTL_MAXIMIZE_SP_NBITS\n";
#endif

#ifdef NTL_GMP_LIP
   cerr << "NTL_GMP_LIP\n";
#endif

#ifdef NTL_GF2X_LIB
   cerr << "NTL_GF2X_LIB\n";
#endif

#ifdef NTL_STD_CXX11
   cerr << "NTL_STD_CXX11\n";
#endif

#ifdef NTL_STD_CXX14
   cerr << "NTL_STD_CXX14\n";
#endif

#ifdef NTL_DISABLE_MOVE_ASSIGN
   cout << "NTL_DISABLE_MOVE_ASSIGN\n";
#endif

#ifdef NTL_DISABLE_MOVE
   cout << "NTL_DISABLE_MOVE\n";
#endif

#ifdef NTL_UNSIGNED_LONG_LONG_TYPE
   cerr << "NTL_UNSIGNED_LONG_LONG_TYPE: ";
   cerr << make_string(NTL_UNSIGNED_LONG_LONG_TYPE) << "\n";
#endif

#ifdef NTL_X86_FIX
   cerr << "NTL_X86_FIX\n";
#endif

#ifdef NTL_NO_X86_FIX
   cerr << "NTL_NO_X86_FIX\n";
#endif

#ifdef NTL_NO_INIT_TRANS
   cerr << "NTL_NO_INIT_TRANS\n";
#endif

#ifdef NTL_CLEAN_INT
   cerr << "NTL_CLEAN_INT\n";
#endif

#ifdef NTL_CLEAN_PTR
   cerr << "NTL_CLEAN_PTR\n";
#endif

#ifdef NTL_SAFE_VECTORS
   cerr << "NTL_SAFE_VECTORS\n";
#endif

#ifdef NTL_RANGE_CHECK
   cerr << "NTL_RANGE_CHECK\n";
#endif

cerr << "\n";
cerr << "Resolution of double-word type:\n";
cerr << make_string(NTL_ULL_TYPE) << "\n";

cerr << "\n";
cerr << "Performance Options:\n";

#ifdef NTL_SPMM_ULL
   cerr << "NTL_SPMM_ULL\n";
#endif

#ifdef NTL_AVOID_BRANCHING
   cerr << "NTL_AVOID_BRANCHING\n";
#endif

#ifdef NTL_FFT_BIGTAB
   cerr << "NTL_FFT_BIGTAB\n";
#endif

#ifdef NTL_FFT_LAZYMUL
   cerr << "NTL_FFT_LAZYMUL\n";
#endif

#ifdef NTL_TBL_REM
   cerr << "NTL_TBL_REM\n";
#endif

#ifdef NTL_CRT_ALTCODE
   cerr << "NTL_CRT_ALTCODE\n";
#endif

#ifdef NTL_CRT_ALTCODE_SMALL
   cerr << "NTL_CRT_ALTCODE_SMALL\n";
#endif

#ifdef NTL_GF2X_ALTCODE
   cerr << "NTL_GF2X_ALTCODE\n";
#endif

#ifdef NTL_GF2X_ALTCODE1
   cerr << "NTL_GF2X_ALTCODE1\n";
#endif

#ifdef NTL_GF2X_NOINLINE
   cerr << "NTL_GF2X_NOINLINE\n";
#endif

   cerr << "\n\n";

   cerr << "running tests";

   long n, k, i;

   n = 250;
   k = 16000;

   ZZ p;

   for (i = 0; i < 15; i++) {
      //cerr << "(" << n << "," << k << ")";
      cerr << ".";
      RandomLen(p, k);
      ZZ_p::init(p);

      ZZ_pX a, b, c, c1;

      random(a, n);
      random(b, n);

      FFTMul(c, a, b);
      //cerr << ZZ_pInfo->FFTInfo->NumPrimes;

      c1 = conv<ZZ_pX>( KarMul( conv<ZZX>(a), conv<ZZX>(b) ) );

      if (c1 != c) {
         cerr << "ZZ_pX mul failed!\n";
         return 1;
      }

      n = long(n * 1.35);
      k = long(k / 1.414);
   }

   // small prime tests...I've made some changes in v5.3
   // that should be checked on various platforms, so
   // we might as well check them here.

   if (SmallModulusTest(17, 1000)) {
      cerr << "first SmallModulusTest failed!!\n";
      return 1;
   }

   if (SmallModulusTest((1L << (NTL_SP_NBITS))-1, 1000)) {
      cerr << "second SmallModulusTest failed!!\n";
      return 1;
   }

   // Test gf2x code....

   if (GF2X_test()) {
      cerr << "GF2X test failed!\n";
      return 1;
   }

   cerr << "OK\n";

   ZZ x1, x2, x3, x4;
   double t;

   RandomLen(x1, 1024);
   RandomBnd(x2, x1);
   RandomBnd(x3, x1);

   mul(x4, x2, x3);

   t = GetTime();
   for (i = 0; i < 100000; i++)
      mul(x4, x2, x3);
   t = GetTime()-t;

   cerr << "time for 1024-bit mul: " << t*10 << "us";
   cerr << "\n";

   rem(x2, x4, x1);

   t = GetTime();
   for (i = 0; i < 100000; i++)
      rem(x2, x4, x1);
   t = GetTime()-t;

   cerr << "time for 2048/1024-bit rem: " << t*10 << "us";
   cerr << "\n";

   GenPrime(p, 1024);
   RandomBnd(x1, p);
   if (IsZero(x1)) set(x1);

   InvMod(x2, x1, p);

   t = GetTime();
   for (i = 0; i < 1000; i++)
      InvMod(x2, x1, p);
   t = GetTime()-t;

   cerr << "time for 1024-bit modular inverse: " << t*1000 << "us";
   cerr << "\n";

   // test modulus switching

   n = 1024;
   k = 1024;
   RandomLen(p, k);

   ZZ_p::init(p);
   if (!IsOdd(p)) p++;

   ZZ_pX j1, j2, j3;

   random(j1, n);
   random(j2, n);

   mul(j3, j1, j2);

   t = GetTime();
   for (i = 0; i < 100; i++) mul(j3, j1, j2);
   t = GetTime()-t;

   cerr << "time to multiply degree 1023 polynomials\n   modulo a 1024-bit number: ";
   cerr << (t/100) << "s";
   cerr << "\n";

   GF2X_time();

   return 0;
}


#pragma once

//#include <sophus/se3.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <complex>

#include <opencv2/core.hpp>
//#include "pose.h"
//#include "pose_utils.h"
//#include "solver_engine.h"
//#include "GaussJordan.hpp"
#include <graph-cut-ransac/src/pygcransac/include/solver_engine.h>
//#include <glog/logging.h>

#include <OneACPose/solver_one_ac_depth.hpp>
#include <Samples/common/numeric.hpp>

namespace gcransac
{
  namespace estimator
  {
    namespace solver
    {
      inline Eigen::Matrix3d getEssentialMatrixFromRelativePose(
        const Eigen::Matrix3d& R_dst_src, const Eigen::Vector3d& t_dst_src) {

        // The cross product matrix of the translation vector
        Eigen::Matrix3d cross_prod_t_dst_src;
        cross_prod_t_dst_src << 0, -t_dst_src(2), t_dst_src(1), t_dst_src(2), 0, -t_dst_src(0),
          -t_dst_src(1), t_dst_src(0), 0;

        return cross_prod_t_dst_src * R_dst_src;
      }

      class PoseOneAffineAndRelativeDepthSolverMinimalSVD : public SolverEngine
      {
      protected:
        Eigen::Matrix3d intrinsics_source_inverse,
          intrinsics_destination_inverse;

      public:
        void initialize(Eigen::Matrix3d intrinsics_source_,
          Eigen::Matrix3d intrinsics_destination_);

        // The minimum number of points required for the estimation
        static constexpr size_t sampleSize()
        {
          return 1;
        }

        static constexpr const char* name()
        {
          return "Minimal SVD Solver";
        }

        // Determines if there is a chance of returning multiple models
        // the function 'estimateModel' is applied.
        static constexpr bool returnMultipleModels()
        {
          return maximumSolutions() > 1;
        }

        static constexpr size_t maximumSolutions()
        {
          return 1;
        }

        // Estimate the model parameters from the given point sample
        // using weighted fitting if possible.
        inline bool estimateModel(
          const cv::Mat& data_, // The set of data points
          const size_t* sample_, // The sample used for the estimation
          size_t sample_number_, // The size of the sample
          std::vector<Model>& models_, // The estimated model parameters
          const double* weights_ = nullptr) const; // The weight for each point

      };

      void PoseOneAffineAndRelativeDepthSolverMinimalSVD::initialize(
        Eigen::Matrix3d intrinsics_source_,
        Eigen::Matrix3d intrinsics_destination_)
      {
        intrinsics_source_inverse = intrinsics_source_.inverse();
        intrinsics_destination_inverse = intrinsics_destination_.inverse();
      }

      inline bool PoseOneAffineAndRelativeDepthSolverMinimalSVD::estimateModel(
        const cv::Mat& data_, // The set of data points
        const size_t* sample_, // The sample used for the estimation
        size_t sample_number_, // The size of the sample
        std::vector<Model>& models_, // The estimated model parameters
        const double* weights_) const // The weight for each point
      {
        const size_t N = sample_number_;
        if (N < sampleSize()) {
          //LOG(INFO)
          std::cerr
            << "You must supply at least 1 correspondences for the linear 1 point + depth essential matrix algorithm." << std::endl;
          return false;
        }

        // Step 1. Check if there enough correspondences with valid depths (i.e. >0)
        const double* data_ptr = reinterpret_cast<double*>(data_.data);
        const size_t cols = data_.cols;
        std::vector<size_t> good_point_indices;
        good_point_indices.reserve(N);

        for (size_t i = 0; i < N; i++)
        {
          size_t idx;
          if (sample_ == nullptr)
            idx = i * cols;
          else
            idx = sample_[i] * cols;

          /*if (*(data_ptr + idx + 4) < std::numeric_limits<double>::epsilon() ||
            *(data_ptr + idx + 5) < std::numeric_limits<double>::epsilon())
            continue;*/

            // TODO: azt is meg lehetne csinálni (túlhatározott esetben), hogy ha a mélységderiváltak rosszak, akkor azok az egyenletek kimaradnak projectionConstraint-ből
          good_point_indices.push_back(idx);
        }

        if (good_point_indices.size() < sampleSize()) {
          //LOG(INFO)
          //	<< "You must supply at least 1 correspondences for the linear 1 point + depth essential matrix algorithm.";
          return false;
        }

        OneACPose::Vec3 src, dst;
        OneACPose::Mat32 src_diff, dst_diff;
        {
          const double* point_ptr = data_ptr + good_point_indices[0];

          using MMat32d = Eigen::Map<const OneACPose::Mat32>;
          using MVec2d = Eigen::Map<const OneACPose::Vec2>;

          src.noalias() = MVec2d(point_ptr + 0).homogeneous();
          dst.noalias() = MVec2d(point_ptr + 2).homogeneous();

          src_diff.noalias() = MMat32d(point_ptr + 4 + 0);
          dst_diff.noalias() = MMat32d(point_ptr + 4 + 6);
        }

        // Step 3. Compute rotation, scaling and translation
        Eigen::Matrix3d R;
        Eigen::Vector3d t;
        double scale;

        // estimate pose using 1AC+D
        OneACPose::OneACD(
          dst, src,
          dst_diff, src_diff,
          scale, R, t);

        Eigen::Matrix3d essential_matrix;

        if (R.hasNaN() || t.hasNaN())
          return false;

        //essential_matrix.noalias() = common::cross_product(t) * R;
        essential_matrix.noalias() = getEssentialMatrixFromRelativePose(R, t);

        if (R.hasNaN() || t.hasNaN())
          return false;

        Eigen::Vector3d scales;
        scales << 1, scale, 0;

        Model model;
        model.descriptor = Eigen::Matrix<double, 3, 3>();
        model.descriptor.block<3, 3>(0, 0) = essential_matrix;
        /*model.descriptor.block<3, 3>(0, 3) = R;
        model.descriptor.block<3, 1>(0, 6) = t;
        model.descriptor.block<3, 1>(0, 7) = scales;*/

        models_.reserve(1);
        models_.emplace_back(model);

        return true;
      }
    }
  }
}


#include <ros/ros.h>
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"

#include <avatar_locomanipulation/helpers/NeuralNetModel.hpp>

# include <cstdlib>
# include <iostream>
# include <iomanip>
# include <cmath>

#include <math.h>
#include <time.h>
#include <random>

#include <avatar_locomanipulation/helpers/IOUtilities.hpp>
#include <Eigen/Dense>
#include <avatar_locomanipulation/BinaryClassifierQuery.h>

#include <avatar_locomanipulation/models/robot_model.hpp>

#include <avatar_locomanipulation/helpers/yaml_data_saver.hpp>
#include <avatar_locomanipulation/helpers/param_handler.hpp>

#include <chrono>
typedef std::chrono::high_resolution_clock Clock;

#define CONTACT_TRANSITION_DATA_LEFT_FOOT_STANCE 0
#define CONTACT_TRANSITION_DATA_RIGHT_FOOT_STANCE 1

#define CONTACT_TRANSITION_DATA_LEFT_HAND 0
#define CONTACT_TRANSITION_DATA_RIGHT_HAND 1
#define CONTACT_TRANSITION_DATA_BOTH_HANDS 2

struct PositionScore{
	double f_score;
	Eigen::Vector3d position;
};

void fill_poses(geometry_msgs::PoseArray & poses);

void fill_hand_position_markers(visualization_msgs::MarkerArray & hand_markers, std::vector<PositionScore> & ps);

Eigen::Vector3d quatToVec(const Eigen::Quaterniond & ori){
	Eigen::AngleAxisd tmp_ori(ori.normalized()); // gets the normalized version of ori and sets it to an angle axis representation
  Eigen::Vector3d ori_vec = tmp_ori.axis()*tmp_ori.angle();
  return ori_vec;
}

void normalizeInputCalculate(const Eigen::VectorXd & x_in, const Eigen::VectorXd & data_mean, const Eigen::VectorXd & data_std_dev, Eigen::VectorXd & x_normalized) {
  // std::cout << x_in.rows() << " " << data_mean.rows() << " " << data_std_dev.rows() << std::endl;
  x_normalized = (x_in - data_mean).cwiseQuotient(data_std_dev);
}

int main(int argc, char ** argv){
	// Find and load the model
  ParamHandler param_handler;
  std::string model_path = "/home/ryan/nasa_ws/src/avatar_locomanipulation/src/python_src/rh_transitions/learned_model/model.yaml";

  std::cout << "Loading Model..." << std::endl;
  myYAML::Node model = myYAML::LoadFile(model_path);
  NeuralNetModel nn_transition(model, false);
  std::cout << "Loaded" << std::endl;

  // Create the structures which will hold the position and score
  std::vector<PositionScore> original;
  PositionScore temp;
  //Define feature vector members
	Eigen::Vector3d nn_swing_foot_start_pos;
	Eigen::Quaterniond nn_swing_foot_start_ori;
	Eigen::Vector3d nn_pelvis_pos;
	Eigen::Quaterniond nn_pelvis_ori;
	Eigen::Vector3d nn_landing_foot_pos;
	Eigen::Quaterniond nn_landing_foot_ori;
	Eigen::Vector3d nn_right_hand_start_pos;
	Eigen::Vector3d nn_right_hand_start_ori;
	Eigen::Vector3d nn_left_hand_start_pos;
	Eigen::Quaterniond nn_left_hand_start_ori;
	double nn_stance_origin;
	double nn_manipulation_type;
	// For addToXVector
	Eigen::AngleAxisd tmp_aa;
	Eigen::Vector3d tmp_ori_vec3;
	// For getting the results
	double prediction_sum, prediction_avg;
	prediction_sum = 0;

  // Initialize all of our values
	nn_stance_origin = CONTACT_TRANSITION_DATA_RIGHT_FOOT_STANCE;
	nn_manipulation_type = CONTACT_TRANSITION_DATA_RIGHT_HAND;

	nn_swing_foot_start_pos.setZero();
	nn_swing_foot_start_ori.setIdentity();

	nn_pelvis_pos.setZero();
	nn_pelvis_ori.setIdentity();

	nn_landing_foot_pos.setZero();
	nn_landing_foot_ori.setIdentity();

	nn_right_hand_start_pos.setZero();
	nn_right_hand_start_ori.setZero();

	nn_left_hand_start_pos.setZero();
	nn_left_hand_start_ori.setIdentity();

	tmp_ori_vec3.setZero();

	// Set all of the NZ/Non Identity values we want fixed
	nn_swing_foot_start_pos[0] = 0.0; nn_swing_foot_start_pos[1] = 0.3; nn_swing_foot_start_pos[2] = 0.0;

	nn_landing_foot_pos[0] = 0.0; nn_landing_foot_pos[1] = 0.3; nn_landing_foot_pos[2] = 0.0;

	nn_pelvis_pos[0] = 0.0; nn_pelvis_pos[1] = 0.15; nn_pelvis_pos[2] = 0.9;

	// Create the blank feature vectors
  Eigen::VectorXd rawDatum(32);
  Eigen::VectorXd datum(32);
	Eigen::MatrixXd data(1053,32);
	// Blank vector for prediction f_scores
	Eigen::MatrixXd pred(100,1);
	std::vector<Eigen::Vector3d> rhand_position;

	 //Normalization Params
  param_handler.load_yaml_file("/home/ryan/nasa_ws/src/avatar_locomanipulation/nn_models/baseline_11500pts/lh_rflh_lfrh_lfrh_rfbh_rfbh_lf/normalization_params.yaml");
  std::vector<double> vmean;
  param_handler.getVector("x_train_mean", vmean);
  std::vector<double> vstd_dev;
  param_handler.getVector("x_train_std", vstd_dev);

  Eigen::VectorXd mean(32);
  Eigen::VectorXd std_dev(32);
  for (int ii = 0; ii < 32; ii++){
    mean[ii] = vmean[ii];
    std_dev[ii] = vstd_dev[ii];
  }

  // For rand number generation (For Orientation)
  unsigned int seed_number = 1;
	std::mt19937 generator;
	generator.seed(seed_number);
	std::uniform_real_distribution<double> u_distribution_double(-M_PI, M_PI);
	Eigen::VectorXd randx(100), randy(100), randz(100);
	// // Generate our list of random numbers
	// for(int k=0; k<100; ++k){
	// 	randx[k] = u_distribution_double(generator);
	// 	randy[k] = u_distribution_double(generator);
	// 	randz[k] = u_distribution_double(generator);
	// }

	// Fill the rawDatum
		// Only the nn_right_hand_start_pos and tmp_ori_vec3 will change
	rawDatum << nn_stance_origin, nn_manipulation_type,
		nn_swing_foot_start_pos, quatToVec(nn_swing_foot_start_ori),
		nn_pelvis_pos, quatToVec(nn_pelvis_ori),
		nn_landing_foot_pos, quatToVec(nn_landing_foot_ori),
		nn_right_hand_start_pos, tmp_ori_vec3,
		nn_left_hand_start_pos, quatToVec(nn_left_hand_start_ori);

	int i=0;
	for(int q=-20; q<=20; q+=5){
		// Set the x position
		nn_right_hand_start_pos[0] = static_cast<double>(q) / 100.0;

		for(int r=-80; r<=-20; r+=5){
			// Set the y position
			nn_right_hand_start_pos[1] = static_cast<double>(r) / 100.0;

			for(int s=80; s<=120; s+=5){
				// Set the z position
				nn_right_hand_start_pos[2] = static_cast<double>(s) / 100.0;
				// Reset the Prediction Sum
				prediction_sum = 0.0;

				// // Loop thru all of the random orientations
				// for(int j=0; j<100; ++j){

					// Set the rhand ori and normalize
					// nn_right_hand_start_ori[0] = randx[j]; nn_right_hand_start_ori[1] = randy[j]; nn_right_hand_start_ori[2] = randz[j];
					// tmp_ori_vec3 = nn_right_hand_start_ori.normalized();
					Eigen::Quaterniond rhand_door_start_ori;
					rhand_door_start_ori.x() = 0.0;
					rhand_door_start_ori.y() = -0.707;
					rhand_door_start_ori.z() = 0.0;
					rhand_door_start_ori.w() = 0.707;

					rhand_position.push_back(nn_right_hand_start_pos);

					tmp_ori_vec3 = quatToVec(rhand_door_start_ori);

					// Set the current right hand position and orientation in the rawDatum
					rawDatum[20] = nn_right_hand_start_pos[0]; rawDatum[21] = nn_right_hand_start_pos[1]; rawDatum[22] = nn_right_hand_start_pos[2];
					rawDatum[23] = tmp_ori_vec3[0]; rawDatum[24] = tmp_ori_vec3[1]; rawDatum[25] = tmp_ori_vec3[2];

					normalizeInputCalculate(rawDatum, mean, std_dev, datum);

					data.row(i) = datum;
					++i;

				// } // random orientation loop closed

				// // Once we have 100 rows of data send to the model and get prediction
				// pred = nn_transition.GetOutput(data);
				// // Set the position for our struct
				// temp.position = nn_right_hand_start_pos;
				// // Get the sum or predictions for the 100 orientations
				// for(int w=0; w<100; ++w){
				// 	prediction_sum += pred(w,0);
				// }
				// // f_score is the avg over 100 orientations
				// temp.f_score = prediction_sum/100.0;
				// Add temp to the vector of structs
				// original.push_back(temp);

			}// z for loop closed
		}// y for loop closed
	}// x for loop closed

	std::cout << "before\n";
	pred = nn_transition.GetOutput(data);
	std::cout << "after\n";
	// Set the position for our struct
	for(int p=0; p<rhand_position.size(); ++p){
		temp.position = rhand_position[p];
		temp.f_score = pred(p,0);
		original.push_back(temp);
	}

	std::cout << "Generated all of the data and f scores\n";

	// NOW WE HAVE ALL OF THE DATA GENERATED AND NEED TO STANDARDIZE IT

	double max_val = original[0].f_score;
	// Find the maximum f_score
	for(int i=1; i < original.size(); ++i){
		if(original[i].f_score >= max_val) max_val = original[i].f_score;
	}
	// Standardize it
	for(int i=0; i < original.size(); ++i){
		original[i].f_score = original[i].f_score/max_val;
	}

	// NOW IT HAS BEEN STANDARDIZED AND NEEDS TO BE VIZ'd

	std::cout << "Standardized all of the data\n";

	std::cout << "original.size(): " << original.size() << std::endl;

	// Initialize ROS node for publishing joint messages
	ros::init(argc, argv, "test_rviz_feasibility_viz");
	ros::NodeHandle n;
	ros::Rate loop_rate(20);
	// Define MarkerArray (Hand Positions)
	visualization_msgs::MarkerArray hand_markers;
	hand_markers.markers[original.size()];
	// Define PoseArray (Pose of pelvis, rfoot, lfoot)
	geometry_msgs::PoseArray poses;
	// Waypoint Publisher
	ros::Publisher hand_markers_pub = n.advertise<visualization_msgs::MarkerArray>("hand_positions", 100);
	// Hand Pose Publisher
	ros::Publisher poses_pub = n.advertise<geometry_msgs::PoseArray>("body_poses", 100);

	fill_poses(poses);
	fill_hand_position_markers(hand_markers, original);

	while (ros::ok()){

		hand_markers_pub.publish(hand_markers);
		poses_pub.publish(poses);
		std::cout << "publishing\n";

		ros::spinOnce();
		loop_rate.sleep();
	}

  return 0;
}

void fill_hand_position_markers(visualization_msgs::MarkerArray & hand_markers, std::vector<PositionScore> & ps){
	// Define the marker for filling up array
	visualization_msgs::Marker temp;
	double f_score;

	// Build a marker for each of the points in space
	for(int w=0; w<=ps.size(); ++w){
		f_score = ps[w].f_score;

		std::cout << "tic\n";

		// Fill the Marker
		temp.pose.position.x = ps[w].position[0];
		temp.pose.position.y = ps[w].position[1];
		temp.pose.position.z = ps[w].position[2];
		temp.pose.orientation.x = 0.;
		temp.pose.orientation.y = 0.;
		temp.pose.orientation.z = 0.;
		temp.pose.orientation.w = 1.;
		temp.ns = "hand_positions";
		temp.id = w;
		temp.scale.x = 0.02;
		temp.scale.y = 0.02;
		temp.scale.z = 0.02;
		temp.header.frame_id = "map";
		// temp.text = score_string;
		temp.color.r = (1.0-f_score);
		temp.color.b = 0.0f;
		temp.color.g = (f_score);
		temp.color.a = 0.3;
		temp.action = visualization_msgs::Marker::ADD;
		temp.type = visualization_msgs::Marker::CUBE;
		hand_markers.markers.push_back(temp);
	}
}

void fill_poses(geometry_msgs::PoseArray & poses){
	// Define the pose
	geometry_msgs::Pose pose;

	poses.header.frame_id = "map";
	// First add pelvis to poses
	pose.position.x = 0.0;
	pose.position.y = 0.15;
	pose.position.z = 0.9;
	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = 0;
	pose.orientation.w = 1;
	poses.poses.push_back(pose);
	// Next add the right foot
	pose.position.x = 0.0;
	pose.position.y = 0.0;
	pose.position.z = 0.0;
	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = 0;
	pose.orientation.w = 1;
	poses.poses.push_back(pose);
	// Last add the left foot
	pose.position.x = 0.0;
	pose.position.y = 0.3;
	pose.position.z = 0.0;
	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = 0;
	pose.orientation.w = 1;
	poses.poses.push_back(pose);
}


#ifndef PYTHONIC_INCLUDE_SCIPY_SPECIAL_GAMMALN_HPP
#define PYTHONIC_INCLUDE_SCIPY_SPECIAL_GAMMALN_HPP

#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#ifdef USE_BOOST_SIMD
#include <boost/simd/function/gammaln.hpp>
#else
#include <cmath>
#endif

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
#define NUMPY_NARY_FUNC_NAME gammaln
#ifdef USE_BOOST_SIMD
#define NUMPY_NARY_FUNC_SYM boost::simd::gammaln
#else
#define NUMPY_NARY_FUNC_SYM std::lgamma
#endif

#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


#include <boost/math/special_functions/atanh.hpp>


//
// Copyright © 2020 Arm Ltd and Contributors. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "LayersFwd.hpp"

#include <Network.hpp>
#include <ResolveType.hpp>
#include <armnn/INetwork.hpp>
#include <test/TestUtils.hpp>

#include <boost/test/unit_test.hpp>

#include <QuantizeHelper.hpp>
#include <string>

using namespace armnn;

BOOST_AUTO_TEST_SUITE(Optimizer)

namespace
{
const float   g_qScale  = 1.0f;
const int32_t g_qOffset = 0;

template<typename T>
std::vector<T> GetVector(unsigned int size, float initial, float increment)
{
    std::vector<float> typeVector(size, initial);
    std::vector<T>     vector(size);

    if (size > 1)
    {
        for (unsigned int i = 0; i < size; ++i)
        {
            vector[i] = T(initial + (increment * static_cast<float>(i)));
        }
    }
    return vector;
}

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct Convolution2dTest
{
    using LayerType = armnn::Convolution2dLayer;
    static std::string GetReceiverLayerName() { return "Convolution2d"; };
    static const bool isElementWise = false;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }  // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 3, 3, 4}); }  // NHWCout
    static TensorShape GetWeightsShape() { return TensorShape( {4, 2, 2, 3}); }  // CoutHWCin

    constexpr static const unsigned int inputSize  = 48; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 36; // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        Convolution2dDescriptor descriptor;
        descriptor.m_BiasEnabled = false;
        descriptor.m_DataLayout  = DataLayout::NHWC;
        descriptor.m_StrideX     = 1;
        descriptor.m_StrideY     = 1;

        std::vector<float> weightsData   = {  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12,
                                             11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22,
                                             21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
                                             31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42};
        std::vector<T>     weightsVector = armnnUtils::QuantizedVector<T>(weightsData, g_qScale, g_qOffset);
        TensorInfo         weightsInfo(GetWeightsShape(), ArmnnType, g_qScale, g_qOffset);
        ConstTensor        weights(weightsInfo, weightsVector);
        Optional<ConstTensor> optionalBias;

        return network->AddConvolution2dLayer(descriptor, weights, optionalBias, name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct DepthwiseConvolution2dTest
{
public:
    using LayerType = armnn::DepthwiseConvolution2dLayer;
    static std::string GetReceiverLayerName() { return "DepthwiseConvolution2d"; };
    static const bool isElementWise = false;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }   // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 3, 3, 12}); }  // NHWCout
    static TensorShape GetWeightsShape() { return TensorShape( {4, 3, 2, 2}); }   // MCinHW

    constexpr static const unsigned int inputSize  = 48; //batchIn * heightIn * widthIn * channelIn;
    constexpr static const unsigned int outputSize = 108; //batchOut * heightOut * widthOut * channelOut;

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        DepthwiseConvolution2dDescriptor descriptor;
        descriptor.m_BiasEnabled = false;
        descriptor.m_DataLayout  = DataLayout::NHWC;
        descriptor.m_StrideX     = 1;
        descriptor.m_StrideY     = 1;

        std::vector<float> weightsData   = { 1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12,
                                            11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22,
                                            21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
                                            31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42};
        std::vector<T>     weightsVector = armnnUtils::QuantizedVector<T>(weightsData, g_qScale, g_qOffset);
        TensorInfo         weightsInfo(GetWeightsShape(), ArmnnType, g_qScale, g_qOffset);
        ConstTensor        weights(weightsInfo, weightsVector);
        Optional<ConstTensor> optionalBias;

        return network->AddDepthwiseConvolution2dLayer(descriptor, weights, optionalBias, name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct FullyConnectedTest
{
public:
    using LayerType = armnn::FullyConnectedLayer;
    static std::string GetReceiverLayerName() { return "FullyConnected"; };
    static const bool isElementWise = false;

    static TensorShape GetInputShape()   { return TensorShape( {2, 5, 1, 1}); } // NCinHW
    static TensorShape GetOutputShape()  { return TensorShape( {2, 3}); }       // NCout
    static TensorShape GetWeightsShape() { return TensorShape( {5, 3}); }       // CinCout

    constexpr static const unsigned int inputSize  = 10; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 6;  // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        FullyConnectedDescriptor descriptor;
        descriptor.m_BiasEnabled = false;

        std::vector<float> weightsData   = { 1,  2,  3,  4,  5,
                                             6,  7,  8,  9, 10,
                                            11, 12, 13, 14, 15};
        std::vector<T>     weightsVector = armnnUtils::QuantizedVector<T>(weightsData, g_qScale, g_qOffset);
        TensorInfo         weightsInfo(GetWeightsShape(), ArmnnType, g_qScale, g_qOffset);
        ConstTensor        weights(weightsInfo, weightsVector);
        Optional<ConstTensor> optionalBias;

        return network->AddFullyConnectedLayer(descriptor, weights, optionalBias, name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct BatchNormTest
{
public:
    using LayerType = armnn::BatchNormalizationLayer;
    static std::string GetReceiverLayerName() { return "BatchNorm"; };
    static const bool isElementWise = false;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }  // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 4, 4, 3}); }  // NHWCout

    constexpr static const unsigned int inputSize  = 48; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 48; // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        BatchNormalizationDescriptor descriptor;
        descriptor.m_DataLayout = DataLayout::NHWC;

        std::vector<T> betaVector     = GetVector<T>(GetOutputShape()[3], 0.0f, 0.2f);
        std::vector<T> gammaVector    = GetVector<T>(GetOutputShape()[3], 0.5f, 0.1f);
        std::vector<T> meanVector     = GetVector<T>(GetOutputShape()[3], 0.1f, 0.1f);
        std::vector<T> varianceVector = GetVector<T>(GetOutputShape()[3], 1.0f, 0.1f);

        const unsigned int outputChannelSize[] = { GetOutputShape()[3] };
        ConstTensor beta(TensorInfo(1, outputChannelSize, ArmnnType), betaVector);
        ConstTensor gamma(TensorInfo(1, outputChannelSize, ArmnnType), gammaVector);
        ConstTensor mean(TensorInfo(1, outputChannelSize, ArmnnType), meanVector);
        ConstTensor variance(TensorInfo(1, outputChannelSize, ArmnnType), varianceVector);

        return network->AddBatchNormalizationLayer(descriptor, mean, variance, beta, gamma, name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct MultiplicationTest
{
    using LayerType = armnn::MultiplicationLayer;
    static std::string GetReceiverLayerName() { return "Multiplication"; };
    static const bool isElementWise = true;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }  // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 4, 4, 3}); }  // NHWCout

    constexpr static const unsigned int inputSize  = 48; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 48; // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        return network->AddMultiplicationLayer(name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct AdditionTest
{
    using LayerType = armnn::AdditionLayer;
    static std::string GetReceiverLayerName() { return "Addition"; };
    static const bool isElementWise = true;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }  // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 4, 4, 3}); }  // NHWCout

    constexpr static const unsigned int inputSize  = 48; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 48; // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        return network->AddAdditionLayer(name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct SubtractionTest
{
    using LayerType = armnn::SubtractionLayer;
    static std::string GetReceiverLayerName() { return "Subtraction"; };
    static const bool isElementWise = true;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }  // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 4, 4, 3}); }  // NHWCout

    constexpr static const unsigned int inputSize  = 48; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 48; // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        return network->AddSubtractionLayer(name);
    }
};

template<armnn::DataType ArmnnType, typename T = armnn::ResolveType<ArmnnType>>
struct DivisionTest
{
    using LayerType = armnn::DivisionLayer;
    static std::string GetReceiverLayerName() { return "Division"; };
    static const bool isElementWise = true;

    static TensorShape GetInputShape()   { return TensorShape( {1, 4, 4, 3}); }  // NHWCin
    static TensorShape GetOutputShape()  { return TensorShape( {1, 4, 4, 3}); }  // NHWCout

    constexpr static const unsigned int inputSize  = 48; // batchIn * heightIn * widthIn * channelIn
    constexpr static const unsigned int outputSize = 48; // batchOut * heightOut * widthOut * channelOut

    static IConnectableLayer* AddReceiverLayer(INetwork* network,
                                               const char* name)
    {
        return network->AddDivisionLayer(name);
    }
};

} // namespace

template<typename LayerTest,
         armnn::DataType ArmnnType>
INetworkPtr CreatNetwork(ActivationDescriptor activationDescriptor, bool preventFusing)
{
    // Create a network
    INetworkPtr network = INetwork::Create();

    IConnectableLayer* inputLayer = network->AddInputLayer(0);

    IConnectableLayer* receiverLayer = LayerTest::AddReceiverLayer(network.get(),
                                                                   "receiverLayer");

    IConnectableLayer* activationLayer = network->AddActivationLayer(activationDescriptor,
                                                                     "activation");

    IConnectableLayer* outputLayer  = network->AddOutputLayer(0);
    IConnectableLayer* output2Layer = preventFusing?network->AddOutputLayer(1):nullptr;

    // Define layers information
    TensorInfo inputInfo(LayerTest::GetInputShape(), ArmnnType, g_qScale, g_qOffset);
    TensorInfo outputInfo(LayerTest::GetOutputShape(), ArmnnType, g_qScale, g_qOffset);

    // Set layer information
    inputLayer->GetOutputSlot(0).SetTensorInfo(inputInfo);
    receiverLayer->GetOutputSlot(0).SetTensorInfo(outputInfo);
    activationLayer->GetOutputSlot(0).SetTensorInfo(outputInfo);

    // Connect layers
    inputLayer->GetOutputSlot(0).Connect(receiverLayer->GetInputSlot(0));
    receiverLayer->GetOutputSlot(0).Connect(activationLayer->GetInputSlot(0));
    activationLayer->GetOutputSlot(0).Connect(outputLayer->GetInputSlot(0));

    if (LayerTest::isElementWise)
    {
        inputLayer->GetOutputSlot(0).Connect(receiverLayer->GetInputSlot(1));
    }
    if (preventFusing)
    {
        receiverLayer->GetOutputSlot(0).Connect(output2Layer->GetInputSlot(0));
    }

    return network;
}

template<typename LayerTest,
         armnn::DataType ArmnnType,
         typename LayerType = typename LayerTest::LayerType,
         typename T = armnn::ResolveType<ArmnnType>>
void FuseActivationIntoPreviousLayerTest(ActivationDescriptor activationDescriptor, float tolerance, armnn::Compute
backendId)
{
    // FIRST NETWORK: Fused
    // Construct ArmNN network
    INetworkPtr networkFused = CreatNetwork<LayerTest, ArmnnType>(activationDescriptor, false);

    // Create ArmNN runtime
    IRuntimePtr run = IRuntime::Create(IRuntime::CreationOptions()); // default options

    // Optimise ArmNN network
    IOptimizedNetworkPtr optNetFused = Optimize(*networkFused, {backendId}, run->GetDeviceSpec());

    Graph graphFused = PolymorphicDowncast<OptimizedNetwork*>(optNetFused.get())->GetGraph();

    auto checkFusedConv2d = [](const armnn::Layer* const layer)->bool {
        return IsLayerOfType<LayerType>(layer) &&
            (layer->GetNameStr() == "fused-activation-into-receiverLayer");
    };

    BOOST_CHECK_MESSAGE(3 == graphFused.GetNumLayers(), LayerTest::GetReceiverLayerName());
    BOOST_TEST(CheckSequence(graphFused.cbegin(),
                             graphFused.cend(),
                             &IsLayerOfType<InputLayer>,
                             checkFusedConv2d,
                             &IsLayerOfType<OutputLayer>));

    // Load network into runtime
    NetworkId networkIdentifier;
    BOOST_TEST(run->LoadNetwork(networkIdentifier, std::move(optNetFused)) == Status::Success);

    //Creates structures for inputs and outputs.
    std::vector<float> data = GetVector<float>(LayerTest::inputSize, 1.0f, 0.1f);
    std::vector<T> inputDataFused = armnnUtils::QuantizedVector<T>(data, g_qScale, g_qOffset);
    std::vector<T> outputDataFused(LayerTest::outputSize);

    InputTensors  inputTensorsFused{
        {0, ConstTensor(run->GetInputTensorInfo(networkIdentifier, 0), inputDataFused.data())}};
    OutputTensors outputTensorsFused{
        {0, Tensor(run->GetOutputTensorInfo(networkIdentifier, 0), outputDataFused.data())}};

    // Execute network
    run->EnqueueWorkload(networkIdentifier, inputTensorsFused, outputTensorsFused);

    // SECOND NETWORK: NotFused
    // Construct ArmNN network
    INetworkPtr networkNotFused = CreatNetwork<LayerTest, ArmnnType>(activationDescriptor, true);

    // Create ArmNN runtime
    IRuntimePtr runNotFused = IRuntime::Create(IRuntime::CreationOptions()); // default options

    // Optimise ArmNN network
    IOptimizedNetworkPtr optNetNotFused = Optimize(*networkNotFused, {backendId}, runNotFused->GetDeviceSpec());

    Graph graphNotFused = PolymorphicDowncast<OptimizedNetwork*>(optNetNotFused.get())->GetGraph();

    BOOST_CHECK(5 == graphNotFused.GetNumLayers());
    BOOST_TEST(CheckSequence(graphNotFused.cbegin(),
                             graphNotFused.cend(),
                             &IsLayerOfType<armnn::InputLayer>,
                             &IsLayerOfType<LayerType>,
                             &IsLayerOfType<armnn::ActivationLayer>,
                             &IsLayerOfType<armnn::OutputLayer>,
                             &IsLayerOfType<armnn::OutputLayer>));

    // Load network into runtime
    NetworkId networkIdentifierNotFused;
    BOOST_TEST(runNotFused->LoadNetwork(networkIdentifierNotFused, std::move(optNetNotFused)) == Status::Success);

    //Creates structures for inputs and outputs.
    std::vector<T> inputDataNotFused = armnnUtils::QuantizedVector<T>(data, g_qScale, g_qOffset);
    std::vector<T> outputDataNotFused(LayerTest::outputSize);
    std::vector<T> outputData2NotFused(LayerTest::outputSize);

    InputTensors  inputTensorsNotFused{
        {0, ConstTensor(runNotFused->GetInputTensorInfo(networkIdentifierNotFused, 0), inputDataNotFused.data())}};
    OutputTensors outputTensorsNotFused{
        {0, Tensor(runNotFused->GetOutputTensorInfo(networkIdentifierNotFused, 0), outputDataNotFused.data())},
        {1, Tensor(runNotFused->GetOutputTensorInfo(networkIdentifierNotFused, 1), outputData2NotFused.data())}};

    // Execute network
    runNotFused->EnqueueWorkload(networkIdentifierNotFused, inputTensorsNotFused, outputTensorsNotFused);

    // Check the output of the fused-activation matches with the output of the activation in the "NotFused" network
    for (unsigned int n = 0; n < outputDataFused.size(); ++n)
    {
        BOOST_CHECK_CLOSE(static_cast<float>(outputDataFused[n]), static_cast<float>(outputDataNotFused[n]),
                          T(tolerance));
    }
}

#if defined(ARMCOMPUTENEON_ENABLED)
// ReLu fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseReLUIntoConvFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoDWConvFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<DepthwiseConvolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoFullyConnectedFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<FullyConnectedTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoBatchNormFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<BatchNormTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}

// BoundedReLu fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoConvFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoDWConvFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest < DepthwiseConvolution2dTest < DataType::Float32 > , DataType::Float32 >
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoFullyConnectedFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<FullyConnectedTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoBatchNormFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<BatchNormTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}

// ReLU fused into Receiver Layers QAsymmU8
BOOST_AUTO_TEST_CASE(FuseReLUIntoConvQAsymmU8CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::QAsymmU8>, DataType::QAsymmU8>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoDWConvQAsymmU8CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<DepthwiseConvolution2dTest<DataType::QAsymmU8>, DataType::QAsymmU8>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoFullyConnectedQAsymmU8CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<FullyConnectedTest<DataType::QAsymmU8>, DataType::QAsymmU8>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}

// HardSwish fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseHardSwishIntoConvFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::HardSwish;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}

// TanH fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseTanHIntoConvFloat32CpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::TanH;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::CpuAcc);
}
#endif

#if defined(ARMCOMPUTECL_ENABLED)
// ReLu fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseReLUIntoConvFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoDWConvFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<DepthwiseConvolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoFullyConnectedFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<FullyConnectedTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoBatchNormFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<BatchNormTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoMulFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<MultiplicationTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoAddFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<AdditionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoSubFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<SubtractionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUIntoDivFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<DivisionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}

// BoundedReLu fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoConvFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoDWConvFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<DepthwiseConvolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoFullyConnectedFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<FullyConnectedTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoBatchNormFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<BatchNormTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoMulFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<MultiplicationTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoAddFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<AdditionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoSubFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<SubtractionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseBoundedReLUIntoDivFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::BoundedReLu;
    activationDescriptor.m_A = 1.0f;
    activationDescriptor.m_B = -1.0f;

    FuseActivationIntoPreviousLayerTest<DivisionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}

// ReLU fused into Receiver Layers QAsymmU8
BOOST_AUTO_TEST_CASE(FuseReLUQIntoConvAsymmU8GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::QAsymmU8>, DataType::QAsymmU8>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUQIntoDWConvAsymmU8GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<DepthwiseConvolution2dTest<DataType::QAsymmU8>, DataType::QAsymmU8>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseReLUQIntoFullyConnectedAsymmU8GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::ReLu;

    FuseActivationIntoPreviousLayerTest<FullyConnectedTest<DataType::QAsymmU8>, DataType::QAsymmU8>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}

// HardSwish fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseHardSwishIntoConvFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::HardSwish;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseHardSwishIntoMulFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::HardSwish;

    FuseActivationIntoPreviousLayerTest<MultiplicationTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseHardSwishIntoAddFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::HardSwish;

    FuseActivationIntoPreviousLayerTest<AdditionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseHardSwishIntoSubFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::HardSwish;

    FuseActivationIntoPreviousLayerTest<SubtractionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseHardSwishIntoDivFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::HardSwish;

    FuseActivationIntoPreviousLayerTest<DivisionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}

// TanH fused into Receiver Layers Float32
BOOST_AUTO_TEST_CASE(FuseTanHIntoConvFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::TanH;

    FuseActivationIntoPreviousLayerTest<Convolution2dTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseTanHIntoMulFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::TanH;

    FuseActivationIntoPreviousLayerTest<MultiplicationTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseTanHIntoAddFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::TanH;

    FuseActivationIntoPreviousLayerTest<AdditionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseTanHIntoSubFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::TanH;

    FuseActivationIntoPreviousLayerTest<SubtractionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
BOOST_AUTO_TEST_CASE(FuseTanHIntoDivFloat32GpuAccTest)
{
    ActivationDescriptor activationDescriptor;
    activationDescriptor.m_Function = ActivationFunction::TanH;

    FuseActivationIntoPreviousLayerTest<DivisionTest<DataType::Float32>, DataType::Float32>
        (activationDescriptor, 0.0001f, armnn::Compute::GpuAcc);
}
#endif

BOOST_AUTO_TEST_SUITE_END()


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

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"
#include "metric_camera_colinearity_tait_bryan_wc_jacobian.h"
#include "metric_camera_colinearity_rodrigues_wc_jacobian.h"
#include "metric_camera_colinearity_quaternion_wc_jacobian.h"
#include "quaternion_constraint_jacobian.h"

struct KeyPoint{
	double ksi;
	double eta;
	int index_to_tie_point;
};

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

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

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = -215, rotate_y = 273;
float translate_z = -68.0;
float translate_x = 4, translate_y = 30.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();

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

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

	cam_params.c = 5000;
	cam_params.eta_0 = 5000;
	cam_params.ksi_0 = 5000;

	for(size_t i = 0 ; i < 100; i++){
		Eigen::Vector3d p;
		p.x() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 100.0;
		p.y() = ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 105.0;
		p.z() = 100 + ((rand()%1000000)/1000000.0 - 0.5) * 2.0 * 5.0;
		tie_points.push_back(p);
	}

	for(int i = -50 ; i < 50; i+=20){
		Camera c;
		c.pose = Eigen::Affine3d::Identity();
		c.pose(0,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		c.pose(1,3) = i;
		c.pose(2,3) = (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.0;
		cameras.push_back(c);
	}

	for(size_t i = 0 ; i < cameras.size(); i++){
		for(size_t j = 0; j < tie_points.size(); j++){
			KeyPoint kp;
			kp.index_to_tie_point = j;

			TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
			metric_camera_colinearity_tait_bryan_wc(kp.ksi, kp.eta, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, tie_points[j].x(), tie_points[j].y(), tie_points[j].z());

			cameras[i].key_points.push_back(kp);
		}
	}

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

	glColor3f(1,0,0);
	glPointSize(5);
	glBegin(GL_POINTS);
	for(size_t i = 0 ; i < tie_points.size(); i++){
		glVertex3f(tie_points[i].x(), tie_points[i].y(), tie_points[i].z());
	}
	glEnd();

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

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

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

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

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < cameras.size(); i++){
		Eigen::Affine3d m = cameras[i].pose;
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();
	glutSwapBuffers();
}

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

				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose);
				cameras[i].pose = cameras[i].pose * m;
			}
			break;
		}
		case 'p':{
			for(size_t i = 0; i < tie_points.size(); i++){
				tie_points[i].x() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.1;
				tie_points[i].y() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.1;
				tie_points[i].z() += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 1.1;
			}
			break;
		}
		case 't':{
			for(size_t i = 0; i < cameras.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
				posetb.px += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.py += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.pz += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				cameras[i].pose = affine_matrix_from_pose_tait_bryan(posetb);
			}

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

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

					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);

					Eigen::Matrix<double, 2, 1> delta;
					observation_equation_metric_camera_colinearity_tait_bryan_wc(delta, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							tie_points[cameras[i].key_points[j].index_to_tie_point].x(), tie_points[cameras[i].key_points[j].index_to_tie_point].y(), tie_points[cameras[i].key_points[j].index_to_tie_point].z(),
							std::floor(cameras[i].key_points[j].ksi), std::floor(cameras[i].key_points[j].eta));

					Eigen::Matrix<double, 2, 9, Eigen::RowMajor> jacobian;
					observation_equation_metric_camera_colinearity_tait_bryan_wc_jacobian(jacobian, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							tie_points[cameras[i].key_points[j].index_to_tie_point].x(), tie_points[cameras[i].key_points[j].index_to_tie_point].y(), tie_points[cameras[i].key_points[j].index_to_tie_point].z(),
							std::floor(cameras[i].key_points[j].ksi), std::floor(cameras[i].key_points[j].eta));

					int ir = tripletListB.size();
					int ic_camera = i * 6;
					int ic_tie_point = cameras.size() * 6 + cameras[i].key_points[j].index_to_tie_point * 3;

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

					tripletListA.emplace_back(ir     , ic_tie_point,     -jacobian(0,6));
					tripletListA.emplace_back(ir     , ic_tie_point + 1, -jacobian(0,7));
					tripletListA.emplace_back(ir     , ic_tie_point + 2, -jacobian(0,8));

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

					tripletListA.emplace_back(ir + 1 , ic_tie_point,     -jacobian(1,6));
					tripletListA.emplace_back(ir + 1 , ic_tie_point + 1, -jacobian(1,7));
					tripletListA.emplace_back(ir + 1 , ic_tie_point + 2, -jacobian(1,8));

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

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

			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(), cameras.size() * 6 + tie_points.size() * 3);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

			if(h_x.size() == cameras.size() * 6 + tie_points.size() * 3){
				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 < cameras.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(cameras[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

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

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

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

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

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

					Eigen::Matrix<double, 2, 1> delta;
					observation_equation_metric_camera_colinearity_rodrigues_wc(delta, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							tie_points[cameras[i].key_points[j].index_to_tie_point].x(), tie_points[cameras[i].key_points[j].index_to_tie_point].y(), tie_points[cameras[i].key_points[j].index_to_tie_point].z(),
							std::floor(cameras[i].key_points[j].ksi), std::floor(cameras[i].key_points[j].eta));

					Eigen::Matrix<double, 2, 9, Eigen::RowMajor> jacobian;
					observation_equation_metric_camera_colinearity_rodrigues_wc_jacobian(jacobian, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							tie_points[cameras[i].key_points[j].index_to_tie_point].x(), tie_points[cameras[i].key_points[j].index_to_tie_point].y(), tie_points[cameras[i].key_points[j].index_to_tie_point].z(),
							std::floor(cameras[i].key_points[j].ksi), std::floor(cameras[i].key_points[j].eta));

					int ir = tripletListB.size();
					int ic_camera = i * 6;
					int ic_tie_point = cameras.size() * 6 + cameras[i].key_points[j].index_to_tie_point * 3;

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

					tripletListA.emplace_back(ir     , ic_tie_point,     -jacobian(0,6));
					tripletListA.emplace_back(ir     , ic_tie_point + 1, -jacobian(0,7));
					tripletListA.emplace_back(ir     , ic_tie_point + 2, -jacobian(0,8));

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

					tripletListA.emplace_back(ir + 1 , ic_tie_point,     -jacobian(1,6));
					tripletListA.emplace_back(ir + 1 , ic_tie_point + 1, -jacobian(1,7));
					tripletListA.emplace_back(ir + 1 , ic_tie_point + 2, -jacobian(1,8));

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

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

			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(), cameras.size() * 6 + tie_points.size() * 3);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

			if(h_x.size() == cameras.size() * 6 + tie_points.size() * 3){
				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 < cameras.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(cameras[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

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

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

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

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

					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);

					Eigen::Matrix<double, 2, 1> delta;
					observation_equation_metric_camera_colinearity_quaternion_wc(delta, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							tie_points[cameras[i].key_points[j].index_to_tie_point].x(), tie_points[cameras[i].key_points[j].index_to_tie_point].y(), tie_points[cameras[i].key_points[j].index_to_tie_point].z(),
							std::floor(cameras[i].key_points[j].ksi), std::floor(cameras[i].key_points[j].eta));

					Eigen::Matrix<double, 2, 10, Eigen::RowMajor> jacobian;
					observation_equation_metric_camera_colinearity_quaternion_wc_jacobian(jacobian, cam_params.ksi_0, cam_params.eta_0, cam_params.c, pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							tie_points[cameras[i].key_points[j].index_to_tie_point].x(), tie_points[cameras[i].key_points[j].index_to_tie_point].y(), tie_points[cameras[i].key_points[j].index_to_tie_point].z(),
							std::floor(cameras[i].key_points[j].ksi), std::floor(cameras[i].key_points[j].eta));

					int ir = tripletListB.size();
					int ic_camera = i * 7;
					int ic_tie_point = cameras.size() * 7 + cameras[i].key_points[j].index_to_tie_point * 3;

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

					tripletListA.emplace_back(ir     , ic_tie_point,     -jacobian(0,7));
					tripletListA.emplace_back(ir     , ic_tie_point + 1, -jacobian(0,8));
					tripletListA.emplace_back(ir     , ic_tie_point + 2, -jacobian(0,9));

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

					tripletListA.emplace_back(ir + 1 , ic_tie_point,     -jacobian(1,7));
					tripletListA.emplace_back(ir + 1 , ic_tie_point + 1, -jacobian(1,8));
					tripletListA.emplace_back(ir + 1 , ic_tie_point + 2, -jacobian(1,9));

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

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

			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);
			tripletListA.emplace_back(ir + 6 , 6, 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);
			tripletListP.emplace_back(ir + 6 , ir + 6, 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);
			tripletListB.emplace_back(ir + 6 , 0, 0);

			for(size_t i = 0 ; i < cameras.size(); i++){
				int ic = i * 7;
				ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);

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

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

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

				tripletListP.emplace_back(ir, ir, 1000000.0);

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

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

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

			if(h_x.size() == cameras.size() * 7 + tie_points.size() * 3){
				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 < cameras.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(cameras[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];

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

				for(size_t i = 0; i < tie_points.size(); i++){
					tie_points[i].x() += h_x[counter++];
					tie_points[i].y() += h_x[counter++];
					tie_points[i].z() += h_x[counter++];
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

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

	mouse_old_x = x;
	mouse_old_y = y;
}

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

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

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

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

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

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodriguez)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
	std::cout << "c: add noise to cameras" << std::endl;
	std::cout << "p: add noise to tie points" << std::endl;
}