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

#include "structures.h"
#include "transformations.h"
#include "point_to_point_source_to_target_tait_bryan_wc_jacobian.h"
#include "point_to_point_source_to_target_rodrigues_wc_jacobian.h"
#include "point_to_point_source_to_target_quaternion_wc_jacobian.h"
#include "quaternion_constraint_jacobian.h"
#include "cauchy.h"
#include "point_to_point_source_to_landmark_tait_bryan_wc_jacobian.h"
#include "point_to_point_source_to_landmark_rodrigues_wc_jacobian.h"
#include "point_to_point_source_to_landmark_quaternion_wc_jacobian.h"
#include "point_to_point_source_to_landmark_tait_bryan_wc_cov.h"

struct Measurement{
	Eigen::Vector3d value;
	int index_landmark;
};

struct Node{
	Eigen::Affine3d pose;
	std::vector<Measurement> measurements;
};

struct PointMeanCov{
	Eigen::Vector3d mean;
	Eigen::Matrix3d cov;
	Eigen::Vector3d coords;
};

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 = -100.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();
void calculate_ICP_COV(std::vector<PointMeanCov>& data_pi,
		std::vector<PointMeanCov>& model_qi, Eigen::Affine3d transform, Eigen::MatrixXd& ICP_COV);

std::vector<Eigen::Vector3d> landmarks;
std::vector<Node> nodes;
std::vector<Node> nodesInitial;

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

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

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

    for (double i = 0; i < 1.0; i+=di) { //horizonal
		double u = i*2*pi;      //0     to  2pi
		const Eigen::Vector3d pp0( cos(u), sin (u),0);
		const Eigen::Vector3d pp1( cos(u+du), sin(u+du),0);
		Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
		Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
		glBegin(GL_LINE_LOOP);
		glVertex3dv(tp0.data());
		glVertex3dv(tp1.data());
		glEnd();
	}
}

