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

#include "structures.h"
#include "transformations.h"
#include "cauchy.h"
#include "../../python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h"
#include "../../python-scripts/constraints/relative_pose_rodrigues_wc_jacobian.h"
#include "../../python-scripts/constraints/relative_pose_quaternion_wc_jacobian.h"
#include "../../python-scripts/constraints/quaternion_constraint_jacobian.h"
#include "../../python-scripts/constraints/relative_pose_wc_jacobian.h"
#include "../../python-scripts/constraints/relative_pose_2_tait_bryan_wc_jacobian.h"
#include "../../python-scripts/constraints/relative_pose_2_rodrigues_wc_jacobian.h"
#include "../../python-scripts/constraints/relative_pose_2_quaternion_wc_jacobian.h"

#include "manif/SE3.h"

using manif::SE3d;
using manif::SE3Tangentd;

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 = -10.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::Affine3d> m_poses;
std::vector<std::tuple<int, int, Eigen::Affine3d>> edges_g2o;
std::vector<std::vector<double>> edges_g2o_w;

int main(int argc, char *argv[]){
	if(argc != 2){
		std::cout << "USAGE: " << argv[0] << " file.g2o" << std::endl;
		return 1;
	}

	std::ifstream g2o_file(argv[1]);

	std::string line;
    while (std::getline(g2o_file, line)) {
        std::stringstream line_stream(line);
        std::string class_element;
        line_stream >> class_element;
        if (class_element == "VERTEX_SE3:QUAT"){
            int id=0;
            QuaternionPose p;
            line_stream>> id;
            line_stream >> p.px;
            line_stream >> p.py;
            line_stream >> p.pz;
            line_stream >> p.q1;
            line_stream >> p.q2;
            line_stream >> p.q3;
            line_stream >> p.q0;
            normalize_quaternion(p);

            Eigen::Affine3d m = affine_matrix_from_pose_quaternion(p);
            m_poses.push_back(m);
        }
        else if(class_element == "EDGE_SE3:QUAT"){
            int pose_id1,pose_id2;
            line_stream>>pose_id1;
            line_stream>>pose_id2;
            QuaternionPose p;
            line_stream >> p.px;
            line_stream >> p.py;
            line_stream >> p.pz;
            line_stream >> p.q1;
            line_stream >> p.q2;
            line_stream >> p.q3;
            line_stream >> p.q0;
            Eigen::Affine3d m = affine_matrix_from_pose_quaternion(p);

            edges_g2o.emplace_back(std::make_tuple(pose_id1,pose_id2, m));

            std::vector<double> w(21);
            for(size_t i = 0 ; i < 21; i++){
            	line_stream >> w[i];
            }
            std::vector<double> ww(6);
            ww[0] = w[0];
            ww[1] = w[6];
            ww[2] = w[11];
            ww[3] = w[15];
            ww[4] = w[18];
            ww[5] = w[20];
            edges_g2o_w.push_back(ww);
        }
    }
    g2o_file.close();

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

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

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

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

    glBegin(GL_LINES);
    for(size_t i = 0; i < edges_g2o.size(); i++){
        const int pose_1 = std::get<0>(edges_g2o[i]);
        const int pose_2 = std::get<1>(edges_g2o[i]);
        glVertex3f(m_poses[pose_1](0,3), m_poses[pose_1](1,3), m_poses[pose_1](2,3) );
        glVertex3f(m_poses[pose_2](0,3), m_poses[pose_2](1,3), m_poses[pose_2](2,3) );
    }
    glEnd();

    glColor3f(1,0,1);
    glPointSize(5);
    glBegin(GL_POINTS);
    for(size_t i = 0; i < m_poses.size(); i++){
        glVertex3f(m_poses[i](0,3), m_poses[i](1,3), m_poses[i](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 < m_poses.size(); i++){
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
				pose.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 1.1;
				pose.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 1.1;
				pose.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 1.1;
				pose.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.11;
				pose.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.11;
				pose.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.11;
				m_poses[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;

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

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);
				TaitBryanPose pose_rel = pose_tait_bryan_from_affine_matrix(rel);

				TaitBryanPose from = poses[first];
				TaitBryanPose to = poses[second];

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
					delta,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].om,
					poses[first].fi,
					poses[first].ka,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].om,
					poses[second].fi,
					poses[second].ka,
					pose_rel.px,
					pose_rel.py,
					pose_rel.pz,
					pose_rel.om,
					pose_rel.fi,
					pose_rel.ka);

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].om,
					poses[first].fi,
					poses[first].ka,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].om,
					poses[second].fi,
					poses[second].ka);
				int ir = tripletListB.size();

				int ic_1 = first * 6;
				int ic_2 = 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));
				}

				float om_diff = delta(3,0);
				if(fabs(om_diff) > M_PI){
					om_diff -= 2.0*M_PI;
				}
				if(fabs(om_diff)< -M_PI){
					om_diff += 2.0*M_PI;
				}

				float fi_diff = delta(4,0);
				if(fabs(fi_diff) > M_PI){
					fi_diff -= 2.0*M_PI;
				}
				if(fabs(fi_diff)< -M_PI){
					fi_diff += 2.0*M_PI;
				}

				float ka_diff = delta(5,0);
				if(fabs(ka_diff) > M_PI){
					ka_diff -= 2.0*M_PI;
				}
				if(fabs(ka_diff)< -M_PI){
					ka_diff += 2.0*M_PI;
				}

				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, om_diff);
				tripletListB.emplace_back(ir + 4, 0, fi_diff);
				tripletListB.emplace_back(ir + 5, 0, ka_diff);

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(om_diff,1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(fi_diff,1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(ka_diff,1) * edges_g2o_w[i][5]);
			}

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

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

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

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

				for(size_t i = 0; i < m_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[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++];
					m_poses[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 'r':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<RodriguesPose> poses;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				TaitBryanPose p = pose_tait_bryan_from_affine_matrix(m_poses[i]);
					p.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p);
				poses.push_back(pose_rodrigues_from_affine_matrix(m));
			}

            for(size_t i = 0 ; i < edges_g2o.size(); i++){
                const int first = std::get<0>(edges_g2o[i]);
                const int second = std::get<1>(edges_g2o[i]);
                const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);

                RodriguesPose ps = pose_rodrigues_from_affine_matrix(rel);

                Eigen::Matrix<double, 6, 1> delta;
                relative_pose_obs_eq_rodrigues_wc(
                        delta,
                        poses[first].px,
                        poses[first].py,
                        poses[first].pz,
                        poses[first].sx,
                        poses[first].sy,
                        poses[first].sz,
                        poses[second].px,
                        poses[second].py,
                        poses[second].pz,
                        poses[second].sx,
                        poses[second].sy,
                        poses[second].sz,
                        ps.px,
                        ps.py,
                        ps.pz,
                        ps.sx,
                        ps.sy,
                        ps.sz);

                Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
                relative_pose_obs_eq_rodrigues_wc_jacobian(jacobian,
					   poses[first].px,
					   poses[first].py,
					   poses[first].pz,
					   poses[first].sx,
					   poses[first].sy,
					   poses[first].sz,
					   poses[second].px,
					   poses[second].py,
					   poses[second].pz,
					   poses[second].sx,
					   poses[second].sy,
					   poses[second].sz);

                int ir = tripletListB.size();

                int ic_1 = first * 6;
                int ic_2 = 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,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
            }

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

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

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

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

				for(size_t i = 0; i < m_poses.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_poses[i]);
					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++];

					m_poses[i] = affine_matrix_from_pose_rodrigues(pose);

					Eigen::Vector3d vx(m_poses[i](0,0), m_poses[i](1,0), m_poses[i](2,0));
					Eigen::Vector3d vy(m_poses[i](0,1), m_poses[i](1,1), m_poses[i](2,1));
					Eigen::Vector3d vz(m_poses[i](0,2), m_poses[i](1,2), m_poses[i](2,2));

					//std::cout << std::setprecision(15);
					//std::cout << "norm: "<< vx.norm() << " " << vy.norm() << " " << vz.norm() << " " <<
					//		vx.dot(vy) << " " << vy.dot(vz) << " " << vx.dot(vz) << std::endl;
				}
				std::cout << "optimizing with rodrigues finished" << std::endl;
			}else{
				std::cout << "optimizing with rodrigues FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<QuaternionPose> poses;

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

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);
				QuaternionPose pose_rel = pose_quaternion_from_affine_matrix(rel);

				Eigen::Matrix<double, 7, 1> delta;
				relative_pose_obs_eq_quaternion_wc(
					delta,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].q0,
					poses[first].q1,
					poses[first].q2,
					poses[first].q3,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].q0,
					poses[second].q1,
					poses[second].q2,
					poses[second].q3,
					pose_rel.px,
					pose_rel.py,
					pose_rel.pz,
					pose_rel.q0,
					pose_rel.q1,
					pose_rel.q2,
					pose_rel.q3);

				Eigen::Matrix<double, 7, 14, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_quaternion_wc_jacobian(jacobian,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].q0,
					poses[first].q1,
					poses[first].q2,
					poses[first].q3,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].q0,
					poses[second].q1,
					poses[second].q2,
					poses[second].q3);

				int ir = tripletListB.size();

				int ic_1 = first * 7;
				int ic_2 = second * 7;

				for(size_t row = 0 ; row < 7; 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_1 + 6, -jacobian(row,6));

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

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

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
				tripletListP.emplace_back(ir + 6, ir + 6, cauchy(delta(6,0),1) * edges_g2o_w[i][5]);
			}

			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 < m_poses.size(); i++){
				int ic = i * 7;
				ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(m_poses[i]);

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

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

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

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

			if(h_x.size() == 7 * m_poses.size()){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(m_poses[i]);
					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++];
					m_poses[i] = affine_matrix_from_pose_quaternion(pose);
				}
				std::cout << "optimizing with quaternions finished" << std::endl;
			}else{
				std::cout << "optimizing with quaternions FAILED" << std::endl;
			}
			break;
		}
		case 'x':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);

				Eigen::Matrix<double, 12, 1> delta;
				relative_pose_obs_eq_wc(
						delta,
						m_poses[first](0,3),
						m_poses[first](1,3),
						m_poses[first](2,3),
						m_poses[first](0,0),
						m_poses[first](0,1),
						m_poses[first](0,2),
						m_poses[first](1,0),
						m_poses[first](1,1),
						m_poses[first](1,2),
						m_poses[first](2,0),
						m_poses[first](2,1),
						m_poses[first](2,2),
						m_poses[second](0,3),
						m_poses[second](1,3),
						m_poses[second](2,3),
						m_poses[second](0,0),
						m_poses[second](0,1),
						m_poses[second](0,2),
						m_poses[second](1,0),
						m_poses[second](1,1),
						m_poses[second](1,2),
						m_poses[second](2,0),
						m_poses[second](2,1),
						m_poses[second](2,2),
						rel(0,3),
						rel(1,3),
						rel(2,3),
						rel(0,0),
						rel(0,1),
						rel(0,2),
						rel(1,0),
						rel(1,1),
						rel(1,2),
						rel(2,0),
						rel(2,1),
						rel(2,2));

				Eigen::Matrix<double, 12, 24, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_wc_jacobian(jacobian,
						m_poses[first](0,3),
						m_poses[first](1,3),
						m_poses[first](2,3),
						m_poses[first](0,0),
						m_poses[first](0,1),
						m_poses[first](0,2),
						m_poses[first](1,0),
						m_poses[first](1,1),
						m_poses[first](1,2),
						m_poses[first](2,0),
						m_poses[first](2,1),
						m_poses[first](2,2),
						m_poses[second](0,3),
						m_poses[second](1,3),
						m_poses[second](2,3),
						m_poses[second](0,0),
						m_poses[second](0,1),
						m_poses[second](0,2),
						m_poses[second](1,0),
						m_poses[second](1,1),
						m_poses[second](1,2),
						m_poses[second](2,0),
						m_poses[second](2,1),
						m_poses[second](2,2));

				int ir = tripletListB.size();
				int ic_1 = first * 12;
				int ic_2 = second * 12;

				for(size_t row = 0 ; row < 12; 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_1 + 6, -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_1 + 7, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_1 + 8, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_1 + 9, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_1 + 10, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_1 + 11, -jacobian(row,11));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,12));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,13));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,14));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,15));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,16));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,17));
					tripletListA.emplace_back(ir + row, ic_2 + 6, -jacobian(row,18));
					tripletListA.emplace_back(ir + row, ic_2 + 7, -jacobian(row,19));
					tripletListA.emplace_back(ir + row, ic_2 + 8, -jacobian(row,20));
					tripletListA.emplace_back(ir + row, ic_2 + 9, -jacobian(row,21));
					tripletListA.emplace_back(ir + row, ic_2 + 10, -jacobian(row,22));
					tripletListA.emplace_back(ir + row, ic_2 + 11, -jacobian(row,23));
				}

				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));
				tripletListB.emplace_back(ir + 6, 0, delta(6,0));
				tripletListB.emplace_back(ir + 7, 0, delta(7,0));
				tripletListB.emplace_back(ir + 8, 0, delta(8,0));
				tripletListB.emplace_back(ir + 9, 0, delta(9,0));
				tripletListB.emplace_back(ir + 10, 0, delta(10,0));
				tripletListB.emplace_back(ir + 11, 0, delta(11,0));

				tripletListP.emplace_back(ir     , ir,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1 , ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2 , ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3 , ir + 3, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4 , ir + 4, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 5 , ir + 5, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 6 , ir + 6, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 7 , ir + 7, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 8 , ir + 8, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 9 , ir + 9, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
				tripletListP.emplace_back(ir + 10 , ir + 10, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
				tripletListP.emplace_back(ir + 11 , ir + 11, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
			}

			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);
			tripletListA.emplace_back(ir + 7 , 7, 1);
			tripletListA.emplace_back(ir + 8 , 8, 1);
			tripletListA.emplace_back(ir + 9 , 9, 1);
			tripletListA.emplace_back(ir + 10 , 10, 1);
			tripletListA.emplace_back(ir + 11 , 11, 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);
			tripletListP.emplace_back(ir + 7 , ir + 7, 1000000);
			tripletListP.emplace_back(ir + 8 , ir + 8, 1000000);
			tripletListP.emplace_back(ir + 9 , ir + 9, 1000000);
			tripletListP.emplace_back(ir + 10 , ir + 10, 1000000);
			tripletListP.emplace_back(ir + 11 , ir + 11, 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);
			tripletListB.emplace_back(ir + 7 , 0, 0);
			tripletListB.emplace_back(ir + 8 , 0, 0);
			tripletListB.emplace_back(ir + 9 , 0, 0);
			tripletListB.emplace_back(ir + 10 , 0, 0);
			tripletListB.emplace_back(ir + 11 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), m_poses.size() * 12);
			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(m_poses.size() * 12 , m_poses.size() * 12);
			Eigen::SparseMatrix<double> AtPB(m_poses.size() * 12 , 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() == 12 * m_poses.size()){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					m_poses[i](0,3) += h_x[counter++];
					m_poses[i](1,3) += h_x[counter++];
					m_poses[i](2,3) += h_x[counter++];
					m_poses[i](0,0) += h_x[counter++];
					m_poses[i](0,1) += h_x[counter++];
					m_poses[i](0,2) += h_x[counter++];
					m_poses[i](1,0) += h_x[counter++];
					m_poses[i](1,1) += h_x[counter++];
					m_poses[i](1,2) += h_x[counter++];
					m_poses[i](2,0) += h_x[counter++];
					m_poses[i](2,1) += h_x[counter++];
					m_poses[i](2,2) += h_x[counter++];

					orthogonalize_rotation(m_poses[i]);
				}
				std::cout << "optimizing without rotation matrix parametrization finished" << std::endl;
			}else{
				std::cout << "optimizing without rotation matrix parametrization 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<SE3d> X;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				Eigen::Vector3d t(m_poses[i](0,3), m_poses[i](1,3), m_poses[i](2,3));
				Eigen::Quaterniond q(m_poses[i].rotation());
				X.push_back(SE3d(t,q));
			}

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);
				Eigen::Vector3d t(rel(0,3), rel(1,3), rel(2,3));
				Eigen::Quaterniond q(rel.rotation());
				SE3d U = SE3d(t,q);

				SE3Tangentd     d;
				SE3Tangentd     u;
				Eigen::Matrix<double, 6, 6>         J_d_xi, J_d_xj;

				SE3d         Xi,
							 Xj;

				Xi = X[first];
				Xj = X[second];

				d  = Xj.rminus(Xi, J_d_xj, J_d_xi);
				u = U.log();

				int ir = tripletListB.size();
				int ic_1 = first * 6;
				int ic_2 = second * 6;

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

					tripletListA.emplace_back(ir + row, ic_2    , -J_d_xj(row,0));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -J_d_xj(row,1));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -J_d_xj(row,2));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -J_d_xj(row,3));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -J_d_xj(row,4));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -J_d_xj(row,5));
				}

				SE3Tangentd delta = d - u;

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

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta.coeffs()(0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta.coeffs()(1),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta.coeffs()(2),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta.coeffs()(3),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta.coeffs()(4),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta.coeffs()(5),1) * edges_g2o_w[i][5]);
			}

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

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

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

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

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

			std::cout << "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(X.size() * 6 == h_x.size()){

				int counter = 0;
				for(size_t i = 0 ; i < X.size(); i++){
					//int dx_row          = i * 6;
					SE3Tangentd     dx;
					dx.coeffs()(0) = h_x[counter++];
					dx.coeffs()(1) = h_x[counter++];
					dx.coeffs()(2) = h_x[counter++];
					dx.coeffs()(3) = h_x[counter++];
					dx.coeffs()(4) = h_x[counter++];
					dx.coeffs()(5) = h_x[counter++];

					X[i] = X[i] +  dx;
				}

				for (int i = 0 ; i < m_poses.size(); i++){
					m_poses[i](0,0) = X[i].rotation()(0,0);
					m_poses[i](1,0) = X[i].rotation()(1,0);
					m_poses[i](2,0) = X[i].rotation()(2,0);

					m_poses[i](0,1) = X[i].rotation()(0,1);
					m_poses[i](1,1) = X[i].rotation()(1,1);
					m_poses[i](2,1) = X[i].rotation()(2,1);

					m_poses[i](0,2) = X[i].rotation()(0,2);
					m_poses[i](1,2) = X[i].rotation()(1,2);
					m_poses[i](2,2) = X[i].rotation()(2,2);

					m_poses[i](0,3) = X[i].translation()(0);
					m_poses[i](1,3) = X[i].translation()(1);
					m_poses[i](2,3) = X[i].translation()(2);
				}
				std::cout << "h_x.size(): " << h_x.size() << std::endl;
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
			}else{
				std::cout << "h_x.size(): " << h_x.size() << std::endl;
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'w':{
			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 < m_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
			}

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);
				TaitBryanPose pose_rel = pose_tait_bryan_from_affine_matrix(rel);

				TaitBryanPose from = poses[first];
				TaitBryanPose to = poses[second];

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_2_obs_eq_tait_bryan_wc_case1(
					delta,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].om,
					poses[first].fi,
					poses[first].ka,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].om,
					poses[second].fi,
					poses[second].ka,
					pose_rel.px,
					pose_rel.py,
					pose_rel.pz,
					pose_rel.om,
					pose_rel.fi,
					pose_rel.ka);

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_2_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].om,
					poses[first].fi,
					poses[first].ka,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].om,
					poses[second].fi,
					poses[second].ka,
					pose_rel.px,
					pose_rel.py,
					pose_rel.pz,
					pose_rel.om,
					pose_rel.fi,
					pose_rel.ka);
				int ir = tripletListB.size();

				int ic_1 = first * 6;
				int ic_2 = 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));
				}

				float om_diff = delta(3,0);
				if(fabs(om_diff) > M_PI){
					om_diff -= 2.0*M_PI;
				}
				if(fabs(om_diff)< -M_PI){
					om_diff += 2.0*M_PI;
				}

				float fi_diff = delta(4,0);
				if(fabs(fi_diff) > M_PI){
					fi_diff -= 2.0*M_PI;
				}
				if(fabs(fi_diff)< -M_PI){
					fi_diff += 2.0*M_PI;
				}

				float ka_diff = delta(5,0);
				if(fabs(ka_diff) > M_PI){
					ka_diff -= 2.0*M_PI;
				}
				if(fabs(ka_diff)< -M_PI){
					ka_diff += 2.0*M_PI;
				}

				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, om_diff);
				tripletListB.emplace_back(ir + 4, 0, fi_diff);
				tripletListB.emplace_back(ir + 5, 0, ka_diff);

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(om_diff,1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(fi_diff,1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(ka_diff,1) * edges_g2o_w[i][5]);
			}

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

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

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

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

				for(size_t i = 0; i < m_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[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++];
					m_poses[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 'e':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<RodriguesPose> poses;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				TaitBryanPose p = pose_tait_bryan_from_affine_matrix(m_poses[i]);
					p.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
					p.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.00001;
				Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p);
				poses.push_back(pose_rodrigues_from_affine_matrix(m));
			}

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);

				RodriguesPose ps = pose_rodrigues_from_affine_matrix(rel);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_2_obs_eq_rodrigues_wc(
						delta,
						poses[first].px,
						poses[first].py,
						poses[first].pz,
						poses[first].sx,
						poses[first].sy,
						poses[first].sz,
						poses[second].px,
						poses[second].py,
						poses[second].pz,
						poses[second].sx,
						poses[second].sy,
						poses[second].sz,
						ps.px,
						ps.py,
						ps.pz,
						ps.sx,
						ps.sy,
						ps.sz);

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_2_obs_eq_rodrigues_wc_jacobian(jacobian,
						poses[first].px,
						poses[first].py,
						poses[first].pz,
						poses[first].sx,
						poses[first].sy,
						poses[first].sz,
						poses[second].px,
						poses[second].py,
						poses[second].pz,
						poses[second].sx,
						poses[second].sy,
						poses[second].sz,
						ps.px,
						ps.py,
						ps.pz,
						ps.sx,
						ps.sy,
						ps.sz);

				int ir = tripletListB.size();

				int ic_1 = first * 6;
				int ic_2 = 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,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
			}

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

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

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

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

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

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

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

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

			std::vector<double> h_x;

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

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

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

				for(size_t i = 0; i < m_poses.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_poses[i]);
					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++];

					m_poses[i] = affine_matrix_from_pose_rodrigues(pose);

					Eigen::Vector3d vx(m_poses[i](0,0), m_poses[i](1,0), m_poses[i](2,0));
					Eigen::Vector3d vy(m_poses[i](0,1), m_poses[i](1,1), m_poses[i](2,1));
					Eigen::Vector3d vz(m_poses[i](0,2), m_poses[i](1,2), m_poses[i](2,2));

					//std::cout << std::setprecision(15);
					//std::cout << "norm: "<< vx.norm() << " " << vy.norm() << " " << vz.norm() << " " <<
					//		vx.dot(vy) << " " << vy.dot(vz) << " " << vx.dot(vz) << std::endl;
				}
				std::cout << "optimizing with rodrigues finished" << std::endl;
			}else{
				std::cout << "optimizing with rodrigues FAILED" << std::endl;
			}
			break;
		}
		case 'a':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<QuaternionPose> poses;

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

			for(size_t i = 0 ; i < edges_g2o.size(); i++){
				const int first = std::get<0>(edges_g2o[i]);
				const int second = std::get<1>(edges_g2o[i]);
				const Eigen::Affine3d& rel = std::get<2>(edges_g2o[i]);
				QuaternionPose pose_rel = pose_quaternion_from_affine_matrix(rel);

				Eigen::Matrix<double, 7, 1> delta;
				relative_pose_2_obs_eq_quaternion_wc(
					delta,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].q0,
					poses[first].q1,
					poses[first].q2,
					poses[first].q3,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].q0,
					poses[second].q1,
					poses[second].q2,
					poses[second].q3,
					pose_rel.px,
					pose_rel.py,
					pose_rel.pz,
					pose_rel.q0,
					pose_rel.q1,
					pose_rel.q2,
					pose_rel.q3);

				Eigen::Matrix<double, 7, 14, Eigen::RowMajor> jacobian;
				relative_pose_2_obs_eq_quaternion_wc_jacobian(jacobian,
					poses[first].px,
					poses[first].py,
					poses[first].pz,
					poses[first].q0,
					poses[first].q1,
					poses[first].q2,
					poses[first].q3,
					poses[second].px,
					poses[second].py,
					poses[second].pz,
					poses[second].q0,
					poses[second].q1,
					poses[second].q2,
					poses[second].q3,
					pose_rel.px,
					pose_rel.py,
					pose_rel.pz,
					pose_rel.q0,
					pose_rel.q1,
					pose_rel.q2,
					pose_rel.q3);

				int ir = tripletListB.size();

				int ic_1 = first * 7;
				int ic_2 = second * 7;

				for(size_t row = 0 ; row < 7; 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_1 + 6, -jacobian(row,6));

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

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

				tripletListP.emplace_back(ir ,    ir,     cauchy(delta(0,0),1) * edges_g2o_w[i][0]);
				tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1,0),1) * edges_g2o_w[i][1]);
				tripletListP.emplace_back(ir + 2, ir + 2, cauchy(delta(2,0),1) * edges_g2o_w[i][2]);
				tripletListP.emplace_back(ir + 3, ir + 3, cauchy(delta(3,0),1) * edges_g2o_w[i][3]);
				tripletListP.emplace_back(ir + 4, ir + 4, cauchy(delta(4,0),1) * edges_g2o_w[i][4]);
				tripletListP.emplace_back(ir + 5, ir + 5, cauchy(delta(5,0),1) * edges_g2o_w[i][5]);
				tripletListP.emplace_back(ir + 6, ir + 6, cauchy(delta(6,0),1) * edges_g2o_w[i][5]);
			}

			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 < m_poses.size(); i++){
				int ic = i * 7;
				ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(m_poses[i]);

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

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

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

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

			if(h_x.size() == 7 * m_poses.size()){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(m_poses[i]);
					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++];
					m_poses[i] = affine_matrix_from_pose_quaternion(pose);
				}
				std::cout << "optimizing with quaternions finished" << std::endl;
			}else{
				std::cout << "optimizing with quaternions 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 << "n: add noise to poses" << 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 << "x: optimize (Without rotation matrix parametrization)" << std::endl;
	std::cout << "y: optimize (Lie Algebra: manif lib)" << std::endl;
	std::cout << "w: optimize (Tait-Bryan 2)" << std::endl;
	std::cout << "e: optimize (Rodriguez 2)" << std::endl;
	std::cout << "a: optimize (Quaternion 2)" << std::endl;
}


