/*
 * Copyright (C) 2011-2012, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */

#include <urbi/object/global.hh>
#include <urbi/object/matrix.hh>
#include <boost/numeric/ublas/lu.hpp> // boost::numeric::ublas::row
#include <kernel/uvalue-cast.hh>

#define FORWARD_EXCEPTION(Command)              \
  do {                                          \
    try                                         \
    {                                           \
      Command;                                  \
    }                                           \
    catch (const std::runtime_error& e)         \
    {                                           \
      FRAISE(e.what());                         \
    }                                           \
  } while (false)

namespace urbi
{
  namespace object
  {

    namespace ublas = boost::numeric::ublas;

    ATTRIBUTE_NORETURN
    static inline void
    raise_incompatible_sizes(const matrix_type& m1, const matrix_type& m2,
                             const std::string& hint = "")
    {
      FRAISE("incompatible sizes: %sx%s, %sx%s%s%s",
             m1.size1(), m1.size2(), m2.size1(), m2.size2(),
             hint.empty() ? "" : ": ",
             hint);
    }

    ATTRIBUTE_NORETURN
    static inline void
    raise_incompatible_sizes(const matrix_type& m, const vector_type& v,
                             const std::string& hint = "")
    {
      FRAISE("incompatible sizes: %sx%s, %s%s%s",
             m.size1(), m.size2(), v.size(),
             hint.empty() ? "" : ": ",
             hint);
    }

#define CHECK_SIZE(Matrix1, Matrix2, Assertion)         \
    do {                                                \
      if (!(Assertion))                                 \
        raise_incompatible_sizes((Matrix1), (Matrix2)); \
    } while (0);

    static inline
    void
    check_equal_size(const matrix_type& m1, const matrix_type& m2)
    {
      CHECK_SIZE(m1, m2, m1.size1() == m2.size1() && m1.size2() == m2.size2());
    }

    static inline
    void
    check_equal_size2(const matrix_type& m1, const matrix_type& m2)
    {
      CHECK_SIZE(m1, m2, m1.size2() == m2.size2());
    }

    static inline
    void
    check_equal_size1(const matrix_type& m, const vector_type& v)
    {
      CHECK_SIZE(m, v, m.size1() == v.size());
    }

    static inline
    void
    check_equal_size2(const matrix_type& m, const vector_type& v)
    {
      CHECK_SIZE(m, v, m.size2() == v.size());
    }

    /*---------.
    | Matrix.  |
    `---------*/

    Matrix::Matrix()
      : value_()
    {
      proto_add(proto);
    }

    Matrix::Matrix(size_t i, size_t j)
      : value_(i, j)
    {
      proto_add(proto);
    }

    Matrix::Matrix(value_type v)
      : value_(v)
    {
      proto_add(proto);
    }

    Matrix::Matrix(const Matrix& model)
      : CxxObject()
      , super_comparable_type()
      , value_(model.value_)
    {
      proto_add(proto);
    }

    Matrix::Matrix(const rMatrix& model)
      : CxxObject()
      , value_(model->value_)
    {
      proto_add(proto);
    }

    Matrix::Matrix(const objects_type& model)
      : CxxObject()
      , value_()
    {
      proto_add(proto);
      fromList(model);
    }

    rMatrix
    Matrix::init(const objects_type& args)
    {
      rMatrix self = args[0]->as<Matrix>();
      if (!self)
        runner::raise_type_error(args[1], Matrix::proto);

      if (args.size() == 1)
        runner::raise_arity_error(0, 1);
      else if (args.size() == 2)
      {
        // Lists of non-floats.
        if (rList l = args[1]->as<List>())
        {
          const objects_type& list = l->value_get();
          if (list.empty() || !list[0]->as<Float>())
          {
            self->fromList(list);
            return self;
          }
          // Process list of floats such as [1, 2] in the fallback,
          // i.e., as [[1, 2]] so that we support "init([1], [2])" but
          // also "init([1, 2])".
        }
        else if (rMatrix v = args[1]->as<Matrix>())
        {
          self->value_ = v->value_;
          return self;
        }
      }
      else if (args.size() == 3
               && args[1]->as<Float>()
               && args[2]->as<Float>())
      {
        self->value_ = create_zeros(self,
                                    from_urbi<unsigned>(args[1]),
                                    from_urbi<unsigned>(args[2]));
        return self;
      }

      objects_type effective_args(args.begin() + 1, args.end());
      return self->fromList(effective_args);
    }

    Matrix::value_type
    Matrix::create_zeros(rObject, size_t size1, size_t size2)
    {
      return ublas::zero_matrix<ufloat>(size1, size2);
    }

    Matrix::value_type
    Matrix::create_identity(rObject, size_t size)
    {
      return ublas::identity_matrix<ufloat>(size);
    }

    Matrix::value_type
    Matrix::create_scalars(rObject, size_t size1, size_t size2, ufloat v)
    {
      return ublas::scalar_matrix<ufloat>(size1, size2, v);
    }