void compute_covariance (std::vector<Eigen::Vector3d> points, Eigen::Vector3d &mean, Eigen::Matrix3d &cov)
{
	mean.x() = 0;
	mean.y() = 0;
	mean.z() = 0;

	for(size_t i = 0 ; i < points.size(); i++){
		mean += points[i];
	}
	mean /= points.size();

    Eigen::Matrix3d covariance;
    for (int x = 0; x < 3; x ++)
    {
        for (int y = 0; y < 3; y ++)
        {
            double element =0;
            for (const auto pp : points)
            {
                element += (pp(x) - mean(x)) * (pp(y) - mean(y));

            }
            covariance(x,y) = element / (points.size());
        }
    };
    cov = covariance;
}

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

	for(size_t i = 0; i < 10; i++){
		landmarks.emplace_back(Eigen::Vector3d((rand()%1000 - 500)*0.1, (rand()%1000 - 500)*0.1, (rand()%1000 - 500)*0.0001));
	}

	for(size_t i = 0; i < 10; i++){
		Eigen::Affine3d m = Eigen::Affine3d::Identity();
		m(0,3) = (rand()%1000 - 500)*0.001;
		m(1,3) = i*5;
		m(2,3) = 1;
		Node n;
		n.pose = m;

		TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m);
		pose.ka = M_PI /4.0;

		n.pose = affine_matrix_from_pose_tait_bryan(pose);

		nodes.emplace_back(n);
	}

	nodesInitial = nodes;

	for(size_t i = 0 ; i < nodes.size(); i++){
		for(size_t j = 0; j < landmarks.size(); j++){
			Measurement meas;
			meas.index_landmark = j;
			meas.value = nodes[i].pose.inverse() * landmarks[j];
			nodes[i].measurements.emplace_back(meas);
		}
	}

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

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

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("point to point source to landmark");
	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);

	glLineWidth(2);
	/*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();*/

	glColor3f(0, 0, 0);
	glBegin(GL_LINES);
	for(const auto &l:landmarks){
		glVertex3f(l.x() - 1, l.y(), l.z());
		glVertex3f(l.x() + 1, l.y(), l.z());

		glVertex3f(l.x(), l.y() - 1, l.z());
		glVertex3f(l.x(), l.y() + 1, l.z());
	}
	glEnd();

	glColor3f(1, 0, 0);
	glBegin(GL_LINE_STRIP);
		for(int i = 0 ; i < nodes.size(); i++){
			glVertex3f(nodes[i].pose(0,3), nodes[i].pose(1,3), nodes[i].pose(2,3));
		}
	glEnd();

	glColor3f(0,0,0);
	glBegin(GL_LINES);
		for(int i = 0 ; i < nodesInitial.size(); i++){
			glVertex3f(nodesInitial[i].pose(0,3)-1, nodesInitial[i].pose(1,3), nodesInitial[i].pose(2,3));
			glVertex3f(nodesInitial[i].pose(0,3)+1, nodesInitial[i].pose(1,3), nodesInitial[i].pose(2,3));

			glVertex3f(nodesInitial[i].pose(0,3), nodesInitial[i].pose(1,3) - 1, nodesInitial[i].pose(2,3));
			glVertex3f(nodesInitial[i].pose(0,3), nodesInitial[i].pose(1,3) + 1, nodesInitial[i].pose(2,3));
		}
	glEnd();

	glColor3f(0.6,0.6,0.6);
	glBegin(GL_LINES);
		for(int i = 0 ; i < nodesInitial.size(); i++){
			glVertex3f(nodesInitial[i].pose(0,3), nodesInitial[i].pose(1,3), nodesInitial[i].pose(2,3));
			glVertex3f(nodes[i].pose(0,3), nodes[i].pose(1,3), nodes[i].pose(2,3));
		}
	glEnd();

	glColor3f(0, 0, 0);
	glBegin(GL_LINES);

	for(size_t i = 0 ; i < nodes.size(); i++){
		for(size_t j = 0 ; j < nodes[i].measurements.size(); j++){
			Eigen::Vector3d meas = nodes[i].pose * nodes[i].measurements[j].value;

			glVertex3f(meas.x() - 0.3, meas.y(), meas.z());
			glVertex3f(meas.x() + 0.3, meas.y(), meas.z());

			glVertex3f(meas.x(), meas.y() - 0.3, meas.z());
			glVertex3f(meas.x(), meas.y() + 0.3, meas.z());
		}
	}
	glEnd();

	std::vector<PointMeanCov> lmc;
	for(int i = 0; i < landmarks.size(); i++){
		std::vector<Eigen::Vector3d> points;
		for(size_t ii = 0 ; ii < nodes.size(); ii++){
			for(size_t j = 0 ; j < nodes[ii].measurements.size(); j++){
				if(nodes[i].measurements[j].index_landmark == i){
					Eigen::Vector3d meas = nodes[ii].pose * nodes[ii].measurements[j].value;
					points.push_back(meas);
				}
			}
		}
		Eigen::Vector3d mean;
		Eigen::Matrix3d cov;
		compute_covariance (points, mean, cov);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(1.0, 0.0, 0.0),1);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0.0, 1.0, 0.0),2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0.0, 0.0, 1.0),3);
		PointMeanCov l;
		l.coords = landmarks[i];
		l.mean = mean;
		l.cov = cov;
		lmc.push_back(l);
	}

	for(size_t i = 0 ; i < nodes.size(); i++){
		std::vector<PointMeanCov> data_pi;
		std::vector<PointMeanCov> model_qi;

		for(size_t j = 0 ; j < nodes[i].measurements.size(); j++){
			PointMeanCov pi;
			pi.coords = nodes[i].measurements[j].value;
			pi.cov = lmc[nodes[i].measurements[j].index_landmark].cov;
			pi.cov(0,0)= 0.03 * 0.03;
			pi.cov(1,1)= 0.03 * 0.03;
			pi.cov(2,2)= 0.03 * 0.03;
			pi.mean = lmc[nodes[i].measurements[j].index_landmark].mean;
			data_pi.push_back(pi);

			PointMeanCov qi = lmc[nodes[i].measurements[j].index_landmark];
			model_qi.push_back(qi);
		}

		Eigen::MatrixXd ICP_COV(6,6);
		ICP_COV = Eigen::MatrixXd::Zero(6,6);
		calculate_ICP_COV(data_pi, model_qi, nodes[i].pose, ICP_COV);

		Eigen::Vector3d mean(nodes[i].pose(0,3), nodes[i].pose(1,3), nodes[i].pose(2,3));
		Eigen::Matrix3d cov;
		cov(0,0) = ICP_COV(0,0);
		cov(0,1) = ICP_COV(0,1);
		cov(0,2) = ICP_COV(0,2);
		cov(1,0) = ICP_COV(1,0);
		cov(1,1) = ICP_COV(1,1);
		cov(1,2) = ICP_COV(1,2);
		cov(2,0) = ICP_COV(2,0);
		cov(2,1) = ICP_COV(2,1);
		cov(2,2) = ICP_COV(2,2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(1,0,0),1);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0,1,0),2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0,0,1),3);
	}
	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 < nodes.size(); i++){
				TaitBryanPose pose;
						pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.5;
						pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.5;
						pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0000005;
						pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0000005;
						pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0000005;
						pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.005;
				nodes[i].pose = nodes[i].pose * 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;

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

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

					Eigen::Vector3d &p_t = landmarks[nodes[i].measurements[j].index_landmark];
					Eigen::Vector3d &p_s = nodes[i].measurements[j].value;

					double delta_x;
					double delta_y;
					double delta_z;
					point_to_point_source_to_landmark_tait_bryan_wc(delta_x, delta_y, delta_z,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

					Eigen::Matrix<double, 3, 9, Eigen::RowMajor> jacobian;
					point_to_point_source_to_landmark_tait_bryan_wc_jacobian(jacobian,
							pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
							p_s.x(), p_s.y(), p_s.z());

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

					if(jacobian(0,0) != 0.0)tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
					if(jacobian(0,1) != 0.0)tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
					if(jacobian(0,2) != 0.0)tripletListA.emplace_back(ir + 0, ic + 2, -jacobian(0,2));
					if(jacobian(0,3) != 0.0)tripletListA.emplace_back(ir + 0, ic + 3, -jacobian(0,3));
					if(jacobian(0,4) != 0.0)tripletListA.emplace_back(ir + 0, ic + 4, -jacobian(0,4));
					if(jacobian(0,5) != 0.0)tripletListA.emplace_back(ir + 0, ic + 5, -jacobian(0,5));

					ic = nodes.size() * 6 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(0,6) != 0.0)tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,6));
					if(jacobian(0,7) != 0.0)tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,7));
					if(jacobian(0,8) != 0.0)tripletListA.emplace_back(ir + 0, ic + 2, -jacobian(0,8));

					ic = 6 * i;
					if(jacobian(1,0) != 0.0)tripletListA.emplace_back(ir + 1, ic + 0, -jacobian(1,0));
					if(jacobian(1,1) != 0.0)tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(1,1));
					if(jacobian(1,2) != 0.0)tripletListA.emplace_back(ir + 1, ic + 2, -jacobian(1,2));
					if(jacobian(1,3) != 0.0)tripletListA.emplace_back(ir + 1, ic + 3, -jacobian(1,3));
					if(jacobian(1,4) != 0.0)tripletListA.emplace_back(ir + 1, ic + 4, -jacobian(1,4));
					if(jacobian(1,5) != 0.0)tripletListA.emplace_back(ir + 1, ic + 5, -jacobian(1,5));

					ic = nodes.size() * 6 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(1,6) != 0.0)tripletListA.emplace_back(ir + 1, ic + 0, -jacobian(1,6));
					if(jacobian(1,7) != 0.0)tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(1,7));
					if(jacobian(1,8) != 0.0)tripletListA.emplace_back(ir + 1, ic + 2, -jacobian(1,8));

					ic = 6 * i;
					if(jacobian(2,0) != 0.0)tripletListA.emplace_back(ir + 2, ic + 0, -jacobian(2,0));
					if(jacobian(2,1) != 0.0)tripletListA.emplace_back(ir + 2, ic + 1, -jacobian(2,1));
					if(jacobian(2,2) != 0.0)tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(2,2));
					if(jacobian(2,3) != 0.0)tripletListA.emplace_back(ir + 2, ic + 3, -jacobian(2,3));
					if(jacobian(2,4) != 0.0)tripletListA.emplace_back(ir + 2, ic + 4, -jacobian(2,4));
					if(jacobian(2,5) != 0.0)tripletListA.emplace_back(ir + 2, ic + 5, -jacobian(2,5));

					ic = nodes.size() * 6 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(2,6) != 0.0)tripletListA.emplace_back(ir + 2, ic + 0, -jacobian(2,6));
					if(jacobian(2,7) != 0.0)tripletListA.emplace_back(ir + 2, ic + 1, -jacobian(2,7));
					if(jacobian(2,8) != 0.0)tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(2,8));

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

			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,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);

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

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

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

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

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

			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
			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){
					std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl;
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == nodes.size() * 6 + landmarks.size() * 3){
				std::cout << "optimization success" << std::endl;
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}

				int counter = 0;

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

				for(size_t i = 0 ; i < landmarks.size(); i++){
					landmarks[i].x() += h_x[counter++];
					landmarks[i].y() += h_x[counter++];
					landmarks[i].z() += h_x[counter++];
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

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

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

					Eigen::Vector3d &p_t = landmarks[nodes[i].measurements[j].index_landmark];
					Eigen::Vector3d &p_s = nodes[i].measurements[j].value;

					double delta_x;
					double delta_y;
					double delta_z;
					point_to_point_source_to_landmark_rodrigues_wc(delta_x, delta_y, delta_z,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

					Eigen::Matrix<double, 3, 9, Eigen::RowMajor> jacobian;
					point_to_point_source_to_landmark_rodrigues_wc_jacobian(jacobian,
							pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
							p_s.x(), p_s.y(), p_s.z());

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

					if(jacobian(0,0) != 0.0)tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
					if(jacobian(0,1) != 0.0)tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
					if(jacobian(0,2) != 0.0)tripletListA.emplace_back(ir + 0, ic + 2, -jacobian(0,2));
					if(jacobian(0,3) != 0.0)tripletListA.emplace_back(ir + 0, ic + 3, -jacobian(0,3));
					if(jacobian(0,4) != 0.0)tripletListA.emplace_back(ir + 0, ic + 4, -jacobian(0,4));
					if(jacobian(0,5) != 0.0)tripletListA.emplace_back(ir + 0, ic + 5, -jacobian(0,5));

					ic = nodes.size() * 6 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(0,6) != 0.0)tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,6));
					if(jacobian(0,7) != 0.0)tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,7));
					if(jacobian(0,8) != 0.0)tripletListA.emplace_back(ir + 0, ic + 2, -jacobian(0,8));

					ic = 6 * i;
					if(jacobian(1,0) != 0.0)tripletListA.emplace_back(ir + 1, ic + 0, -jacobian(1,0));
					if(jacobian(1,1) != 0.0)tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(1,1));
					if(jacobian(1,2) != 0.0)tripletListA.emplace_back(ir + 1, ic + 2, -jacobian(1,2));
					if(jacobian(1,3) != 0.0)tripletListA.emplace_back(ir + 1, ic + 3, -jacobian(1,3));
					if(jacobian(1,4) != 0.0)tripletListA.emplace_back(ir + 1, ic + 4, -jacobian(1,4));
					if(jacobian(1,5) != 0.0)tripletListA.emplace_back(ir + 1, ic + 5, -jacobian(1,5));

					ic = nodes.size() * 6 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(1,6) != 0.0)tripletListA.emplace_back(ir + 1, ic + 0, -jacobian(1,6));
					if(jacobian(1,7) != 0.0)tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(1,7));
					if(jacobian(1,8) != 0.0)tripletListA.emplace_back(ir + 1, ic + 2, -jacobian(1,8));

					ic = 6 * i;
					if(jacobian(2,0) != 0.0)tripletListA.emplace_back(ir + 2, ic + 0, -jacobian(2,0));
					if(jacobian(2,1) != 0.0)tripletListA.emplace_back(ir + 2, ic + 1, -jacobian(2,1));
					if(jacobian(2,2) != 0.0)tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(2,2));
					if(jacobian(2,3) != 0.0)tripletListA.emplace_back(ir + 2, ic + 3, -jacobian(2,3));
					if(jacobian(2,4) != 0.0)tripletListA.emplace_back(ir + 2, ic + 4, -jacobian(2,4));
					if(jacobian(2,5) != 0.0)tripletListA.emplace_back(ir + 2, ic + 5, -jacobian(2,5));

					ic = nodes.size() * 6 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(2,6) != 0.0)tripletListA.emplace_back(ir + 2, ic + 0, -jacobian(2,6));
					if(jacobian(2,7) != 0.0)tripletListA.emplace_back(ir + 2, ic + 1, -jacobian(2,7));
					if(jacobian(2,8) != 0.0)tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(2,8));

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

			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,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);

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

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

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

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

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

			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
			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){
					std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl;
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == nodes.size() * 6 + landmarks.size() * 3){
				std::cout << "optimization success" << std::endl;
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}

				int counter = 0;

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

				for(size_t i = 0 ; i < landmarks.size(); i++){
					landmarks[i].x() += h_x[counter++];
					landmarks[i].y() += h_x[counter++];
					landmarks[i].z() += h_x[counter++];
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

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

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

					Eigen::Vector3d &p_t = landmarks[nodes[i].measurements[j].index_landmark];
					Eigen::Vector3d &p_s = nodes[i].measurements[j].value;

					double delta_x;
					double delta_y;
					double delta_z;
					point_to_point_source_to_landmark_quaternion_wc(delta_x, delta_y, delta_z,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

					Eigen::Matrix<double, 3, 10, Eigen::RowMajor> jacobian;
					point_to_point_source_to_landmark_quaternion_wc_jacobian(jacobian,
							pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
							p_s.x(), p_s.y(), p_s.z());

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

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

					ic = nodes.size() * 7 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(0,7) != 0.0)tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,7));
					if(jacobian(0,8) != 0.0)tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,8));
					if(jacobian(0,9) != 0.0)tripletListA.emplace_back(ir + 0, ic + 2, -jacobian(0,9));

					ic = 7 * i;
					if(jacobian(1,0) != 0.0)tripletListA.emplace_back(ir + 1, ic + 0, -jacobian(1,0));
					if(jacobian(1,1) != 0.0)tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(1,1));
					if(jacobian(1,2) != 0.0)tripletListA.emplace_back(ir + 1, ic + 2, -jacobian(1,2));
					if(jacobian(1,3) != 0.0)tripletListA.emplace_back(ir + 1, ic + 3, -jacobian(1,3));
					if(jacobian(1,4) != 0.0)tripletListA.emplace_back(ir + 1, ic + 4, -jacobian(1,4));
					if(jacobian(1,5) != 0.0)tripletListA.emplace_back(ir + 1, ic + 5, -jacobian(1,5));
					if(jacobian(1,6) != 0.0)tripletListA.emplace_back(ir + 1, ic + 6, -jacobian(1,6));

					ic = nodes.size() * 7 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(1,7) != 0.0)tripletListA.emplace_back(ir + 1, ic + 0, -jacobian(1,7));
					if(jacobian(1,8) != 0.0)tripletListA.emplace_back(ir + 1, ic + 1, -jacobian(1,8));
					if(jacobian(1,9) != 0.0)tripletListA.emplace_back(ir + 1, ic + 2, -jacobian(1,9));

					ic = 7 * i;
					if(jacobian(2,0) != 0.0)tripletListA.emplace_back(ir + 2, ic + 0, -jacobian(2,0));
					if(jacobian(2,1) != 0.0)tripletListA.emplace_back(ir + 2, ic + 1, -jacobian(2,1));
					if(jacobian(2,2) != 0.0)tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(2,2));
					if(jacobian(2,3) != 0.0)tripletListA.emplace_back(ir + 2, ic + 3, -jacobian(2,3));
					if(jacobian(2,4) != 0.0)tripletListA.emplace_back(ir + 2, ic + 4, -jacobian(2,4));
					if(jacobian(2,5) != 0.0)tripletListA.emplace_back(ir + 2, ic + 5, -jacobian(2,5));
					if(jacobian(2,6) != 0.0)tripletListA.emplace_back(ir + 2, ic + 6, -jacobian(2,6));

					ic = nodes.size() * 7 + nodes[i].measurements[j].index_landmark * 3;
					if(jacobian(2,7) != 0.0)tripletListA.emplace_back(ir + 2, ic + 0, -jacobian(2,7));
					if(jacobian(2,8) != 0.0)tripletListA.emplace_back(ir + 2, ic + 1, -jacobian(2,8));
					if(jacobian(2,9) != 0.0)tripletListA.emplace_back(ir + 2, ic + 2, -jacobian(2,9));

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

			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,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);
			tripletListP.emplace_back(ir + 6 , ir + 6, 10000000000000);

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

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

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

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

				tripletListP.emplace_back(ir, ir, 1000000.0);

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

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

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

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

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

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

			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
			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){
					std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl;
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == nodes.size() * 7 + landmarks.size() * 3){
				std::cout << "optimization success" << std::endl;
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}

				int counter = 0;

				for(size_t i = 0 ; i < nodes.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(nodes[i].pose);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];
					nodes[i].pose = affine_matrix_from_pose_quaternion(pose);
				}
				for(size_t i = 0 ; i < landmarks.size(); i++){
					landmarks[i].x() += h_x[counter++];
					landmarks[i].y() += h_x[counter++];
					landmarks[i].z() += h_x[counter++];
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

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

	mouse_old_x = x;
	mouse_old_y = y;
}

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

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

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

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

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

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to poses" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}