#include <am/succinct/minimal_perfect_hash.hpp>

#include <cstdlib>
#include <sstream>
#include <vector>
#include <set>
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

template<typename Hash, typename Key>
void CheckHashValues(const Hash &hash, const std::vector<Key> &keys)
{
    std::vector<uint32_t> h(keys.size());
    for (size_t i = 0; i < keys.size(); ++i)
    {
        h[i] = hash.GetHash(keys[i]);
        BOOST_CHECK(0 <= h[i]);
        BOOST_CHECK((unsigned)hash.GetRange() > h[i]);
    }
    std::sort(h.begin(), h.end());
    BOOST_CHECK(std::adjacent_find(h.begin(), h.end()) == h.end());
}

template<typename Key, typename Generator>
class PerfectHashTester
{
public:
    static void Test(size_t size)
    {
        std::vector<Key> keys;
        Generator::Generate(size, &keys);
        izenelib::am::succinct::PerfectHash<Key> ph;
        BOOST_CHECK_EQUAL(0, ph.Build(keys));
        CheckHashValues(ph, keys);
    }

    static void TestSerialization(size_t size)
    {
        std::vector<Key> keys;
        Generator::Generate(size, &keys);
        std::string data;
        // Save
        {
            izenelib::am::succinct::PerfectHash<Key> mph;
            BOOST_CHECK_EQUAL(0, mph.Build(keys));
            std::ostringstream oss;
            boost::archive::text_oarchive oa(oss);
            oa << mph;
            data = oss.str();
        }
        // Load and test
        {
            std::istringstream iss(data);
            boost::archive::text_iarchive ia(iss);
            izenelib::am::succinct::PerfectHash<Key> mph;
            ia >> mph;
            CheckHashValues(mph, keys);
        }
    }
};