    Matrix::value_type
    Matrix::create_ones(rObject self, size_t size1, size_t size2)
    {
      return create_scalars(self, size1, size2, 1.0);
    }

    Matrix::value_type
    Matrix::transpose() const
    {
      return trans(value_);
    }

    Matrix::value_type
    Matrix::inverse() const
    {
      FORWARD_EXCEPTION(return ublas::inverse(value_));
    }

    Matrix::value_type
    Matrix::dot_times(const value_type& m) const
    {
      value_type res(size1(), size2());
      for (size_t i = 0; i < size1(); ++i)
        for (size_t j = 0; j < size2(); ++j)
          res(i, j) = value_(i, j) * m(i, j);
      return res;
    }

    Matrix*
    Matrix::fromList(const objects_type& args)
    {
      size_type width;
      // For the first iteration, register and the matrix width and
      // resize, for following iterations make sure the sizes are
      // correct.
#define CHECK_WIDTH(Width)                                              \
      do {                                                              \
        size_t _width = Width;                                          \
        if (!i)                                                         \
        {                                                               \
          width = _width;                                               \
          value_.resize(height, width, false);                          \
        }                                                               \
        else if (width != _width)                                       \
          FRAISE("expecting rows of size %d, got size %d for row %d",   \
                 width, _width, i + 1);                                 \
      } while (false)

      const size_type height = args.size();
      for (unsigned i = 0; i < height; ++i)
      {
        if (rList l = args[i]->as<List>())
        {
          CHECK_WIDTH(l->size());
          unsigned j = 0;
          foreach (const rObject& o, l->value_get())
          {
            if (rFloat f = o->as<Float>())
              value_(i, j) = f->value_get();
            else
              runner::raise_type_error(args[i], Float::proto);
            ++j;
          }
        }
        else if (rVector l = args[i]->as<Vector>())
        {
          CHECK_WIDTH(l->size());
          unsigned j = 0;
          foreach (ufloat v, l->value_get())
            value_(i, j++) = v;
        }
        else
          runner::raise_type_error(args[i], List::proto);
      }
      return this;
#undef CHECK_WIDTH
    }

// FIXME: We should be able to return value_type, but then, it is
// bind_variadic that does not manage to bind minus and so forth.
// In that case, get rid of Conversion.
#define OP(Op, Res, Conversion, Name, Sym)                      \
    Res                                                         \
    Matrix::Name(const objects_type& args)                      \
    {                                                           \
      rMatrix self = args[0]->as<Matrix>();                     \
      check_arg_count(args, 1);                                 \
      if (rFloat f = args[1]->as<Float>())                      \
        return Conversion(self->operator Op(f->value_get()));   \
      else if (rMatrix m = args[1]->as<Matrix>())               \
        return Conversion(self->operator Op(m->value_get()));   \
      runner::raise_argument_type_error                         \
        (1, args[1], Matrix::proto, to_urbi(SYMBOL(Sym)));      \
    }

    OP(+,  Matrix*, new Matrix, plus,         PLUS)
    OP(-,  Matrix*, new Matrix, minus,        MINUS)
    OP(/,  Matrix*, new Matrix, div,          SLASH)
    OP(*,  Matrix*, new Matrix, times,        STAR)

    OP(+=, Matrix*,           , plus_assign,  PLUS_EQ)
    OP(-=, Matrix*,           , minus_assign, MINUS_EQ)
    OP(/=, Matrix*,           , div_assign,   SLASH_EQ)
    OP(*=, Matrix*,           , times_assign, STAR_EQ)

#undef OP

    /*------------------------------------------------------.
    | Arithmetic and in-place arithmetic between matrices.  |
    `------------------------------------------------------*/

#define OP(Op)                                          \
    Matrix::value_type                                  \
    Matrix::operator Op(const value_type& m) const      \
    {                                                   \
      check_equal_size(value_, m);                      \
      value_type res(value_);                           \
      res Op##= m;                                      \
      return res;                                       \
    }                                                   \
                                                        \
    Matrix*                                             \
    Matrix::operator Op##=(const value_type& m)         \
    {                                                   \
      check_equal_size(value_, m);                      \
      value_ Op##= m;                                   \
      return this;                                      \
    }

    OP(+)
    OP(-)
#undef OP

    Matrix::value_type
    Matrix::operator /(const value_type& rhs) const
    {
      FORWARD_EXCEPTION(return prod(value_, ublas::inverse(rhs)));
    }

    Matrix*
    Matrix::operator /=(const value_type& rhs)
    {
      FORWARD_EXCEPTION(value_ = prod(value_, ublas::inverse(rhs)));
      return this;
    }

    Matrix::value_type
    Matrix::operator *(const value_type& rhs) const
    {
      CHECK_SIZE(value_, rhs, value_.size2() == rhs.size1());
      return prod(value_, rhs);
    }