void calculate_ICP_COV(std::vector<PointMeanCov>& data_pi,
		std::vector<PointMeanCov>& model_qi, Eigen::Affine3d transform, Eigen::MatrixXd& ICP_COV)
{
	TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transform);

    Eigen::MatrixXd d2sum_dbeta2(6,6);
    d2sum_dbeta2 = Eigen::MatrixXd::Zero(6,6);

    for (size_t s = 0; s < data_pi.size(); ++s )
    {
        double pix = data_pi[s].coords.x();
        double piy = data_pi[s].coords.y();
        double piz = data_pi[s].coords.z();
        double qix = model_qi[s].coords.x();
        double qiy = model_qi[s].coords.y();
        double qiz = model_qi[s].coords.z();

        Eigen::Matrix<double, 6, 6, Eigen::RowMajor> d2sum_dbeta2i;
		point_to_point_source_to_landmark_tait_bryan_wc_d2sum_dbeta2(d2sum_dbeta2i, pose.px, pose.py, pose.pz, pose.om,
        			pose.fi, pose.ka, pix, piy, piz, qix, qiy, qiz);

        Eigen::MatrixXd d2sum_dbeta2_temp(6,6);
        d2sum_dbeta2_temp << d2sum_dbeta2i;
        d2sum_dbeta2 = d2sum_dbeta2 + d2sum_dbeta2_temp;
    }

    int n = data_pi.size();
    if (n > 200) n = 200;
    Eigen::MatrixXd d2sum_dbetadx(6,6*n);
    for (int k = 0; k < n ; ++k)
    {
        double pix = data_pi[k].coords.x();
        double piy = data_pi[k].coords.y();
        double piz = data_pi[k].coords.z();
        double qix = model_qi[k].coords.x();
        double qiy = model_qi[k].coords.y();
        double qiz = model_qi[k].coords.z();

        Eigen::MatrixXd d2sum_dbetadx_temp(6,6);
        Eigen::Matrix<double, 6, 6, Eigen::RowMajor> d2sum_dbetadxi;
        point_to_point_source_to_landmark_tait_bryan_wc_d2sum_dbetadx(d2sum_dbetadxi, pose.px, pose.py, pose.pz, pose.om,
        		pose.fi, pose.ka, pix, piy, piz, qix, qiy, qiz);

        d2sum_dbetadx_temp << d2sum_dbetadxi;
        d2sum_dbetadx.block<6,6>(0,6*k) = d2sum_dbetadx_temp;
    }

    Eigen::MatrixXd cov_x(6*n,6*n);
    cov_x = 0.0 * Eigen::MatrixXd::Identity(6*n,6*n);

    for(size_t i = 0; i < n ; i ++){
    	int row = i * 6;
    	int col = i * 6;

    	cov_x(row, col + 0) = data_pi[i].cov(0,0);
    	cov_x(row, col + 1) = data_pi[i].cov(0,1);
    	cov_x(row, col + 2) = data_pi[i].cov(0,2);

    	cov_x(row + 1, col + 0) = data_pi[i].cov(1,0);
    	cov_x(row + 1, col + 1) = data_pi[i].cov(1,1);
    	cov_x(row + 1, col + 2) = data_pi[i].cov(1,2);

    	cov_x(row + 2, col + 0) = data_pi[i].cov(2,0);
    	cov_x(row + 2, col + 1) = data_pi[i].cov(2,1);
    	cov_x(row + 2, col + 2) = data_pi[i].cov(2,2);

    	cov_x(row + 3, col + 3 + 0) = model_qi[i].cov(0,0);
    	cov_x(row + 3, col + 3 + 1) = model_qi[i].cov(0,1);
    	cov_x(row + 3, col + 3 + 2) = model_qi[i].cov(0,2);

    	cov_x(row + 4, col + 3 + 0) = model_qi[i].cov(1,0);
    	cov_x(row + 4, col + 3 + 1) = model_qi[i].cov(1,1);
    	cov_x(row + 4, col + 3 + 2) = model_qi[i].cov(1,2);

    	cov_x(row + 5, col + 3 + 0) = model_qi[i].cov(2,0);
    	cov_x(row + 5, col + 3 + 1) = model_qi[i].cov(2,1);
    	cov_x(row + 5, col + 3 + 2) = model_qi[i].cov(2,2);
    }
    ICP_COV =  d2sum_dbeta2.inverse() * d2sum_dbetadx * cov_x * d2sum_dbetadx.transpose() * d2sum_dbeta2.inverse();
}


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

#include <string>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <vector>
#include <map>
#include <time.h>

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>

#include <graphlab.hpp>
#include <graphlab/graph/distributed_graph.hpp>

//shared parameters
float gaussian_kernel_scale_parameter = 0.1;
float threshold_to_discard_small_similarities = 0.0;
size_t number_of_nearest_neighbors= 20;

//data point
struct vertex_data {
  std::vector<float> x;
  float D_ii;
  vertex_data():x(), D_ii(0.0) {};
  explicit vertex_data(const std::vector<float>& x_in) :
      x(x_in), D_ii(0.0) {}

  void save(graphlab::oarchive& oarc) const {
    oarc << x.size();
    for(size_t i=0;i<x.size();++i)
      oarc << x[i];
    oarc << D_ii;
  }

  void load(graphlab::iarchive& iarc) {
    size_t size = 0;
    iarc >> size;
    for(size_t i=0;i<size;++i){
      float temp = 0.0;
      iarc >> temp;
      x.push_back(temp);
    }
    iarc >> D_ii;
  }
};

//similarity
struct edge_data{
  float A_ij;
  bool nearest;
  edge_data() : A_ij(0.0), nearest(false){}
  void save(graphlab::oarchive& oarc) const {
    oarc << A_ij << nearest;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> A_ij >> nearest;
  }
};

typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