template<typename Key, typename Generator>
class MinimalPerfectHashTester
{
public:
    static void Test(size_t size)
    {
        std::vector<Key> keys;
        Generator::Generate(size, &keys);
        izenelib::am::succinct::MinimalPerfectHash<Key> mph;
        BOOST_CHECK_EQUAL(0, mph.Build(keys));
        CheckHashValues(mph, keys);
        BOOST_CHECK_EQUAL(keys.size(), mph.GetRange());
    }

    static void TestSerialization(size_t size)
    {
        std::vector<Key> keys;
        Generator::Generate(size, &keys);
        std::string data;
        // Save
        {
            izenelib::am::succinct::MinimalPerfectHash<Key> mph;
            BOOST_CHECK_EQUAL(0, mph.Build(keys));
            std::ostringstream oss;
            boost::archive::text_oarchive oa(oss);
            oa << mph;
            data = oss.str();
        }
        // Load and test
        {
            std::istringstream iss(data);
            boost::archive::text_iarchive ia(iss);
            izenelib::am::succinct::MinimalPerfectHash<Key> mph;
            ia >> mph;
            CheckHashValues(mph, keys);
            BOOST_CHECK_EQUAL(keys.size(), mph.GetRange());
        }
    }
};

template<typename T> class RandomElementGenerator;