    Matrix*
    Matrix::operator *=(const value_type& rhs)
    {
      value_ = *this * rhs;
      return this;
    }

    /*--------------------------.
    | Arithmetic between rows.  |
    `--------------------------*/

#define OP(Name, Op)                            \
    Matrix::value_type                          \
    Matrix::Name(const vector_type& rhs) const  \
    {                                           \
      check_equal_size1(value_, rhs);           \
      const size_t height = size1();            \
      const size_t width = size2();             \
      value_type res = value_;                  \
      for (unsigned i = 0; i < height; ++i)     \
        for (unsigned j = 0; j < width; ++j)    \
          res(i, j) Op ## = rhs(i);             \
      return res;                               \
    }

    OP(rowAdd, +)
    OP(rowSub, -)
    OP(rowMul, *)
    OP(rowDiv, /)
#undef OP

    /*--------------------------.
    | Arithmetic with scalars.  |
    `--------------------------*/

#define OP(Op)                                  \
    Matrix::value_type                          \
    Matrix::operator Op(ufloat s) const         \
    {                                           \
      value_type res(value_);                   \
      res Op##= s;                              \
      return res;                               \
    }                                           \
                                                \
    Matrix*                                     \
    Matrix::operator Op##=(ufloat s)            \
    {                                           \
      value_ Op##= s;                           \
      return this;                              \
    }

    OP(*)
    OP(/)

#undef OP

#define OP(Op)                                                  \
    Matrix::value_type                                          \
    Matrix::operator Op(ufloat s) const                         \
    {                                                           \
      value_type copy(value_);                                  \
      value_type ones = create_scalars(0, size1(), size2(), s); \
      value_type res = copy Op ones;                            \
      return res;                                               \
    }                                                           \
                                                                \
    Matrix*                                                     \
    Matrix::operator Op##=(ufloat s)                            \
    {                                                           \
      value_ = operator Op(s);                                  \
      return this;                                              \
    }

    OP(+)
    OP(-)
#undef OP

    URBI_CXX_OBJECT_INIT(Matrix)
      : value_()
    {
      BIND_VARIADIC(MINUS, minus);
      BIND_VARIADIC(MINUS_EQ, minus_assign);
      BIND_VARIADIC(PLUS, plus);
      BIND_VARIADIC(PLUS_EQ, plus_assign);
      BIND_VARIADIC(SLASH, div);
      BIND_VARIADIC(SLASH_EQ, div_assign);
      BIND_VARIADIC(STAR, times);
      BIND_VARIADIC(STAR_EQ, times_assign);

      //BIND(dot_times, dotWiseMult);
      //BIND(solve);
      BIND(EQ_EQ, operator==, bool, (const rObject&) const);
      BIND(SBL_SBR, operator());
      BIND(SBL_SBR_EQ, set);
      BIND(appendRow);
      BIND(asPrintable);
      BIND(asString);
      BIND(asTopLevelPrintable);
      BIND(column);
      BIND(createIdentity, create_identity);
      BIND(createOnes, create_ones);
      BIND(createScalars, create_scalars);
      BIND(createZeros, create_zeros);
      BIND(distanceMatrix);
      BIND(inverse);
      BIND(resize);
      BIND(row);
      BIND(rowAdd);
      BIND(rowDiv);
      BIND(rowMul);
      BIND(rowNorm);
      BIND(rowSub);
      BIND(set, fromList);
      BIND(setRow);
      BINDG(size, size, rObject, () const);
      BIND(transpose);
      BIND(uvalueSerialize);
      slot_set_value(SYMBOL(init), new Primitive(&init));
    }

    std::string
    Matrix::asString() const
    {
      return make_string('<', '>', "<", ">");
    }

    std::string
    Matrix::asPrintable() const
    {
      return make_string('[', ']', "Matrix([", "])");
    }

    std::string
    Matrix::make_string(char col_lsep, char col_rsep,
                        const std::string row_lsep,
                        const std::string row_rsep) const
    {
      const size_t height = size1();
      const size_t width = size2();

      std::ostringstream s;
      s << row_lsep;
      for (unsigned i = 0; i < height; ++i)
      {
        if (i)
          s << ", ";
        s << col_lsep;
        for (unsigned j = 0; j < width; ++j)
        {
          if (j)
            s << ", ";
          s << value_(i, j);
        }
        s << col_rsep;
      }
      s << row_rsep;
      return s.str();
    }

    std::string
    Matrix::asTopLevelPrintable() const
    {
      const size_t height = size1();
      const size_t width = size2();

      std::ostringstream s;
      s << "Matrix([";

      for (unsigned i = 0; i < height; ++i)
      {
        if (i)
          s << ',';
        s << std::endl;
        s << "  [";
        for (unsigned j = 0; j < width; ++j)
        {
          if (j)
            s << ", ";
          s << value_(i, j);
        }
        s << "]";
      }

      s << "])";

      return s.str();
    }