//[vertex_id] [element1] [element2] [element3] ...
bool line_parser(graph_type& graph, const std::string& filename,
    const std::string& line) {
  if (line.empty()) return true;
  size_t id = 0;
  namespace qi = boost::spirit::qi;
  namespace ascii = boost::spirit::ascii;
  namespace phoenix = boost::phoenix;
  vertex_data vtx;
  const bool success = qi::phrase_parse
    (line.begin(), line.end(),
     //  Begin grammar
     (
      qi::ulong_[phoenix::ref(id) = qi::_1] >> -qi::char_(",") >>
      (qi::double_[phoenix::push_back(phoenix::ref(vtx.x), qi::_1)] % -qi::char_(",") )
      )
     ,
     //  End grammar
     ascii::space);
  if (!success) return false;
  graph.add_vertex(id, vtx);

  for(size_t i=1;i<id;++i){
    graph.add_edge(i, id);
  }

  return true;
}

// helper function to compute similarity between points
float similarity(const std::vector<float>& v1, const std::vector<float>& v2) {
  float ret = 0.0;
  for (size_t i = 0; i < v1.size(); ++i) {
    float tmp = v1[i] - v2[i];
    ret += tmp * tmp;
  }
  return exp(-ret / gaussian_kernel_scale_parameter);
}

//calculate similarities between data points
void calc_similarities(graph_type::edge_type& edata) {
  edata.data().A_ij = similarity(edata.source().data().x, edata.target().data().x);
}

//discard small similarities (Optional)
void discard_small_similarity(graph_type::edge_type& edata) {
  if(edata.data().A_ij < threshold_to_discard_small_similarities)
    edata.data().A_ij = 0.0;
}

//gather T-nearest neighbor (Optional)
struct top_t_similarity{
  std::vector<size_t> ids;
  std::vector<float> sims;
  top_t_similarity(): ids(number_of_nearest_neighbors, std::numeric_limits<size_t>::max()),
      sims(number_of_nearest_neighbors, -1.0){}
  top_t_similarity(size_t id, float sim): ids(number_of_nearest_neighbors, std::numeric_limits<size_t>::max()),
      sims(number_of_nearest_neighbors, -1.0){
    ids[0] = id;
    sims[0] = sim;
  }

  top_t_similarity& operator+=(const top_t_similarity& other){
    std::vector<size_t> new_ids;
    std::vector<float> new_sims;
    size_t pos1=0;
    size_t pos2=0;
    while(pos1+pos2 < number_of_nearest_neighbors){
      if(sims[pos1] >= other.sims[pos2]){
        new_ids.push_back(ids[pos1]);
        new_sims.push_back(sims[pos1]);
        pos1++;
      }else{
        new_ids.push_back(other.ids[pos2]);
        new_sims.push_back(other.sims[pos2]);
        pos2++;
      }
    }
    ids = new_ids;
    sims = new_sims;
    return *this;
  }

  void save(graphlab::oarchive& oarc) const {
    oarc << ids.size();
    for(size_t i=0;i<ids.size();++i)
      oarc << ids[i];
    for(size_t i=0;i<sims.size();++i)
      oarc << sims[i];
  }
  void load(graphlab::iarchive& iarc) {
    ids.clear();
    sims.clear();
    size_t size = 0;
    iarc >> size;
    for(size_t i=0;i<size;++i){
      size_t id = 0;
      iarc >> id;
      ids.push_back(id);
    }
    for(size_t i=0;i<size;++i){
      float sim = 0;
      iarc >> sim;
      sims.push_back(sim);
    }
  }
};