template<> class RandomElementGenerator<int>
{
public:
    static int Generate()
    {
        return rand();
    }
};

template<typename T, typename U>
class RandomElementGenerator<std::pair<T, U> >
{
public:
    static std::pair<T, U> Generate()
    {
        return std::make_pair(RandomElementGenerator<T>::Generate(),
                              RandomElementGenerator<U>::Generate());
    }
};

template<typename T> class RandomElementGenerator<std::vector<T> >
{
    static constexpr double kLastingProbability = 0.9;
public:
    static std::vector<T> Generate()
    {
        std::vector<T> res;
        while (rand() < RAND_MAX * kLastingProbability)
        {
            res.push_back(RandomElementGenerator<T>::Generate());
        }
        return res;
    }
};

template<> class RandomElementGenerator<std::string>
{
    static constexpr double kLastingProbability = 0.9;
public:
    static std::string Generate()
    {
        std::vector<int> t = RandomElementGenerator<std::vector<int> >::Generate();
        std::string s(t.size(), ' ');
        for (size_t i = 0; i < t.size(); ++i)
        {
            s[i] = 'a' + t[i] % 26;
        }
        return s;
    }
};

template<typename T> class RandomArrayGenerator
{
public:
    static void Generate(size_t s, std::vector<T> *v)
    {
        std::set<T> se;
        while (se.size() < s)
        {
            se.insert(RandomElementGenerator<T>::Generate());
        }
        v->resize(s);
        std::copy(se.begin(), se.end(), v->begin());
    }
};