    ufloat
    Matrix::operator()(int i, int j) const
    {
      return get(i, j);
    }

    ufloat
    Matrix::set(int i, int j, ufloat v)
    {
      return value_(index1(i), index2(j)) = v;
    }

    ufloat
    Matrix::get(int i, int j) const
    {
      return value_(index1(i), index2(j));
    }

    rObject
    Matrix::size() const
    {
      CAPTURE_GLOBAL(Pair);
      return Pair->call(SYMBOL(new), new Float(size1()), new Float(size2()));
    }

    Matrix::vector_type
    Matrix::row(int i) const
    {
      return boost::numeric::ublas::row(value_, index1(i));
    }

    Matrix::vector_type
    Matrix::column(int i) const
    {
      return boost::numeric::ublas::column(value_, index2(i));
    }

    Matrix::value_type
    Matrix::distanceMatrix(const value_type& b) const
    {
      check_equal_size2(value_, b);
      const size_t height = size1();
      const size_t width = size2();
      const size_t height2 = b.size1();
      value_type res(height, height2);
      for (unsigned p1 = 0; p1 < height; ++p1)
        for (unsigned p2 = 0; p2 < height2; ++p2)
        {
          ufloat v = 0;
          for (unsigned i=0; i < width; ++i)
          {
            ufloat t = value_(p1, i) - b(p2, i);
            v += t*t;
          }
          res(p1, p2) = sqrt(v);
        }
      return res;
    }

    Matrix::vector_type
    Matrix::rowNorm() const
    {
      // FIXME: there might be something in Boost to do that.
      const size_t height = size1();
      const size_t width = size2();
      vector_type res(height);
      for (unsigned p1 = 0; p1<height; ++p1)
      {
        ufloat v = 0;
        for (unsigned i=0; i<width; ++i)
        {
          ufloat t = value_(p1, i);
          v += t*t;
        }
        res[p1] = sqrt(v);
      }
      return res;
    }

    Matrix*
    Matrix::setRow(int r, const vector_type& v)
    {
      check_equal_size2(value_, v);
      int j = index1(r);
      for (unsigned i = 0; i< v.size(); ++i)
        value_(j, i) = v(i);
      return this;
    }

    Matrix*
    Matrix::appendRow(const vector_type& v)
    {
      check_equal_size2(value_, v);
      value_.resize(size1()+1, size2());
      setRow(size1()-1, v);
      return this;
    }

    Matrix*
    Matrix::resize(size_t ns1, size_t ns2)
    {
      size_t s1 = value_.size1();
      size_t s2 = value_.size2();
      value_.resize(ns1, ns2);
      // Boost does not ensure that we have 0 for new members.
      for (size_t i = 0; i < ns1; ++i)
        for (size_t j = i < s1 ? s2 : 0; j < ns2; ++j)
          value_(i, j) = 0;
      return this;
    }

    rObject
    Matrix::uvalueSerialize() const
    {
      CAPTURE_GLOBAL(Binary);
      // This is ugly: we cannot go through object-cast as it would give us
      // back the vector. So make the Binary ourselve.
      // This is also a duplicate of Vector::uvalueSerialize.
      rObject o(const_cast<Matrix*>(this));
      urbi::UValue v = ::uvalue_cast(o);
      rObject res = new object::Object();
      res->proto_add(Binary);
      res->slot_set_value(SYMBOL(keywords),
                    new object::String(v.binary->getMessage()));
      res->slot_set_value(SYMBOL(data),
                    new object::String
                    (std::string(static_cast<char*>(v.binary->common.data),
				 v.binary->common.size)));
      return res;
    }
  }
}


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

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../example_func_xy_jacobian.h"
#include "../../constraints_jacobian.h"
#include "../../relative_pose_tait_bryan_wc_jacobian.h"
#include "../../point_to_point_source_to_target_tait_bryan_wc_jacobian.h"
#include "../include/cauchy.h"
#include "../../smoothness_tait_bryan_wc_jacobian.h"

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

std::vector<Eigen::Vector3d> reference_points;

std::vector<Eigen::Affine3d> vertices;
std::vector<Eigen::Affine3d> vertices_odo;
int number_rows = 0;
int number_cols = 0;
std::vector<std::pair<int, int>> odo_edges;

struct TripletIndexes{
	int index_before;
	int index_curr;
	int index_after;
};

std::vector<TripletIndexes> smoothness_indexes;