//get T-nearest neighbor and discard others (Optional)
class t_nearest: public graphlab::ivertex_program<graph_type,
  top_t_similarity>, public graphlab::IS_POD_TYPE {
private:
  float threshold;

public:
  t_nearest():threshold(0.0){}

  edge_dir_type gather_edges(icontext_type& context,
      const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  top_t_similarity gather(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
    if(edge.target().id() == vertex.id()){//in edge
      return top_t_similarity(edge.source().id(), edge.data().A_ij);
    }else{//out edge
      return top_t_similarity(edge.target().id(), edge.data().A_ij);
    }
  }

  //assign a cluster, considering the clusters of neighbors
  void apply(icontext_type& context, vertex_type& vertex,
      const gather_type& total) {
    threshold = total.sims[number_of_nearest_neighbors-1];
//    std::cout << vertex.id() << "\t" << total.ids[0] << "-" << total.sims[0] << ", "
//        << total.ids[1] << "-" << total.sims[1] << std::endl;
  }

  edge_dir_type scatter_edges(icontext_type& context,
      const vertex_type& vertex) const {
      return graphlab::ALL_EDGES;
  }
  void scatter(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
    if(edge.data().A_ij >= threshold)
      edge.data().nearest = true;
  }
};

//discard small similarities (Optional)
void make_other_similarities_zero(graph_type::edge_type& edata) {
  if(edata.data().nearest == false)
    edata.data().A_ij = 0.0;
}

//compute sums over rows and then take inverse square root
class calc_degrees: public graphlab::ivertex_program<graph_type,
    float>, public graphlab::IS_POD_TYPE {
public:
  //gather A_ij
  edge_dir_type gather_edges(icontext_type& context,
      const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  float gather(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
    return edge.data().A_ij;
  }

  //assign a cluster, considering the clusters of neighbors
  void apply(icontext_type& context, vertex_type& vertex,
      const gather_type& total) {
    vertex.data().D_ii = 1.0 / sqrt(total);
  }

  edge_dir_type scatter_edges(icontext_type& context,
      const vertex_type& vertex) const {
      return graphlab::NO_EDGES;
  }
  void scatter(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
  }
};

//multiply D^-1/2
void mult_D(graph_type::edge_type& edata) {
  edata.data().A_ij = edata.data().A_ij * edata.source().data().D_ii * edata.target().data().D_ii;
}

struct max_min_similarity{
  float max_sim;
  float min_sim;

  max_min_similarity(): max_sim(0.0), min_sim(0.0){}
  explicit max_min_similarity(float similarity): max_sim(similarity),
      min_sim(similarity){}

  max_min_similarity& operator+=(const max_min_similarity& other){
    if(max_sim < 1.0 && other.max_sim < 1.0){
      max_sim = std::max(max_sim, other.max_sim);
    }else if(other.max_sim < 1.0){
      max_sim = other.max_sim;
    }
    if(min_sim > 0.0 && other.min_sim > 0.0){
      min_sim = std::min(min_sim, other.min_sim);
    }else if(other.min_sim > 0.0){
      min_sim = other.min_sim;
    }
    return *this;
  }
  void save(graphlab::oarchive& oarc) const {
    oarc << max_sim << min_sim;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> max_sim >> min_sim;
  }
};
max_min_similarity absolute_edge_data(const graph_type::edge_type& edge) {
  return max_min_similarity(edge.data().A_ij);
}

struct max_vid{
  size_t vid;
  max_vid(): vid(0){}
  explicit max_vid(size_t in_vid): vid(in_vid){}

  max_vid& operator+=(const max_vid& other){
    vid = std::max(vid, other.vid);
    return *this;
  }
  void save(graphlab::oarchive& oarc) const {
    oarc << vid;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> vid;
  }
};
max_vid absolute_vertex_data(const graph_type::vertex_type& vertex) {
  return max_vid(vertex.id());
}

class graph_writer {
public:
  std::string save_vertex(graph_type::vertex_type v) {
    std::stringstream strm;
    size_t vid = v.id();
    if(vid == 0)
      return "";
    strm << vid << " " << vid << " 1.0\n";
    return strm.str();
  }

  std::string save_edge(graph_type::edge_type e) {
    const float& A_ij = e.data().A_ij;
    std::stringstream strm;
    if(A_ij > 0.0){
      strm << e.source().id() << " " << e.target().id() << " " <<
          A_ij << "\n";
      strm << e.target().id() << " " << e.source().id() << " " <<
          A_ij << "\n";
    }
    return strm.str();
  }
};

int main(int argc, char** argv) {
  std::cout << "construct graph Laplacian for spectral clustering.\n\n";

  //parse command line
  std::string datafile;
  graphlab::command_line_options clopts
    ("Constructing graph Laplacian for spectral clustering");
  clopts.attach_option("data", datafile,
                       "Input file. Each line hold a sample id followed by a white-space or "
                       "comma separated numeric vector. Id should start from 1");
  clopts.attach_option("sigma",  gaussian_kernel_scale_parameter,
                       "Scale parameter for Gaussian kernel.");
  clopts.attach_option("similarity-thres", threshold_to_discard_small_similarities,
                       "Threshold to discard small similarities. ");
  clopts.attach_option("t-nearest", number_of_nearest_neighbors,
                      "Number of nearest neighbors (=t). Will use only the t-nearest similarities "
                      "for each datapoint. If set at 0, will use all similarities.");
  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (datafile == "") {
    std::cout << "--data is not optional\n";
    return EXIT_FAILURE;
  }
  gaussian_kernel_scale_parameter *= 2.0*gaussian_kernel_scale_parameter;

  //construct graph
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;
  graph_type graph(dc, clopts);
  graph.load(
      datafile,
      line_parser);
  graph.finalize();

  time_t start, end;
  time(&start);
  size_t data_num = graph.map_reduce_vertices<max_vid>(absolute_vertex_data).vid;

  //calculate similarities
  graph.transform_edges(calc_similarities);

  //show the max similarity less than 1 and the min similarity grater than 0
  max_min_similarity stat = graph.map_reduce_edges<max_min_similarity>(absolute_edge_data);
  dc.cout() << "max squared distance(min similarity): "
      << -log(stat.min_sim)*gaussian_kernel_scale_parameter
      << "(" << stat.min_sim << ")\n"
      << "min squared distance(max similarity):"
      << -log(stat.max_sim)*gaussian_kernel_scale_parameter
      << "(" << stat.max_sim << ")\n";

  //if t is set, use only t-nearest similarities
  if(number_of_nearest_neighbors > 0){
    if(number_of_nearest_neighbors > data_num-1)
      number_of_nearest_neighbors = data_num-1;
    dc.cout() << "use only the " << number_of_nearest_neighbors
        << "-nearest similarities for each datapoint\n";
    graphlab::omni_engine<t_nearest> engine_nearest(dc, graph, "sync", clopts);
    engine_nearest.signal_all();
    engine_nearest.start();
    graph.transform_edges(make_other_similarities_zero);
  }
  //if threshold is set, discard similarities less then the threshold
  if(threshold_to_discard_small_similarities > 0.0){
    dc.cout() << "discard small similarities less than "
        << threshold_to_discard_small_similarities << "\n";
    graph.transform_edges(discard_small_similarity);
  }

  //sum elements over rows (calculate the degree matrix D)
  graphlab::omni_engine<calc_degrees> engine(dc, graph, "sync", clopts);
  engine.signal_all();
  engine.start();
  //multiply D
  graph.transform_edges(mult_D);
  time(&end);

  dc.cout() << "graph calculation time is " << (end - start) << " sec\n";
  dc.cout() << "writing...\n";

  //write results
  const std::string outputname = datafile + ".glap";
  graph.save(
      outputname + "_diag",
      graph_writer(), false, //set to true if each output file is to be gzipped
      true, //whether vertices are saved
      false,1); //whether edges are saved
  graph.save(
      outputname + "_other",
      graph_writer(), false, //set to true if each output file is to be gzipped
      false, //whether vertices are saved
      true,1); //whether edges are saved

  //write the number of data
  const std::string datanum_filename = datafile + ".datanum";
  std::ofstream ofs(datanum_filename.c_str());
  if(!ofs) {
    std::cout << "can't create file for number of data" << std::endl;
    return EXIT_FAILURE;
  }
  ofs << data_num;

  graphlab::mpi_tools::finalize();

  return EXIT_SUCCESS;
}


/*
 * pose.hpp
 *
 *  Copyright (c) 2013 Kumar Robotics. All rights reserved.
 *
 *  This file is part of kr_math.
 *
 *  Created on: 28/06/2014
 *      Author: gareth
 */

#ifndef KR_MATH_POSE_H_
#define KR_MATH_POSE_H_

/* GTSAM include first since it needs to be included before Eigen as they
 * have a custom Eigen version where they overload some Eigen classes. Having it
 * after the Eigen include prevents their Eigen header being included due to
 * header guards
 */
#ifdef KR_MATH_GTSAM_CONVERSIONS
#include <gtsam/geometry/Pose3.h>
#endif

#include "base_types.hpp"
#include <Eigen/Geometry>

#ifdef KR_MATH_ROS_CONVERSIONS
#include <geometry_msgs/Pose.h>
#endif

namespace kr {

/**
 * @brief Pose, an orientation and position in R3.
 *
 * @note Some important notes on convention:
 *
 * When we write _A to B_ transformation (or transform), it is in reference to
 * the operation effecuated by matrix H, such that:
 *
 * b = H * a, where a is a vector in the A frame and b is that same vector
 * in the B frame.
 *
 * kr::Pose stores `q`, which rotates vectors from body to world frame,
 * and `p`, the center of the body as expressed in the world frame.
 *
 * Transforming a vector v from world to camera (body) is performed with:
 *
 * vb = R(q') * (vw - p), where R(.) maps from S(4) to SO(3).
 *
 * It is important to note that p is equivalent to the body origin in world
 * frame, often denoted wTb.
 *
 */
template <typename Scalar> struct Pose {
private:
  kr::Quat<Scalar> q_; /**< Orientation (body to world transform, or wRb) */
  kr::Vec3<Scalar> p_; /**< Position (origin to body vector) */

public:
  /**
   * @brief Construct with identity pose.
   */
  Pose() : q_(1, 0, 0, 0) { p_[0] = p_[1] = p_[2] = 0; }

  /**
   * @brief Copy-ctor
   */
  Pose(const Pose<Scalar> &pose) : q_(pose.q_), p_(pose.p_) {}

  /**
   * @brief Construct from quaternion and position vector.
   * @param wQb Body to world rotation.
   * @param pInW Origin to body vector.
   */
  Pose(const Quat<Scalar> &wQb, const Vec3<Scalar> &pInW) : q_(wQb), p_(pInW) {}

  /**
   * @brief Construct from a 4x4 homogenous transformation matrix.
   * @param bHw 4x4 matrix representing the body to world transform.
   *
   * @note wHb is expected to perform the operation: vw = wHb * vb.
   */
  Pose(const kr::Mat4<Scalar>& wHb) :
    q_(wHb.template block<3,3>(0,0)), p_(wHb.template block<3,1>(0,3)) {}

#ifdef KR_MATH_ROS_CONVERSIONS
  /**
   * @brief Construct pose from ROS message.
   * @param geo Instance of geometry_msgs::Pose.
   * @note Loss of accuracy will occur if Scalar is float32.
   */
  Pose(const geometry_msgs::Pose &geo) {
    q_ = kr::Quat<Scalar>(geo.orientation.w, geo.orientation.x,
                         geo.orientation.y, geo.orientation.z);
    p_[0] = geo.position.x;
    p_[1] = geo.position.y;
    p_[2] = geo.position.z;
  }

  /**
   * @brief Explicit cast to ROS geometry_msgs::Pose.
   */
  explicit operator geometry_msgs::Pose() const {
    geometry_msgs::Pose geo;
    geo.orientation.w = q_.w();
    geo.orientation.x = q_.x();
    geo.orientation.y = q_.y();
    geo.orientation.z = q_.z();
    geo.position.x = p_[0];
    geo.position.y = p_[1];
    geo.position.z = p_[2];
    return geo;
  }
#endif

#ifdef KR_MATH_GTSAM_CONVERSIONS
  /**
   * @brief Construct pose from GTSAM Pose3.
   * @param gtpose Instance of GTSAM 6DOF pose. GTSAM poses store a body to
   * world transformation.
   */
  Pose(const gtsam::Pose3& gtpose) :
    q_(gtpose.rotation().toQuaternion().cast<Scalar>()),
    p_(gtpose.translation().vector().cast<Scalar>()) {}

  /**
   * @brief Explicit cast to GTSAM Pose3.
   * @note Converts to the GTSAM convention of body to world.
   */
  explicit operator gtsam::Pose3() const {
    const gtsam::Rot3 rot(q_.template cast<double>());
    return gtsam::Pose3(rot, gtsam::Point3(p_.template cast<double>()));
  }
#endif

  /**
   * @brief Position of this pose in the world frame.
   * @return kr::Vec3
   */
  const kr::Vec3<Scalar>& p() const { return p_; }
  kr::Vec3<Scalar>& p() { return p_; }

  /**
   * @brief Quaternion that performs the body to world rotation on vectors.
   * @return kr::Quat
   */
  const kr::Quat<Scalar>& q() const { return q_; }
  kr::Quat<Scalar>& q() { return q_; }

  /**
   * @brief Construct a pose from an rvec/tvec pair.
   *
   * @param rvec Rotation vector. If R = exp(rvec), then R*v will rotate vector
   * v from world to body frame.
   * @param tvec Translation vector, or t = -R*p.
   *
   * @return The corresponding pose.
   *
   * @note The vectors accepted here are in the same form produced by OpenCV.
   */
  static Pose<Scalar> fromVectors(const Vec3<Scalar>& rvec,
                                  const Vec3<Scalar>& tvec) {
    Vec3<Scalar> rnorm(0.0,0.0,0.0);
    const Scalar rn = rvec.norm();
    if (rn > std::numeric_limits<Scalar>::epsilon()*10) {
      rnorm = rvec / rn;
    }
    const Eigen::AngleAxis<Scalar> aa(rn, rnorm);
    Pose<Scalar> pose;
    pose.q_ = kr::Quat<Scalar>(aa).conjugate();
    pose.p_ = -(pose.q_.matrix() * tvec);
    return pose;
  }

  /**
   * @brief Convert this pose so that it is expressed in the body frame of the
   * argument.
   *
   * @param alt Treat as identity reference frame.
   * @return The receiver, as expressed in frame alt.
   */
  Pose<Scalar> expressedIn(const Pose<Scalar> &alt) const {
    const kr::Vec3<Scalar> pn = alt.bRw() * (p_ - alt.p_);
    return Pose(alt.q_.conjugate() * q_, pn);
  }

  /**
   * @brief Generate the inverse transformation.
   * @return Inverse of this pose: [R^T, -R * p]
   *
   * @note If H is the homo. matrix representation of the receiver, then the
   * returned pose will have representation H' where H * H' = eye(4).
   */
  Pose<Scalar> inverse() const {
    return Pose<Scalar>().expressedIn(*this);
  }

  /**
   * @brief Difference between this pose and the argument in world frame.
   * @param alt Pose to subtract from the receiver.
   *
   * @return The difference between the receiver and the argument, as expressed
   * in the world frame.
   */
  Pose<Scalar> difference(const Pose<Scalar> &alt) const {
    return Pose(alt.q_.conjugate() * q_, p_ - alt.p_);
  }

  /**
   * @brief Translation vector of this pose.
   * @return Vector corresponding to: t = -bRw * p.
   */
  kr::Vec3<Scalar> translation() const { return bRw() * -p_; }

  /**
   * @brief Compose a pose onto the receiver.
   * @param rhs Pose to multiply/add onto this one.
   * @return New pose after composition.
   *
   * @note It is assumed that `rhs` expresses a pose in the body frame of the
   * receiver. This operation therefore rotates `rhs` position vector into the
   * world frame prior to addition. Orientation is composed from the right,
   * corresponding to a multiplication in the body frame.
   */
  Pose<Scalar> composeInBody(const Pose<Scalar>& rhs) const {
    return Pose(q_ * rhs.q_, p_ + q_.matrix() * rhs.p_);
  }

  /**
   * @brief Transform a point into the body frame of this pose.
   * @param v Point to transform, expressed in the world frame.
   * @return Point `v` after conversion to the body frame.
   */
  kr::Vec3<Scalar> transformToBody(const kr::Vec3<Scalar>& v) const {
    return bRw() * (v - p_);
  }

  /**
   * @brief Transform a point from the body frame of this pose.
   * @param v Point to transform, expressed in body frame.
   * @return Point `v` after conversion to the world frame.
   */
  kr::Vec3<Scalar> transformFromBody(const kr::Vec3<Scalar>& v) const {
    return wRb()*v + p_;
  }

  /**
   * @brief SE(3) transformation corresponding to this pose.
   * @return 4x4 matrix that performs the body to world transformation: wHb
   */
  kr::Mat4<Scalar> matrix() const {
    kr::Mat4<Scalar> H;
    H.template block<3,3>(0,0) = q_.matrix();
    H.template block<3,1>(0,3) = p_;
    H(3,0) = H(3,1) = H(3,2) = 0;
    H(3,3) = 1;
    return H;
  }

  /**
   * @brief Shorthand for body to world rotation.
   * @note wP = wRb * bP + wTb
   */
  kr::Mat3<Scalar> wRb() const {
    return q_.matrix();
  }

  /**
   * @brief Shorthand for world to body roration.
   * @note bP = bRw * wP + bTw
   */
  kr::Mat3<Scalar> bRw() const {
    return q_.conjugate().matrix();
  }

  /**
   * @brief bTw Translation from body to world in body frame
   * @note bP = bRw * wP + bTw
   * @note Equivalent to the 'translation' vector.
   */
  kr::Vec3<Scalar> bTw() const {
    return translation(); //  -(bRw() * wTb())
  }

  /**
   * @brief Shorthand for translation vector from world origin to body.
   * @note wP = wRb * bP + wTb
   */
  const kr::Vec3<Scalar>& wTb() const {
    return p_;
  }
};

typedef Pose<float> Posef;
typedef Pose<double> Posed;

} // namespace kr

#endif // KR_MATH_POSE_H_


#ifndef _MATRIX_OPERATIONS_HPP_
#define _MATRIX_OPERATIONS_HPP_

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

namespace CIS {
/// @brief Manual transform implementation, recommend using the following instead
///
/// @verbatim
///    Eigen::Affine3d transform;
///
///    transform.setIdentity();
///
///    // Define a translation of 2.5 meters on the x axis.
///    transform.translation() << 2.5, 0.0, 0.0;
///
///    double theta = 0;//boost::math::constants::pi<double>();
///    // The same rotation matrix as before; tetha radians arround Z axis
///    transform.rotate (Eigen::AngleAxisd (theta, Eigen::Vector3d::UnitZ()));
///
///    Eigen::Vector3d v3d(1,1,1);
///
///    Eigen::Vector3d v3d2 = (transform*v3d).transpose();
/// @endverbatim
template<typename Scalar>
Eigen::Matrix<Scalar, 3, 1> transform(const Eigen::Transform<Scalar, 3, Eigen::Affine>& hTrans, const Eigen::Matrix<Scalar, 3, 1>& point){
   return Eigen::Matrix<Scalar, 3, 1>
    (
        hTrans (0, 0) * point(0) + hTrans (0, 1) * point(1) + hTrans (0, 2) * point(2) + hTrans (0, 3),
        hTrans (1, 0) * point(0) + hTrans (1, 1) * point(1) + hTrans (1, 2) * point(2) + hTrans (1, 3),
        hTrans (2, 0) * point(0) + hTrans (2, 1) * point(1) + hTrans (2, 2) * point(2) + hTrans (2, 3)
     );

}
}

/// @todo provide instructions to replace this with eigen built in functionality
/// Create transformation matrix from Rotation R and Translation vector P
Eigen::Matrix4d homogeneousmatrix(Eigen::Matrix3d R, Eigen::Vector3d p)
{
    Eigen::Matrix4d F = Eigen::Matrix4d::Identity();
    F.block<3,3>(0,0) = R;
    F.block<3,1>(0,3) = p;
    return F;
}

/// @todo provide instructions to replace this with eigen built in functionality
/// Computes the inverse of the transformation matrix
Eigen::MatrixXd homogeneousInverse(const Eigen::MatrixXd& F)
{
    Eigen::MatrixXd Finv = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d R = F.block<3,3>(0,0);
    Eigen::Matrix3d Rtrans = R.transpose();
    Finv.block<3,3>(0,0) = Rtrans;
    Finv.block<3,1>(0,3) = -Rtrans*F.block<3,1>(0,3);
    return Finv;
}

/// Swap indexing order of vector of vectors, so if it is row major the returned vv will be column major.
/// @todo this is pretty inefficient, but the lengthts can vary. Maybe store the data differently
template<typename T>
const std::vector<std::vector<T> > swapIndexing(const std::vector<std::vector<T> >& uv){
    std::vector<std::vector<T> > vu;
    for(auto uvi : uv){
        int i = 0;
        for(auto vi :uvi){
            if(i == vu.size()) vu.push_back(std::vector<T>());
            vu[i].push_back(vi);
            ++i;
        }
    }
    return vu;
}

/// @brief combine a vector<vector<T> > into a single vector<T>
template<typename T>
const std::vector<T> concat(const std::vector<std::vector<T> >& uv){
    std::vector<T> u;
    for(auto uvi : uv){
        u.insert(u.end(), uvi.begin(), uvi.end());
    }
    return u;
}

/// @brief split a large Eigen::MatrixXd by rows into a vector of smaller MatrixXd
std::vector<Eigen::MatrixXd> splitRows(const Eigen::MatrixXd& mat,std::size_t numRowsPerMat){

    std::vector<Eigen::MatrixXd> vec;
    std::size_t cols = mat.cols();

    for(std::size_t currentRow = 0; currentRow < mat.rows(); currentRow+=numRowsPerMat){
        Eigen::MatrixXd partialMat(mat.block(currentRow,0,numRowsPerMat,cols));
        vec.push_back(partialMat);
    }

    return vec;
}

Eigen::MatrixXd concatToMatrix(const std::vector<Eigen::Vector3d>& points){
    Eigen::MatrixXd mat;
    mat.resize(points.size(),3);
    int i = 0;
    for(Eigen::Vector3d point : points){
        mat.block<1,3>(i,0) = point.transpose();
        ++i;
    }

    return mat;
}

// splits a stack of vectors that is nx3, into a std::vector of n 3x1 vectors
std::vector<Eigen::Vector3d> splitVectors(const Eigen::MatrixXd& mat){
	std::vector<Eigen::Vector3d> vec;

	for(int i = 0; i < mat.rows(); ++i){
		vec.push_back(Eigen::Vector3d(mat.block<1,3>(i,0).transpose()));
	}

	return vec;
}

#endif // _MATRIX_OPERATIONS_HPP_


#include "include/ellipse.h"
#include "include/component_labelling.h"
#include "include/point_helpers.h"

#include <cmath>
#include <Eigen/Cholesky>
#include <Eigen/Dense>
using namespace Eigen;

#include <limits>

typedef Matrix<double, 5, 5> Matrix5d;
typedef Matrix<double, 5, 1> Vector5d;

int Ellipse_detector::fit(const Component_labeller& cl, const Gradient& gradient,
    const Pointlist& raw_points, int tl_x, int tl_y, int dilate) {

    int width  = gradient.width();
    int height = gradient.height();

    const cv::Mat& grad_x = gradient.grad_x();
    const cv::Mat& grad_y = gradient.grad_y();
    const cv::Mat& grad_m = gradient.grad_magnitude();

    set<iPoint> boundary;

    const int border = 1;
    bool edge_touched = false;
    double mx = 0;
    double my = 0;
    for (size_t i=0; i < raw_points.size(); i++) {
        mx += raw_points[i].x;
        my += raw_points[i].y;
        boundary.insert(iPoint(lrint(raw_points[i].x), lrint(raw_points[i].y)) );

        if (raw_points[i].x <= border || raw_points[i].x >= width - 1 - border ||
            raw_points[i].y <= border || raw_points[i].y >= height - 1 - border) {

            edge_touched = true;
        }
    }

    mx /= raw_points.size();
    my /= raw_points.size();

    if (edge_touched) return 0; // skip objects that touch the edge of the image (ellipses not really allowed there)

    if (std::isnan(mx) || std::isnan(my)) {
        return 0; // 0 -> not a circle
    }

    _dilate(boundary, width, height, dilate);
    _dilate_outer_only(boundary, width, height);

    Matrix<double, 5, 5> wK;
    Matrix<double, 5, 1> K;
    K.setZero();
    Vector3d l;
    l.setZero();
    Matrix<double, 5, 1> rhs;
    rhs.setZero();

    double sum_c4 = 0;

    wK.setZero();
    int count = (int)boundary.size();
    Matrix<double, Eigen::Dynamic, 3> L(count, 3);
    L.setZero();

    double mean_dist = 0;
    int counter = 0;
    for (set<iPoint>::const_iterator it=boundary.begin(); it != boundary.end(); it++) {
        const int& x_pos = it->first;
        const int& y_pos = it->second;

        if (grad_m.at<float>(y_pos, x_pos) > 1e-7) {
            Vector3d v;
            v << x_pos,y_pos,1;
            double dist = sqrt( SQR(x_pos-mx) + SQR(y_pos-my) );
            mean_dist += dist;
            counter++;
        }
    }
    mean_dist /= counter;
    double iso_scale = sqrt(2.0) / mean_dist;

    size_t idx = 0;
    for (set<iPoint>::const_iterator it=boundary.begin(); it != boundary.end(); it++) {
        const int& x_pos = it->first;
        const int& y_pos = it->second;

        if (grad_m.at<float>(y_pos, x_pos) > 1e-7) {

            l[0] = grad_x.at<float>(y_pos, x_pos);
            l[1] = grad_y.at<float>(y_pos, x_pos);
            l[2] = -(grad_x.at<float>(y_pos, x_pos) * (x_pos-mx) * iso_scale + grad_y.at<float>(y_pos, x_pos) * (y_pos-my) * iso_scale);

            L.row(idx++) = l;

        }
    }

    for (size_t r=0; r < (size_t)L.rows(); r++) {

        Vector3d l = L.row(r);

        sum_c4 += SQR(SQR(l[2]));

        K[0] = l[0]*l[0];
        K[1] = l[0]*l[1];
        K[2] = l[1]*l[1];
        K[3] = l[0]*l[2];
        K[4] = l[1]*l[2];

        double weight = (l[0]*l[0] + l[1]*l[1]);

        wK += weight * K * K.transpose();

        rhs -= weight * K*(l[2]*l[2]);
    }

    if (rhs.rows() != 5 || rhs.cols() != 1) {
        printf("rhs undefined\n");
        exit(1);
    }

    bool has_nans = false;
    for (size_t ri=0; ri < rhs.rows(); ri++) {
        if (std::isnan(rhs(ri,0))) {
            has_nans = true;
            return 0;
        }
    }

    Vector5d sol;
    sol = wK.fullPivHouseholderQr().solve(rhs);

    Matrix3d Cstar;
    Cstar(0, 0) = sol[0];
    Cstar(0, 1) = sol[1]*0.5;
    Cstar(0, 2) = sol[3]*0.5;
    Cstar(1, 0) = sol[1]*0.5;
    Cstar(1, 1) = sol[2];
    Cstar(1, 2) = sol[4]*0.5;
    Cstar(2, 0) = sol[3]*0.5;
    Cstar(2, 1) = sol[4]*0.5;
    Cstar(2, 2) = 1;      // F* == 1

    Matrix3d C = Cstar.inverse();
    C *= 1.0/C(2,2);
    _C = C;

    Matrix<double, 1, 5> s;
    s.row(0) = sol;
    double sAAs = (s * wK * (s.transpose()))(0,0);
    double R = (sAAs - 2*(sol.dot(rhs)) + sum_c4) / double(count - 5);
    Matrix<double, 5, 5> cov = wK.inverse();
    cov = cov * R;
    Matrix2d cov2;
    cov2(0, 0) = cov(3, 3);
    cov2(0, 1) = cov(3, 4);
    cov2(1, 0) = cov(4, 3);
    cov2(1, 1) = cov(4, 4);
    JacobiSVD<Matrix2d> svd(cov2, ComputeFullU | ComputeFullV);
    Matrix2d Vs = svd.matrixV();
    Vector2d ws = svd.singularValues();

    Matrix2d V;
    V.row(0) = Vs.row(1);
    V.row(1) = Vs.row(0);

    Vector2d w;
    w[0] = ws[1];
    w[1] = ws[0];

    Vector2d centre_uncertainty;
    centre_uncertainty[0] = sqrt(w[0]) * 0.25;
    centre_uncertainty[1] = sqrt(w[1]) * 0.25;

    // shift the ellipse back to the original pixel coordinate system
    Matrix3d S;
    S.setIdentity();
    S(0, 0) = iso_scale;
    S(1, 1) = iso_scale;
    C = _C = S.transpose()*C*S;
    S.setIdentity();
    S(0, 2) = -(tl_x + mx);
    S(1, 2) = -(tl_y + my);
    C = _C = S.transpose()*C*S;

    int result = _matrix_to_ellipse(C);

    if (minor_axis > major_axis) {
        _C = -_C;
        _matrix_to_ellipse(C);
    }

    bool gradient_ok = gradient_check(cl, gradient, raw_points);

    int is_circle = 0;

    if (result == 0 &&
        major_axis >= min_major_axis &&
        minor_axis >= min_minor_axis &&
        gradient_ok) {

        is_circle = 1;
    }

    if (std::isnan(centroid_x) || std::isnan(centroid_y) || std::isnan(major_axis) || std::isnan(minor_axis)) {
        is_circle = 0;
    }

    printf("centre (%.2lf, %.2lf), major = %lf, minor = %lf, angle = %lf, is_circle = %d\n",
        centroid_x, centroid_y, major_axis, minor_axis, angle/M_PI*180.0, is_circle);

    if (is_circle) {
        scanset.clear();
        for (size_t i=0; i < raw_points.size(); i++) {
            int ix = lrint(raw_points[i].x);
            int iy = lrint(raw_points[i].y);

            map<int, scanline>::iterator it = scanset.find(iy);
            if (it == scanset.end()) {
                scanline sl(ix,ix);
                scanset.insert(make_pair(iy, sl));
            }
            if (ix < scanset[iy].start) {
                scanset[iy].start = ix;
            }
            if (ix > scanset[iy].end) {
                scanset[iy].end = ix;
            }
        }
        int clabel = cl(lrint(raw_points[0].x), lrint(raw_points[0].y));
        printf("label used for scanset: %d\n", clabel);
        int total = 0;
        int foreground = 0;
        for (map<int, scanline>::iterator it=scanset.begin(); it != scanset.end(); it++) {
            int y=it->first;
            for (int x=it->second.start; x <= it->second.end; x++) {
                if (cl(x,y) == clabel) {
                    foreground++;
                }
                total++;
            }
        }
        fg_fraction = double(foreground)/double(total);
    }

    return is_circle;
}

void Ellipse_detector::_dilate(set<iPoint>& s, int width, int height, int iters) {
    const int border = 1;

    if (iters > 0) {

        set<iPoint> gen_s;

        for (int k=0; k < iters; k++) {
            for (set<iPoint>::const_iterator it=s.begin(); it != s.end(); it++) {

                gen_s.insert(*it);

                int left = max(border, it->first-1);
                int right = min(width-1-border, it->first+1);
                int top = max(border, it->second-1);
                int bottom = min(height-1-border, it->second+1);

                gen_s.insert(iPoint(right, it->second));
                gen_s.insert(iPoint(right, bottom));
                gen_s.insert(iPoint(it->first, bottom));
                gen_s.insert(iPoint(left, bottom));
                gen_s.insert(iPoint(left, it->second));
                gen_s.insert(iPoint(left, top));
                gen_s.insert(iPoint(it->first, top));
                gen_s.insert(iPoint(right, top));

            }
            s = gen_s; // copy it back
        }
    }
}

void Ellipse_detector::_dilate_outer_only(set<iPoint>& s, int width, int height) {
    const int border = 1;

    double cx = 0;
    double cy = 0;
    for (set<iPoint>::const_iterator it=s.begin(); it != s.end(); it++) {
        cx += it->first;
        cy += it->second;
    }
    cx /= s.size();
    cy /= s.size();

    set<iPoint> gen_s;

    for (set<iPoint>::const_iterator it=s.begin(); it != s.end(); it++) {

        gen_s.insert(*it);

        Point2d dir(it->first - cx, it->second - cy); // current radial direction

        int left = max(border, it->first-1);
        int right = min(width-1-border, it->first+1);
        int top = max(border, it->second-1);
        int bottom = min(height-1-border, it->second+1);

        if ((right - it->first)*dir.x >= 0) gen_s.insert(iPoint(right, it->second));
        if ((right - it->first)*dir.x >= 0 && (bottom - it->second)*dir.y >= 0) gen_s.insert(iPoint(right, bottom));
        if ((bottom - it->second)*dir.y >= 0) gen_s.insert(iPoint(it->first, bottom));
        if ((left - it->first)*dir.x >= 0 && (bottom - it->second)*dir.y >= 0) gen_s.insert(iPoint(left, bottom));
        if ((left - it->first)*dir.x >= 0) gen_s.insert(iPoint(left, it->second));
        if ((left - it->first)*dir.x >= 0 && (top - it->second)*dir.y >= 0) gen_s.insert(iPoint(left, top));
        if ((top - it->second)*dir.y >= 0) gen_s.insert(iPoint(it->first, top));
        if ((right - it->first)*dir.x >= 0 && (top - it->second)*dir.y >= 0) gen_s.insert(iPoint(right, top));

    }
    s = gen_s; // copy it back
}

int Ellipse_detector::_matrix_to_ellipse(Matrix3d& C) {

    double a = C(0,0);
    double b = C(0,1)*2;
    double c = C(1,1);
    double d = C(0,2)*2;
    double e = C(1,2)*2;
    double f = C(2,2);

    double thetarad = 0.5*atan2(b, a - c);
    double cost = cos(thetarad);
    double sint = sin(thetarad);
    double sin_squared = sint*sint;
    double cos_squared = cost*cost;
    double cos_sin = sint*cost;

    double Ao = f;
    double Au =   d * cost + e * sint;
    double Av = - d * sint + e * cost;
    double Auu = a * cos_squared + c * sin_squared + b * cos_sin;
    double Avv = a * sin_squared + c * cos_squared - b * cos_sin;

    if(Auu==0 || Avv==0) {
        // invalid ellipse
        return -1;
    }

    double tuCentre = - Au/(2*Auu);
    double tvCentre = - Av/(2*Avv);
    double wCentre = Ao - Auu*tuCentre*tuCentre - Avv*tvCentre*tvCentre;

    double uCentre = tuCentre * cost - tvCentre * sint;
    double vCentre = tuCentre * sint + tvCentre * cost;

    double Ru = -wCentre/Auu;
    double Rv = -wCentre/Avv;

    Ru = sqrt(fabs(Ru))*(Ru < 0 ? -1 : 1);
    Rv = sqrt(fabs(Rv))*(Rv < 0 ? -1 : 1);

    centroid_x = uCentre;
    centroid_y = vCentre;
    major_axis = Ru;
    minor_axis = Rv;
    angle = thetarad;

    return 0;
}

double Ellipse_detector::calculate_curve_length(const Pointlist& points) {

    int n = points.size();

    // calculate the curve length of the  boundary
    double curve_len = 0;
    double prev_x = points[0].x - centroid_x;
    double prev_y = points[0].y - centroid_y;
    for (int i=1; i < n; i++) {
        double x = points[i].x - centroid_x;
        double y = points[i].y - centroid_y;
        curve_len += sqrt(SQR(x - prev_x) + SQR(y - prev_y));
        prev_x = x;
        prev_y = y;
    }
    curve_len += sqrt(SQR(points[0].x - centroid_x - prev_x) +
        SQR(points[0].y - centroid_y - prev_y));

    return curve_len;
}

bool Ellipse_detector::gradient_check(const Component_labeller& cl, const Gradient& gradient, const Pointlist& raw_points) {

    // gradient just outside ellipse must be perpendicular to ellipse tangent
    double cosa = cos(-angle);
    double sina = sin(-angle);
    int neighbours[8][2] = {
        {-1, -1}, {0, -1}, {1, -1},
        {-1, 0}, {1, 0},
        {-1,  1}, {0,  1}, {1, 1}
    };

    // TODO: we need improved handling for objects falling on the edge of the scene?

    int not_fg_count = 0;
    vector<double> phi_diff;
    for (size_t i=0; i < raw_points.size(); i++) {
        // now generate a point just outside the ellipse ....
        int ox = lrint(raw_points[i].x);
        int oy = lrint(raw_points[i].y);
        int px = ox;
        int py = oy;
        double maxdist = sqrt((px - centroid_x)*(px - centroid_x) + (py - centroid_y)*(py - centroid_y));
        for (int n=0; n < 8; n++) {
            int lx = px + neighbours[n][0];
            int ly = py + neighbours[n][1];

            if (lx >= 5 && lx < gradient.width() - 5 &&
                ly >= 5 && ly < gradient.height() - 5) {

                double dist = sqrt((lx - centroid_x)*(lx - centroid_x) + (ly - centroid_y)*(ly - centroid_y));
                if (dist > maxdist) {
                    px = lx;
                    py = ly;
                    maxdist = dist;
                }
            }
        }

        // points just outside ellipse should have labels of 0 or -1 (not foreground)
        not_fg_count += cl(px, py) <= 0 ? 1 : 0;

        Point2d d(raw_points[i].x - centroid_x, raw_points[i].y - centroid_y);
        double rx = cosa*d.x - sina*d.y;
        double ry = sina*d.x + cosa*d.y;
        double theta = atan2(ry, rx); // ellipse curve parameter theta
        Point2d tangent = normalize(Point2d(-major_axis*sin(theta), minor_axis*cos(theta)));
        // rotate the tangent vector back to image coordinates
        rx = cosa*tangent.x + sina*tangent.y;
        ry = (-sina)*tangent.x + cosa*tangent.y;
        tangent.x = rx;
        tangent.y = ry;

        Point2d grad = normalize(Point2d(gradient.grad_x().at<float>(py, px), gradient.grad_y().at<float>(py, px)));

        double dot = tangent.x*grad.x + tangent.y*grad.y;
        double phi = acos(dot);

        phi_diff.push_back(phi/M_PI*180 - 90);
    }
    if ((raw_points.size() - not_fg_count) > 1) {
        return false; // we can leave early here ...
    }

    sort(phi_diff.begin(), phi_diff.end());
    const double phi_percentile = 0.9;

    double phi_delta = phi_diff[phi_percentile*phi_diff.size()];

    return (phi_delta < max_ellipse_gradient_error) && phi_delta >= 0;
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Transformation.hpp
/// \brief Header file for a transformation matrix class.
/// \details Light weight transformation class, intended to be fast, and not to provide
///          unnecessary functionality.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////
///
/// A note on EIGEN_MAKE_ALIGNED_OPERATOR_NEW (Sean Anderson, as of May 23, 2013)
/// (also see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
///
/// Fortunately, Eigen::Matrix3d and Eigen::Vector3d are NOT 16-byte vectorizable,
/// therefore this class should not require alignment, and can be used normally in STL.
///
/// To inform others of the issue, classes that include *fixed-size vectorizable Eigen types*,
/// see http://eigen.tuxfamily.org/dox-devel/group__TopicFixedSizeVectorizable.html,
/// must include the above macro! Furthermore, special considerations must be taken if
/// you want to use them in STL containers, such as std::vector or std::map.
/// The macro overloads the dynamic "new" operator so that it generates
/// 16-byte-aligned pointers, this MUST be in the public section of the header!
///
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGM_TRANSFORMATION_HPP
#define LGM_TRANSFORMATION_HPP

#include <Eigen/Dense>

namespace lgmath {
namespace se3 {

class Transformation
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(const Transformation&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(Transformation&& T) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor (from Eigen)
  //////////////////////////////////////////////////////////////////////////////////////////////
  explicit Transformation(const Eigen::Matrix4d& T);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = [C_ba, -C_ba*r_ba_ina; 0 0 0 1]
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(const Eigen::Matrix3d& C_ba, const Eigen::Vector3d& r_ba_ina);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab)
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation(const Eigen::Matrix<double,6,1>& xi_ab, unsigned int numTerms = 0);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The transformation will be T_ba = vec2tran(xi_ab), xi_ab must be 6x1
  //////////////////////////////////////////////////////////////////////////////////////////////
  // explicit because you want operator*(Eigen::Vector4d) and operator*(this) --- ambiguous
  explicit Transformation(const Eigen::VectorXd& xi_ab);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Destructor. Default implementation.
  //////////////////////////////////////////////////////////////////////////////////////////////
  ~Transformation() = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy assignment operator. Default implementation causes functional failure.
  /// \todo (yuchen) Figure out why default does not work.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation& operator=(const Transformation&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move assignment operator. Manually implemented as Eigen doesn't support moving.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation& operator=(Transformation&& T) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets basic matrix representation of the transformation
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix4d matrix() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the underlying rotation matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  const Eigen::Matrix3d& C_ba() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the "forward" translation r_ba_ina = -C_ba.transpose()*r_ab_inb
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d r_ba_ina() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the underlying r_ab_inb vector.
  //////////////////////////////////////////////////////////////////////////////////////////////
  const Eigen::Vector3d& r_ab_inb() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the corresponding Lie algebra using the logarithmic map
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double,6,1> vec() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the inverse matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  Transformation inverse() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the 6x6 adjoint transformation matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double,6,6> adjoint() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Reproject the transformation matrix back onto SE(3). Setting force to false
  ///        triggers a conditional reproject that only happens if the determinant is of the
  ///        rotation matrix is poor; this is more efficient than always performing it.
  //////////////////////////////////////////////////////////////////////////////////////////////
  void reproject(bool force = true);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation& operator*=(const Transformation& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation operator*(const Transformation& T_rhs) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply this matrix by the inverse of T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation& operator/=(const Transformation& T_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply this matrix by the inverse of T_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Transformation operator/(const Transformation& T_rhs) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply this matrix by the homogeneous vector p_a
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector4d operator*(const Eigen::Ref<const Eigen::Vector4d>& p_a) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Rotation matrix from a to b
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix3d C_ba_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Translation vector from b to a, expressed in frame b
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d r_ab_inb_;
};

} // se3
} // lgmath

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief print transformation
//////////////////////////////////////////////////////////////////////////////////////////////
std::ostream& operator<<(std::ostream& out, const lgmath::se3::Transformation& T);

#endif // LGM_TRANSFORMATION_HPP