template<typename T> class OrderedArrayGenerator;

template<> class OrderedArrayGenerator<int>
{
public:
    static void Generate(size_t s, std::vector<int> *v)
    {
        v->resize(s);
        for (size_t i = 0; i < s; ++i)
        {
            v->at(i) = i;
        }
    }
};

template<> class OrderedArrayGenerator<std::pair<int, int> >
{
public:
    static void Generate(size_t s, std::vector<std::pair<int, int> > *v)
    {
        v->clear();
        for (size_t t = 0; ; ++t)
        {
            for (size_t a = 0; a <= t; ++a)
            {
                if (v->size() >= s) return;
                size_t b = t - a;
                v->push_back(std::make_pair(a, b));
            }
        }
    }
};

template<> class OrderedArrayGenerator<std::vector<int> >
{
public:
    static void Generate(size_t s, std::vector<std::vector<int> > *v)
    {
        int base = 2 + rand() % 4;  // [2, 5]

        for (int t = 1; ; ++t)
        {
            std::vector<int> a(t);
            for (;;)
            {
                if (v->size() >= s)
                {
                    return;
                }
                v->push_back(a);
                int c = 1;
                for (int i = t - 1; i >= 0; --i)
                {
                    a[i] += c;
                    c = a[i] / base;
                    a[i] %= base;
                }
                if (c > 0) break;
            }
        }
    }
};