bool find_nearest_neighbour(Eigen::Vector3d &p_t, Eigen::Affine3d vertex, const std::vector<Eigen::Vector3d>& reference_points){
	double min_dist_xy = 1000000.0;
	double search_radious = 1.0;
	bool found = false;

	for(size_t i = 0 ; i < reference_points.size(); i++){
		float dist = sqrt ( (vertex(0,3) - reference_points[i].x()) * (vertex(0,3) - reference_points[i].x()) + (vertex(1,3) - reference_points[i].y()) * (vertex(1,3) - reference_points[i].y()) );

		if( dist < search_radious  ) {
			if( dist < min_dist_xy){
				min_dist_xy = dist;
				p_t = reference_points[i];
				p_t.x() = vertex(0,3) + ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
				p_t.y() = vertex(1,3) + ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
				found = true;
			}
		}
	}
	return found;
}

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

	for(size_t i = 0; i < 10000; i++){
		double x = ((float(rand()%1000000)/1000000.0f - 0.5) * 2.0) * 20.0;
		double y = ((float(rand()%1000000)/1000000.0f - 0.5) * 2.0) * 20.0;
		double z;
		example_func_xy(z, x, y);

		if( (x-4)*(x-4) + (y-5)*(y-5) > 49){
			reference_points.emplace_back(x,y,z*10 + ((float(rand()%1000000)/1000000.0f - 0.5) * 2.0) * 0.1);
		}
	}

	for(int x = -20; x <= 20; x += 1){
		for(int y = -20; y <= 20; y += 1){
			if(number_rows == 0){
				number_cols ++;
			}
			Eigen::Affine3d pose = Eigen::Affine3d::Identity();
			pose(0,3) = x;
			pose(1,3) = y;
			pose(2,3) = -10;
			vertices.push_back(pose);
		}
		number_rows ++;
	}
	vertices_odo = vertices;

	for(int row = 0; row < number_rows; row++){
		for(int col = 0; col < number_cols; col++){
			int index = row + col * number_rows;
			if(row + 1 < number_rows){
				int index_next_row = (row + 1) + col * number_rows;
				odo_edges.emplace_back(index, index_next_row);
			}

			if(col + 1 < number_cols){
				int index_next_col = (row) + (col + 1) * number_rows;
				odo_edges.emplace_back(index, index_next_col);
			}

			if(row + 1 < number_rows && row - 1 >= 0){
				int index_prev_row = (row - 1) + col * number_rows;
				int index_next_row = (row + 1) + col * number_rows;
				TripletIndexes tr;
				tr.index_curr = index;
				tr.index_before = index_prev_row;
				tr.index_after = index_next_row;
				smoothness_indexes.push_back(tr);
			}
			if(col + 1 < number_cols && col - 1 >= 0){
				int index_next_col = (row) + (col + 1) * number_rows;
				int index_prev_col = (row) + (col - 1) * number_rows;
				TripletIndexes tr;
				tr.index_curr = index;
				tr.index_before = index_prev_col;
				tr.index_after = index_next_col;
				smoothness_indexes.push_back(tr);
			}
		}
	}

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

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

	glPointSize(2);
	glColor3f(0,0,0);
	glBegin(GL_POINTS);
	for(int row = 0; row < number_rows; row++){
		for(int col = 0; col < number_cols; col++){
			int index = row + col * number_rows;
			glVertex3f(vertices[index](0,3), vertices[index](1,3), vertices[index](2,3));
		}
	}
	glEnd();
	glPointSize(1);

	glBegin(GL_LINES);
	for(size_t i = 0; i < odo_edges.size(); i++){
		glVertex3f(vertices[odo_edges[i].first](0,3), vertices[odo_edges[i].first](1,3), vertices[odo_edges[i].first](2,3));
		glVertex3f(vertices[odo_edges[i].second](0,3), vertices[odo_edges[i].second](1,3), vertices[odo_edges[i].second](2,3));
	}
	glEnd();

	glutSwapBuffers();
}

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

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

			std::vector<TaitBryanPose> poses;
			std::vector<TaitBryanPose> poses_odo;

			for(size_t i = 0 ; i < vertices.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(vertices[i]));
			}
			for(size_t i = 0 ; i < vertices_odo.size(); i++){
				poses_odo.push_back(pose_tait_bryan_from_affine_matrix(vertices_odo[i]));
			}

			for(size_t i = 0 ; i < odo_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
						poses_odo[odo_edges[i].first].px,
						poses_odo[odo_edges[i].first].py,
						poses_odo[odo_edges[i].first].pz,
						poses_odo[odo_edges[i].first].om,
						poses_odo[odo_edges[i].first].fi,
						poses_odo[odo_edges[i].first].ka,
						poses_odo[odo_edges[i].second].px,
						poses_odo[odo_edges[i].second].py,
						poses_odo[odo_edges[i].second].pz,
						poses_odo[odo_edges[i].second].om,
						poses_odo[odo_edges[i].second].fi,
						poses_odo[odo_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka,
						relative_pose_measurement_odo(0,0),
						relative_pose_measurement_odo(1,0),
						relative_pose_measurement_odo(2,0),
						relative_pose_measurement_odo(3,0),
						relative_pose_measurement_odo(4,0),
						relative_pose_measurement_odo(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = odo_edges[i].first * 6;
				int ic_2 = odo_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

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

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

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

			for(size_t i = 0; i < vertices.size(); i++){
				TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(vertices[i]);

				Eigen::Vector3d p_t;//(georeference_data[i].first(0,3), georeference_data[i].first(1,3), georeference_data[i].first(2,3));
				if(!find_nearest_neighbour(p_t, vertices[i], reference_points)){
					continue;
				}

				int ir = tripletListB.size();

				Eigen::Vector3d p_s(0,0,0);
				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, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.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, p_s.x(), p_s.y(), p_s.z());

				int ic = i * 6;
				tripletListA.emplace_back(ir     , ic + 0, -jacobian(0,0));
				tripletListA.emplace_back(ir + 1 , ic + 1, -jacobian(1,1));
				tripletListA.emplace_back(ir + 2 , ic + 2, -jacobian(2,2));

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

				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(), vertices.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(vertices.size() * 6 , vertices.size() * 6);
			Eigen::SparseMatrix<double> AtPB(vertices.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());
				}
			}

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

			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * vertices.size()){
				int counter = 0;

				for(size_t i = 0; i < vertices.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(vertices[i]);
					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++];
					vertices[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan FAILED" << std::endl;
			}

			break;
		}
		case 'y':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;
			std::vector<TaitBryanPose> poses_odo;

			for(size_t i = 0 ; i < vertices.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(vertices[i]));
			}
			for(size_t i = 0 ; i < vertices_odo.size(); i++){
				poses_odo.push_back(pose_tait_bryan_from_affine_matrix(vertices_odo[i]));
			}

			for(size_t i = 0 ; i < odo_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
						poses_odo[odo_edges[i].first].px,
						poses_odo[odo_edges[i].first].py,
						poses_odo[odo_edges[i].first].pz,
						poses_odo[odo_edges[i].first].om,
						poses_odo[odo_edges[i].first].fi,
						poses_odo[odo_edges[i].first].ka,
						poses_odo[odo_edges[i].second].px,
						poses_odo[odo_edges[i].second].py,
						poses_odo[odo_edges[i].second].pz,
						poses_odo[odo_edges[i].second].om,
						poses_odo[odo_edges[i].second].fi,
						poses_odo[odo_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka,
						relative_pose_measurement_odo(0,0),
						relative_pose_measurement_odo(1,0),
						relative_pose_measurement_odo(2,0),
						relative_pose_measurement_odo(3,0),
						relative_pose_measurement_odo(4,0),
						relative_pose_measurement_odo(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = odo_edges[i].first * 6;
				int ic_2 = odo_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

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

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

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

			for(size_t i = 0; i < vertices.size(); i++){
				TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(vertices[i]);

				Eigen::Vector3d p_t;
				if(!find_nearest_neighbour(p_t, vertices[i], reference_points)){
					continue;
				}

				int ir = tripletListB.size();
				int ic = i * 6;
				double delta;
				double a = 1;
				Eigen::Matrix<double, 1, 1> jacobian;
				observation_equation_constraint(delta, a, vertices[i](0,3), p_t.x());
				observation_equation_constraint_jacobian(jacobian, a, vertices[i](0,3), p_t.x());

				tripletListA.emplace_back(ir, ic,  -jacobian(0,0));
				tripletListP.emplace_back(ir, ir,  1);
				tripletListB.emplace_back(ir, 0,  delta);

				observation_equation_constraint(delta, a, vertices[i](1,3), p_t.y());
				observation_equation_constraint_jacobian(jacobian, a, vertices[i](1,3), p_t.y());

				tripletListA.emplace_back(ir + 1, ic + 1,  -jacobian(0,0));
				tripletListP.emplace_back(ir + 1, ir + 1,  1);
				tripletListB.emplace_back(ir + 1, 0,  delta);

				observation_equation_constraint(delta, a, vertices[i](2,3), p_t.z());
				observation_equation_constraint_jacobian(jacobian, a, vertices[i](2,3), p_t.z());

				tripletListA.emplace_back(ir + 2, ic + 2,  -jacobian(0,0));
				tripletListP.emplace_back(ir + 2, ir + 2,  1);
				tripletListB.emplace_back(ir + 2, 0,  delta);
			}

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

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

			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * vertices.size()){
				int counter = 0;

				for(size_t i = 0; i < vertices.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(vertices[i]);
					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++];
					vertices[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan FAILED" << std::endl;
			}

			break;
		}
		case 'x':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;

			for(size_t i = 0 ; i < vertices.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(vertices[i]));
			}

			for(size_t i = 0; i < smoothness_indexes.size(); i++){
				Eigen::Matrix<double, 6, 1> delta;
				smoothness_obs_eq_tait_bryan_wc(delta,
						poses[smoothness_indexes[i].index_before].px,
						poses[smoothness_indexes[i].index_before].py,
						poses[smoothness_indexes[i].index_before].pz,
						poses[smoothness_indexes[i].index_before].om,
						poses[smoothness_indexes[i].index_before].fi,
						poses[smoothness_indexes[i].index_before].ka,
						poses[smoothness_indexes[i].index_curr].px,
						poses[smoothness_indexes[i].index_curr].py,
						poses[smoothness_indexes[i].index_curr].pz,
						poses[smoothness_indexes[i].index_curr].om,
						poses[smoothness_indexes[i].index_curr].fi,
						poses[smoothness_indexes[i].index_curr].ka,
						poses[smoothness_indexes[i].index_after].px,
						poses[smoothness_indexes[i].index_after].py,
						poses[smoothness_indexes[i].index_after].pz,
						poses[smoothness_indexes[i].index_after].om,
						poses[smoothness_indexes[i].index_after].fi,
						poses[smoothness_indexes[i].index_after].ka);

				Eigen::Matrix<double, 6, 18, Eigen::RowMajor> jacobian;
				smoothness_obs_eq_tait_bryan_wc_jacobian(jacobian,
						poses[smoothness_indexes[i].index_before].px,
						poses[smoothness_indexes[i].index_before].py,
						poses[smoothness_indexes[i].index_before].pz,
						poses[smoothness_indexes[i].index_before].om,
						poses[smoothness_indexes[i].index_before].fi,
						poses[smoothness_indexes[i].index_before].ka,
						poses[smoothness_indexes[i].index_curr].px,
						poses[smoothness_indexes[i].index_curr].py,
						poses[smoothness_indexes[i].index_curr].pz,
						poses[smoothness_indexes[i].index_curr].om,
						poses[smoothness_indexes[i].index_curr].fi,
						poses[smoothness_indexes[i].index_curr].ka,
						poses[smoothness_indexes[i].index_after].px,
						poses[smoothness_indexes[i].index_after].py,
						poses[smoothness_indexes[i].index_after].pz,
						poses[smoothness_indexes[i].index_after].om,
						poses[smoothness_indexes[i].index_after].fi,
						poses[smoothness_indexes[i].index_after].ka);

				int ir = tripletListB.size();

				int ic_1 = smoothness_indexes[i].index_before * 6;
				int ic_2 = smoothness_indexes[i].index_curr * 6;
				int ic_3 = smoothness_indexes[i].index_after * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

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

					tripletListA.emplace_back(ir + row, ic_3    , -jacobian(row,12));
					tripletListA.emplace_back(ir + row, ic_3 + 1, -jacobian(row,13));
					tripletListA.emplace_back(ir + row, ic_3 + 2, -jacobian(row,14));
					tripletListA.emplace_back(ir + row, ic_3 + 3, -jacobian(row,15));
					tripletListA.emplace_back(ir + row, ic_3 + 4, -jacobian(row,16));
					tripletListA.emplace_back(ir + row, ic_3 + 5, -jacobian(row,17));
				}
				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

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

			for(size_t i = 0; i < vertices.size(); i++){
				TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(vertices[i]);

				Eigen::Vector3d p_t;//(georeference_data[i].first(0,3), georeference_data[i].first(1,3), georeference_data[i].first(2,3));
				if(!find_nearest_neighbour(p_t, vertices[i], reference_points)){
					continue;
				}

				int ir = tripletListB.size();

				Eigen::Vector3d p_s(0,0,0);
				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, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.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, p_s.x(), p_s.y(), p_s.z());

				int ic = i * 6;
				tripletListA.emplace_back(ir     , ic + 0, -jacobian(0,0));
				tripletListA.emplace_back(ir + 1 , ic + 1, -jacobian(1,1));
				tripletListA.emplace_back(ir + 2 , ic + 2, -jacobian(2,2));

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

				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(), vertices.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(vertices.size() * 6 , vertices.size() * 6);
			Eigen::SparseMatrix<double> AtPB(vertices.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());
				}
			}

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

			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * vertices.size()){
				int counter = 0;

				for(size_t i = 0; i < vertices.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(vertices[i]);
					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++];
					vertices[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan FAILED" << std::endl;
			}

			break;
		}
		case 'z':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;

			for(size_t i = 0 ; i < vertices.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(vertices[i]));
			}

			for(size_t i = 0; i < smoothness_indexes.size(); i++){
				Eigen::Matrix<double, 6, 1> delta;
				smoothness_obs_eq_tait_bryan_wc(delta,
						poses[smoothness_indexes[i].index_before].px,
						poses[smoothness_indexes[i].index_before].py,
						poses[smoothness_indexes[i].index_before].pz,
						poses[smoothness_indexes[i].index_before].om,
						poses[smoothness_indexes[i].index_before].fi,
						poses[smoothness_indexes[i].index_before].ka,
						poses[smoothness_indexes[i].index_curr].px,
						poses[smoothness_indexes[i].index_curr].py,
						poses[smoothness_indexes[i].index_curr].pz,
						poses[smoothness_indexes[i].index_curr].om,
						poses[smoothness_indexes[i].index_curr].fi,
						poses[smoothness_indexes[i].index_curr].ka,
						poses[smoothness_indexes[i].index_after].px,
						poses[smoothness_indexes[i].index_after].py,
						poses[smoothness_indexes[i].index_after].pz,
						poses[smoothness_indexes[i].index_after].om,
						poses[smoothness_indexes[i].index_after].fi,
						poses[smoothness_indexes[i].index_after].ka);

				Eigen::Matrix<double, 6, 18, Eigen::RowMajor> jacobian;
				smoothness_obs_eq_tait_bryan_wc_jacobian(jacobian,
						poses[smoothness_indexes[i].index_before].px,
						poses[smoothness_indexes[i].index_before].py,
						poses[smoothness_indexes[i].index_before].pz,
						poses[smoothness_indexes[i].index_before].om,
						poses[smoothness_indexes[i].index_before].fi,
						poses[smoothness_indexes[i].index_before].ka,
						poses[smoothness_indexes[i].index_curr].px,
						poses[smoothness_indexes[i].index_curr].py,
						poses[smoothness_indexes[i].index_curr].pz,
						poses[smoothness_indexes[i].index_curr].om,
						poses[smoothness_indexes[i].index_curr].fi,
						poses[smoothness_indexes[i].index_curr].ka,
						poses[smoothness_indexes[i].index_after].px,
						poses[smoothness_indexes[i].index_after].py,
						poses[smoothness_indexes[i].index_after].pz,
						poses[smoothness_indexes[i].index_after].om,
						poses[smoothness_indexes[i].index_after].fi,
						poses[smoothness_indexes[i].index_after].ka);

				int ir = tripletListB.size();

				int ic_1 = smoothness_indexes[i].index_before * 6;
				int ic_2 = smoothness_indexes[i].index_curr * 6;
				int ic_3 = smoothness_indexes[i].index_after * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

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

					tripletListA.emplace_back(ir + row, ic_3    , -jacobian(row,12));
					tripletListA.emplace_back(ir + row, ic_3 + 1, -jacobian(row,13));
					tripletListA.emplace_back(ir + row, ic_3 + 2, -jacobian(row,14));
					tripletListA.emplace_back(ir + row, ic_3 + 3, -jacobian(row,15));
					tripletListA.emplace_back(ir + row, ic_3 + 4, -jacobian(row,16));
					tripletListA.emplace_back(ir + row, ic_3 + 5, -jacobian(row,17));
				}
				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));
				tripletListB.emplace_back(ir + 3, 0, delta(3,0));
				tripletListB.emplace_back(ir + 4, 0, delta(4,0));
				tripletListB.emplace_back(ir + 5, 0, delta(5,0));

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

			for(size_t i = 0; i < vertices.size(); i++){
				TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(vertices[i]);

				Eigen::Vector3d p_t;
				if(!find_nearest_neighbour(p_t, vertices[i], reference_points)){
					continue;
				}

				int ir = tripletListB.size();
				int ic = i * 6;
				double delta;
				double a = 1;
				Eigen::Matrix<double, 1, 1> jacobian;
				observation_equation_constraint(delta, a, vertices[i](0,3), p_t.x());
				observation_equation_constraint_jacobian(jacobian, a, vertices[i](0,3), p_t.x());

				tripletListA.emplace_back(ir, ic,  -jacobian(0,0));
				tripletListP.emplace_back(ir, ir,  1);
				tripletListB.emplace_back(ir, 0,  delta);

				observation_equation_constraint(delta, a, vertices[i](1,3), p_t.y());
				observation_equation_constraint_jacobian(jacobian, a, vertices[i](1,3), p_t.y());

				tripletListA.emplace_back(ir + 1, ic + 1,  -jacobian(0,0));
				tripletListP.emplace_back(ir + 1, ir + 1,  1);
				tripletListB.emplace_back(ir + 1, 0,  delta);

				observation_equation_constraint(delta, a, vertices[i](2,3), p_t.z());
				observation_equation_constraint_jacobian(jacobian, a, vertices[i](2,3), p_t.z());

				tripletListA.emplace_back(ir + 2, ic + 2,  -jacobian(0,0));
				tripletListP.emplace_back(ir + 2, ir + 2,  1);
				tripletListB.emplace_back(ir + 2, 0,  delta);
			}

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

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

			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * vertices.size()){
				int counter = 0;

				for(size_t i = 0; i < vertices.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(vertices[i]);
					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++];
					vertices[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan 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 (relative pose constraint and source to target)" << std::endl;
	std::cout << "y: optimize (relative pose constraint and linear function constraint)" << std::endl;
	std::cout << "x: optimize (smoothness constraint and source to target)" << std::endl;
	std::cout << "z: optimize (smoothness constraint and linear function constraint)" << std::endl;
	std::cout << "n: add noise to mesh" << std::endl;
}