template<> class OrderedArrayGenerator<std::string>
{
public:
    static void Generate(size_t s, std::vector<std::string> *v)
    {
        std::vector<std::vector<int> > tv;
        OrderedArrayGenerator<std::vector<int> >::Generate(s, &tv);
        v->resize(s);
        for (size_t i = 0; i < s; ++i)
        {
            v->at(i).resize(tv[i].size());
            for (size_t j = 0; j < tv[i].size(); ++j)
            {
                v->at(i)[j] = 'a' + (tv[i][j] % 26);
            }
        }
    }
};

typedef boost::mpl::list<// Test |PerfectHash| with basic types
    PerfectHashTester<int, RandomArrayGenerator<int> >,
    PerfectHashTester<int, OrderedArrayGenerator<int> >,
    PerfectHashTester<std::pair<int, int>, RandomArrayGenerator<std::pair<int, int> > >,
    PerfectHashTester<std::pair<int, int>, OrderedArrayGenerator<std::pair<int, int> > >,
    PerfectHashTester<std::string, RandomArrayGenerator<std::string> >,
    PerfectHashTester<std::string, OrderedArrayGenerator<std::string> >,
    PerfectHashTester<std::vector<int>, RandomArrayGenerator<std::vector<int> > >,
    PerfectHashTester<std::vector<int>, OrderedArrayGenerator<std::vector<int> > >,
    // Test |MinimalPerfectHash| with basic types
    MinimalPerfectHashTester<int, RandomArrayGenerator<int> >,
    MinimalPerfectHashTester<int, OrderedArrayGenerator<int> >,
    MinimalPerfectHashTester<std::pair<int, int>, RandomArrayGenerator<std::pair<int, int> > >,
    MinimalPerfectHashTester<std::pair<int, int>, OrderedArrayGenerator<std::pair<int, int> > >,
    MinimalPerfectHashTester<std::string, RandomArrayGenerator<std::string> >,
    MinimalPerfectHashTester<std::string, OrderedArrayGenerator<std::string> >,
    MinimalPerfectHashTester<std::vector<int>, RandomArrayGenerator<std::vector<int> > >,
    MinimalPerfectHashTester<std::vector<int>, OrderedArrayGenerator<std::vector<int> > >,
    // Test with some complex types (heavy...)
    MinimalPerfectHashTester<std::vector<std::string>, RandomArrayGenerator<std::vector<std::string> > >,
    MinimalPerfectHashTester<std::vector<std::pair<int, std::string> >,
    RandomArrayGenerator<std::vector<std::pair<int, std::string> > > >
> HashTesters;

BOOST_AUTO_TEST_SUITE( t_min_perfect_hash_suite )

BOOST_AUTO_TEST_CASE(sample)
{
    // int
    {
        // Prepare the keys
        std::vector<int> t;
        t.push_back(1);
        t.push_back(11);
        t.push_back(111);
        t.push_back(1111);
        t.push_back(11111);
        t.push_back(111111);

        // Build
        izenelib::am::succinct::MinimalPerfectHash<int> mph;
        BOOST_CHECK(0 == mph.Build(t));

        // GetHash
        for (size_t i = 0; i < t.size(); ++i) {
            std::cout << t[i] << "\t" << mph.GetHash(t[i]) << std::endl;
        }
        std::cout << std::endl;
    }

    // string
    {
        // Prepare the keys
        std::vector<std::string> t;
        t.push_back("hoge");
        t.push_back("piyo");
        t.push_back("fuga");
        t.push_back("foo");
        t.push_back("bar");

        // Build
        izenelib::am::succinct::MinimalPerfectHash<std::string> mph;
        BOOST_CHECK(0 == mph.Build(t));

        // GetHash
        for (size_t i = 0; i < t.size(); ++i) {
            std::cout << t[i] << "\t" << mph.GetHash(t[i]) << std::endl;
        }
        std::cout << std::endl;
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(small, T, HashTesters)
{
    const int kNumTrial = 10;
    for (size_t s = 0; s <= 100; ++s)
    {
        for (int t = 0; t < kNumTrial; ++t)
        {
            T::Test(s);
        }
    }
}
/*
BOOST_AUTO_TEST_CASE_TEMPLATE(large, T, HashTesters)
{
    const int kNumTrial = 10;
    const int kMaxSize = 100000;
    for (int t = 0; t < kNumTrial; ++t)
    {
        size_t s = rand() % kMaxSize;
        T::Test(s);
    }
}
*/
BOOST_AUTO_TEST_CASE_TEMPLATE(serialization, T, HashTesters)
{
    const int kNumTrial = 10;
    const int kMaxSize = 100;
    for (int t = 0; t < kNumTrial; ++t)
    {
        size_t s = rand() % kMaxSize;
        T::TestSerialization(s);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file delta_visitor.hpp
 * @author Marcus Edel
 *
 * This file provides an abstraction for the Delta() function for different
 * layers and automatically directs any parameter to the right layer type.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_VISITOR_DELTA_VISITOR_HPP
#define MLPACK_METHODS_ANN_VISITOR_DELTA_VISITOR_HPP

#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>

#include <boost/variant.hpp>

namespace mlpack {
namespace ann {

/**
 * DeltaVisitor exposes the delta parameter of the given module.
 */
class DeltaVisitor : public boost::static_visitor<arma::mat&>
{
 public:
  //! Return the delta parameter.
  template<typename LayerType>
  arma::mat& operator()(LayerType* layer) const;

  arma::mat& operator()(MoreTypes layer) const;
};

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "delta_visitor_impl.hpp"

#endif


/*
 * stat_cmoea.hpp
 *
 *  Created on: Feb 26, 2015
 *      Author: Joost Huizinga
 */

#ifndef EXP_CMOEA_EXAMPLE_STAT_CMOEA_HPP_
#define EXP_CMOEA_EXAMPLE_STAT_CMOEA_HPP_

#include <numeric>
#include <iomanip>
#include <sstream>
#include <string>
#include <iostream>
#include <sferes/stat/stat.hpp>

#include <boost/graph/iteration_macros.hpp>
#include <boost/spirit/include/karma_real.hpp>

#include <sferes/dbg/dbg.hpp>

#include <modules/datatools/common_compare.hpp>
#include <modules/cmoea/cmoea_util.hpp>

namespace sferes
{
namespace stat
{

SFERES_STAT(StatCmoea, Stat)
{

public:
    StatCmoea(){
        _first_line = true;
    }

    static const size_t period = Params::stats::period;

    typedef boost::shared_ptr<Phen> indiv_t;
    typedef std::vector<indiv_t> pop_t;

    template<typename E>
    void refresh(const E& ea)
    {
        dbg::trace trace("stat", DBG_HERE);
        typedef typename E::pop_t mixed_pop_t;

        size_t categories = Params::cmoea::nb_of_bins;
        size_t objectives = cmoea::getNrOfObjectives(categories);
        size_t bin_size = Params::cmoea::bin_size;

        dbg::out(dbg::info, "stat") << "Gen: " << ea.gen() << " period: " << period << std::endl;
        if (ea.gen() % period != 0) return;

        //Write header
        dbg::out(dbg::info, "stat") << "Creating log file" << std::endl;
        this->_create_log_file(ea, "innov_all_bin.dat");
        dbg::out(dbg::info, "stat") << "Log file: " << this->_log_file << std::endl;
        if(!this->_log_file) return;

        if(this->firstLine()){
            dbg::out(dbg::info, "stat") << "Writing header" << std::endl;
            (*this->_log_file) << "#gen ";
            for(size_t i=0; i<categories; ++i){
                (*this->_log_file) << "obj_" << cmoea::getTaskIndicesStr(i, objectives) << " ";
            }

            (*this->_log_file) << "\n";
            _first_line = false;
        }

        std::vector<size_t> best_fit_index(categories, 0);
        std::vector<float> best_fit(categories, 0);
        dbg::assertion(DBG_ASSERTION(categories == ea.archive().size()));

        //Add them in this exact order so it is easy to put them back into the archive
        _all.clear();
        dbg::out(dbg::info, "stat") << "Archive size: " << ea.archive().size() << std::endl;
        for(size_t i=0; i<ea.archive().size(); ++i){
            dbg::check_bounds(dbg::error, 0, i, ea.archive().size(), DBG_HERE);
            dbg::out(dbg::info, "stat") << "Bin: " << i << " size: " << ea.archive()[i].size() << std::endl;

            best_fit[i]=ea.archive()[i][0]->fit().getBinFitness(i);
            for(size_t j=0; j<ea.archive()[i].size(); ++j){
                dbg::check_bounds(dbg::error, 0, j, ea.archive()[i].size(), DBG_HERE);
                float fitness = ea.archive()[i][j]->fit().getBinFitness(i);
                if(fitness > best_fit[i]){
                    dbg::check_bounds(dbg::error, 0, i, best_fit.size(), DBG_HERE);
                    dbg::check_bounds(dbg::error, 0, i, best_fit_index.size(), DBG_HERE);
                    best_fit[i] = fitness;
                    best_fit_index[i] = j;
                }
                _all.push_back(ea.archive()[i][j]);
            }
        }

        dbg::assertion(DBG_ASSERTION(_all.size() == bin_size*categories));
        dbg::check_bounds(dbg::error, 0, 0, ea.archive().size(), DBG_HERE);
        dbg::check_bounds(dbg::error, 0, 0, best_fit_index.size(), DBG_HERE);
        dbg::check_bounds(dbg::error, 0, best_fit_index[0], ea.archive()[0].size(), DBG_HERE);
        _best_individual = ea.archive()[0][best_fit_index[0]];

        //Start writing
        dbg::out(dbg::info, "stat") << "Writing line" << std::endl;
        (*this->_log_file) << ea.gen() << " ";                // generation
        for(size_t i=0; i<best_fit.size(); ++i){
            (*this->_log_file) << best_fit[i] << " ";
        }
        (*this->_log_file) << std::endl;

        std::cout << "Genenration: " << ea.gen() << " combined performance: " << best_fit [0] << std::endl;
    }

    bool firstLine(){
      return _first_line;
    }

    pop_t& getPopulation(){
        dbg::trace trace("stat", DBG_HERE);
        return _all;
    }

    std::string name(){
        return "StatCmoea";
    }

protected:
    indiv_t _best_individual;
    pop_t _all;
    bool _first_line;
};
}
}

#endif /* EXP_CMOEA_EXAMPLE_STAT_CMOEA_HPP_ */


//
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#define BOOST_DISABLE_ASSERTS 1 // kernel_1d_adaptor assertions are too strict
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>

#include <boost/core/lightweight_test.hpp>

#include <vector>

namespace gil = boost::gil;

void test_kernel_1d_fixed_default_constructor() {
  gil::kernel_1d_fixed<int, 9> k;
  BOOST_TEST_EQ(k.center(), 0);
  BOOST_TEST_EQ(k.left_size(), 0);
  BOOST_TEST_EQ(k.right_size(), 8); // TODO: Why not 0 or -1 if not set?
  // std::array interface
  BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_2d_fixed_default_constructor() {
  gil::detail::kernel_2d_fixed<int, 9> k;
  BOOST_TEST_EQ(k.center_x(), 0);
  BOOST_TEST_EQ(k.center_y(), 0);
  BOOST_TEST_EQ(k.left_size(), 0);
  BOOST_TEST_EQ(k.right_size(), 8); // TODO: Why not 0 or -1 if not set?
  BOOST_TEST_EQ(k.upper_size(), 0);
  BOOST_TEST_EQ(k.lower_size(), 8);
  // std::array interface
  BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_1d_fixed_parameterized_constructor() {
  gil::kernel_1d_fixed<int, 9> k(4);
  BOOST_TEST_EQ(k.center(), 4);
  BOOST_TEST_EQ(k.left_size(), 4);
  BOOST_TEST_EQ(k.right_size(), 4);
  // std::vector interface
  BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_2d_fixed_parameterized_constructor() {
  gil::detail::kernel_2d_fixed<int, 9> k(4, 4);
  BOOST_TEST_EQ(k.center_x(), 4);
  BOOST_TEST_EQ(k.center_y(), 4);
  BOOST_TEST_EQ(k.left_size(), 4);
  BOOST_TEST_EQ(k.right_size(), 4);
  BOOST_TEST_EQ(k.upper_size(), 4);
  BOOST_TEST_EQ(k.lower_size(), 4);
  // std::vector interface
  BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_1d_fixed_parameterized_constructor_with_iterator() {
  // FIXME: The constructor should throw if v.size() < k.size()
  std::vector<int> v(9);
  gil::kernel_1d_fixed<int, 9> k(v.cbegin(), 4);
  BOOST_TEST_EQ((gil::kernel_1d_fixed<int, 9>::static_size), 9);
  BOOST_TEST_EQ(k.center(), 4);
  BOOST_TEST_EQ(k.left_size(), 4);
  BOOST_TEST_EQ(k.right_size(), 4);
  // std::vector interface
  BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_2d_fixed_parameterized_constructor_with_iterator() {
  //    // FIXME: The constructor should throw if v.size() < k.size()
  std::array<int, 81> v;
  gil::detail::kernel_2d_fixed<int, 9> k(v.cbegin(), 4, 4);
  BOOST_TEST_EQ((gil::detail::kernel_2d_fixed<int, 9>::static_size), 9);
  BOOST_TEST_EQ(k.center_y(), 4);
  BOOST_TEST_EQ(k.center_x(), 4);
  BOOST_TEST_EQ(k.left_size(), 4);
  BOOST_TEST_EQ(k.right_size(), 4);
  BOOST_TEST_EQ(k.upper_size(), 4);
  BOOST_TEST_EQ(k.lower_size(), 4);
  // std::vector interface
  BOOST_TEST_EQ(k.size(), 9);
}

void test_kernel_1d_fixed_copy_constructor() {
  gil::kernel_1d_fixed<int, 9> d(4);
  gil::kernel_1d_fixed<int, 9> k(d);
  BOOST_TEST_EQ((gil::kernel_1d_fixed<int, 9>::static_size), 9);
  BOOST_TEST_EQ(k.center(), 4);
  BOOST_TEST_EQ(k.center(), d.center());
  BOOST_TEST_EQ(k.left_size(), d.left_size());
  BOOST_TEST_EQ(k.right_size(), d.right_size());
  // std::vector interface
  BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_2d_fixed_copy_constructor() {
  gil::detail::kernel_2d_fixed<int, 9> d(4, 4);
  gil::detail::kernel_2d_fixed<int, 9> k(d);
  BOOST_TEST_EQ((gil::detail::kernel_2d_fixed<int, 9>::static_size), 9);
  BOOST_TEST_EQ(k.center_x(), 4);
  BOOST_TEST_EQ(k.center_y(), 4);
  BOOST_TEST_EQ(k.center_x(), d.center_x());
  BOOST_TEST_EQ(k.center_y(), d.center_y());
  BOOST_TEST_EQ(k.left_size(), d.left_size());
  BOOST_TEST_EQ(k.right_size(), d.right_size());
  BOOST_TEST_EQ(k.lower_size(), d.lower_size());
  BOOST_TEST_EQ(k.upper_size(), d.upper_size());
  // std::vector interface
  BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_1d_fixed_assignment_operator() {
  gil::kernel_1d_fixed<int, 9> d(4);
  gil::kernel_1d_fixed<int, 9> k;
  k = d;
  BOOST_TEST_EQ((gil::kernel_1d_fixed<int, 9>::static_size), 9);
  BOOST_TEST_EQ(k.center(), 4);
  BOOST_TEST_EQ(k.center(), d.center());
  BOOST_TEST_EQ(k.left_size(), d.left_size());
  BOOST_TEST_EQ(k.right_size(), d.right_size());
  // std::vector interface
  BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_2d_fixed_assignment_operator() {
  gil::detail::kernel_2d_fixed<int, 9> d(4, 4);
  gil::detail::kernel_2d_fixed<int, 9> k;
  k = d;
  BOOST_TEST_EQ((gil::detail::kernel_2d_fixed<int, 9>::static_size), 9);
  BOOST_TEST_EQ(k.center_x(), 4);
  BOOST_TEST_EQ(k.center_y(), 4);
  BOOST_TEST_EQ(k.center_x(), d.center_x());
  BOOST_TEST_EQ(k.center_y(), d.center_y());
  BOOST_TEST_EQ(k.left_size(), d.left_size());
  BOOST_TEST_EQ(k.right_size(), d.right_size());
  BOOST_TEST_EQ(k.lower_size(), d.lower_size());
  BOOST_TEST_EQ(k.upper_size(), d.upper_size());
  // std::vector interface
  BOOST_TEST_EQ(k.size(), d.size());
}

void test_kernel_1d_fixed_reverse_kernel() {
  std::array<int, 3> values = {{1, 2, 3}};
  gil::kernel_1d_fixed<int, 3> k(values.begin(), 1);
  BOOST_TEST_EQ((gil::kernel_1d_fixed<int, 3>::static_size), 3);
  BOOST_TEST_ALL_EQ(k.begin(), k.end(), values.begin(), values.end());

  std::array<int, 3> values_rev = {{3, 2, 1}};
  auto const k_rev = gil::reverse_kernel(k);
  BOOST_TEST_ALL_EQ(k_rev.begin(), k_rev.end(), values_rev.begin(),
                    values_rev.end());
}

int main() {
  test_kernel_1d_fixed_default_constructor();
  test_kernel_2d_fixed_default_constructor();
  test_kernel_1d_fixed_parameterized_constructor();
  test_kernel_2d_fixed_parameterized_constructor();
  test_kernel_1d_fixed_parameterized_constructor_with_iterator();
  test_kernel_2d_fixed_parameterized_constructor_with_iterator();
  test_kernel_1d_fixed_copy_constructor();
  test_kernel_2d_fixed_copy_constructor();
  test_kernel_1d_fixed_assignment_operator();
  test_kernel_2d_fixed_assignment_operator();
  test_kernel_1d_fixed_reverse_kernel();

  return ::boost::report_errors();
}


#include <algorithm>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <map>
#include <numeric>
#include <sstream>
#include <stdint.h>
#include <string>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind.hpp>

using namespace std;

struct int_div {
  float operator()(int x, int y) const { return ((float)x)/y; };
};

void do_sum_avg(int values[], int n, int& sum, float& avg)
{
  sum = 0;
  for (int i = 0; i < n; i++)
    sum += values[i];
  avg = (float)sum / n;
}

struct X {
  int foo(int i){ return i*i; }
};

int main(int argc, char **argv)
{
  boost::function<float (int x, int y)> f;
 // boost::function2<float, int, int> f2;
  f = int_div();
  std::cout << f(5, 3) << endl;

  boost::function<void (int values[], int n, int& sum, float& avg)> sum_avg;
//  boost::function4<void, int*, int, int&, float&> sum_avg;

  sum_avg = &do_sum_avg;
  int values[] = {2, 3, 5, 7, 11, 13};
  int sum;
  float avg;
  sum_avg(values, 6, sum, avg);
  std::cout << "sum = " << sum << ", avg = " << avg << endl;

  boost::function<int (X*, int)> g;
  // boost::function2<int, X*, int> g;
  g = &X::foo;
  X x;
  cout << "g(7) = " << g(&x, 7) << endl;

  boost::function<int (int)> h(boost::bind(&X::foo, &x, _1));
  cout << "h(9) = " << h(9) << endl;
  return 0;
}