// Unit test for the ConicFit class
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Includes
#include <rc_utils/conicfit.h>
#include <Eigen/Geometry>
#include <gtest/gtest.h>

// Namespaces
using namespace rc_utils;

// Defines
#define TOL_HIGH 1e-14
#define TOL_MED  1e-12
#define TOL_LOW  1e-10

//
// Helper functions
//

// Generate 3D data from 2D data by adding z values of a given mean
void gen3DFrom2D(ConicFit::Points3D& PP, const ConicFit::Points2D& P, double Z)
{
	// Retrieve the size
	size_t N = P.size();

	// Clear the output vector
	PP.clear();

	// If size is small then just write the constant z value in
	if(N < 3)
	{
		for(size_t i = 0; i < N; i++)
			PP.push_back(Eigen::Vector3d(P[i].x(), P[i].y(), Z));
		return;
	}

	// If larger then generate some more interesting data
	double Nm = 0.5*(N - 1);
	for(size_t i = 0; i < N; i++)
		PP.push_back(Eigen::Vector3d(P[i].x(), P[i].y(), Z + (0.5*Z + 1.0)*(i - Nm)));
}

// Generate random weights within a particular interval
void genWeights(ConicFit::Weights& W, size_t N, double minW, double maxW)
{
	// Generate the required weights
	W.resize(N);
	for(size_t i = 0; i < N; i++)
		W[i] = minW + drand48()*(maxW - minW);
}

// Generate random weights within a particular interval
void genWeights(ConicFit::WeightedPoints2D& WP, double minW, double maxW)
{
	// Generate the required weights
	size_t N = WP.size();
	for(size_t i = 0; i < N; i++)
		WP[i].z() = minW + drand48()*(maxW - minW);
}

// Merge a set of weighted points from a set of weights and a set of points
void mergeWeights(ConicFit::WeightedPoints2D& WP, const ConicFit::Points2D& P, const ConicFit::Weights& W)
{
	// Merge the two sets together
	size_t N = std::min(P.size(), W.size());
	WP.resize(N);
	for(size_t i = 0; i < N; i++)
	{
		WP[i].head<2>() = P[i];
		WP[i].z() = W[i];
	}
}

//
// Test functions
//

// Test circle fitting
TEST(ConicFitTest, testFitCircle)
{
	// Declare variables
	Eigen::Vector2d FC1, FC2, FC3, FC4;
	Eigen::Vector3d FCC1, FCC2, FCC3, FCC4;
	double FR1, FR2, FR3, FR4;

	// Generate circular test data
	Eigen::Vector2d C1(0.6, -0.8), C2(21312.0, -934511.0), C3(0.0, 0.0), C4(-5.6, 6123.0);
	double R1 = 1.0, R2 = 4.2, R3 = 0.0, R4 = 17.0;
	ConicFit::Points2D P1, P2, P3, P4;
	ConicFit::genCircleData(P1, 130, C1, R1);
	ConicFit::genCircleData(P2, 50, C2, R2);
	ConicFit::genCircleData(P3, 90, C3, R3);
	ConicFit::genCircleData(P4, 2000, C4, R4);
	double Z1 = 0.3, Z2 = 312.0, Z3 = 0.0, Z4 = -93.0;
	Eigen::Vector3d CC1(C1.x(), C1.y(), Z1), CC2(C2.x(), C2.y(), Z2), CC3(C3.x(), C3.y(), Z3), CC4(C4.x(), C4.y(), Z4);
	ConicFit::Points3D PP1, PP2, PP3, PP4;
	gen3DFrom2D(PP1, P1, Z1);
	gen3DFrom2D(PP2, P2, Z2);
	gen3DFrom2D(PP3, P3, Z3);
	gen3DFrom2D(PP4, P4, Z4);

	// Sanity check the fitting errors of the generated data
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P1, C1, R1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P2, C2, R2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P3, C3, R3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P4, C4, R4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP1, CC1, R1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP2, CC2, R2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP3, CC3, R3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP4, CC4, R4), TOL_HIGH);

	// Perform circle fitting 2D
	FC1.setZero(); FC2.setZero(); FC3.setZero(); FC4.setZero();
	FR1 = FR2 = FR3 = FR4 = 0.0;
	ConicFit::fitCircle(P1, FC1, FR1);
	ConicFit::fitCircle(P2, FC2, FR2);
	ConicFit::fitCircle(P3, FC3, FR3);
	ConicFit::fitCircle(P4, FC4, FR4);

	// Check circle fitting 2D
	EXPECT_NEAR(C1.x(), FC1.x(), TOL_HIGH);
	EXPECT_NEAR(C1.y(), FC1.y(), TOL_HIGH);
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(C2.x(), FC2.x(), TOL_HIGH);
	EXPECT_NEAR(C2.y(), FC2.y(), TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW); // ULP size of C2
	EXPECT_NEAR(C3.x(), FC3.x(), TOL_HIGH);
	EXPECT_NEAR(C3.y(), FC3.y(), TOL_HIGH);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(C4.x(), FC4.x(), TOL_HIGH);
	EXPECT_NEAR(C4.y(), FC4.y(), TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_HIGH);

	// Check fitting errors 2D
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P1, FC1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P2, FC2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P3, FC3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P4, FC4, FR4), TOL_HIGH);

	// Perform circle fitting 3D
	FCC1.setZero(); FCC2.setZero(); FCC3.setZero(); FCC4.setZero();
	FR1 = FR2 = FR3 = FR4 = 0.0;
	ConicFit::fitCircle(PP1, FCC1, FR1);
	ConicFit::fitCircle(PP2, FCC2, FR2);
	ConicFit::fitCircle(PP3, FCC3, FR3);
	ConicFit::fitCircle(PP4, FCC4, FR4);

	// Check circle fitting 3D
	EXPECT_NEAR(CC1.x(), FCC1.x(), TOL_HIGH);
	EXPECT_NEAR(CC1.y(), FCC1.y(), TOL_HIGH);
	EXPECT_NEAR(CC1.z(), FCC1.z(), TOL_HIGH);
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(CC2.x(), FCC2.x(), TOL_HIGH);
	EXPECT_NEAR(CC2.y(), FCC2.y(), TOL_HIGH);
	EXPECT_NEAR(CC2.z(), FCC2.z(), TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW); // ULP size of CC2
	EXPECT_NEAR(CC3.x(), FCC3.x(), TOL_HIGH);
	EXPECT_NEAR(CC3.y(), FCC3.y(), TOL_HIGH);
	EXPECT_NEAR(CC3.z(), FCC3.z(), TOL_HIGH);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(CC4.x(), FCC4.x(), TOL_HIGH);
	EXPECT_NEAR(CC4.y(), FCC4.y(), TOL_HIGH);
	EXPECT_NEAR(CC4.z(), FCC4.z(), TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_HIGH);

	// Check fitting errors 3D
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP1, FCC1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP2, FCC2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP3, FCC3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP4, FCC4, FR4), TOL_HIGH);

	// Perform circle fitting 2D with a known centre
	FR1 = FR2 = FR3 = FR4 = 0.0;
	ConicFit::fitCircleCentred(P1, C1, FR1);
	ConicFit::fitCircleCentred(P2, C2, FR2);
	ConicFit::fitCircleCentred(P3, C3, FR3);
	ConicFit::fitCircleCentred(P4, C4, FR4);

	// Check circle fitting 2D with a known centre
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW); // ULP size of C2
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_MED); // ULP size of C4

	// Check fitting errors 2D with a known centre
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P1, C1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P2, C2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P3, C3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(P4, C4, FR4), TOL_MED);

	// Perform circle fitting 3D with a known centre
	FR1 = FR2 = FR3 = FR4 = 0.0;
	ConicFit::fitCircleCentred(PP1, CC1, FR1);
	ConicFit::fitCircleCentred(PP2, CC2, FR2);
	ConicFit::fitCircleCentred(PP3, CC3, FR3);
	ConicFit::fitCircleCentred(PP4, CC4, FR4);

	// Check circle fitting 3D with a known centre
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW); // ULP size of C2
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_MED); // ULP size of C4

	// Check fitting errors 3D
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP1, CC1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP2, CC2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP3, CC3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleError(PP4, CC4, FR4), TOL_MED);
}

// Test circle fitting
TEST(ConicFitTest, testFitCircleWeighted)
{
	// Declare variables
	Eigen::Vector2d FC1, FC2, FC3, FC4, FC5;
	double FR1, FR2, FR3, FR4, FR5;

	// Generate circular test data
	Eigen::Vector2d C1(0.6, -0.8), C2(21312.0, -934511.0), C3(0.0, 0.0), C4(-5.6, 6123.0), C5(1.7, 0.5);
	double R1 = 1.0, R2 = 4.2, R3 = 0.0, R4 = 17.0, R5 = 4.0;
	ConicFit::Points2D P1, P2, P3, P4, P5;
	ConicFit::genCircleData(P1, 130, C1, R1);
	ConicFit::genCircleData(P2, 50, C2, R2);
	ConicFit::genCircleData(P3, 90, C3, R3);
	ConicFit::genCircleData(P4, 2000, C4, R4);
	ConicFit::genCircleData(P5, 110, C5, R5);
	ConicFit::Weights W1, W2, W3, W4, W5;
	genWeights(W1, P1.size(), 0.0, 1.0);
	genWeights(W2, P2.size(), 0.5, 3.0);
	genWeights(W3, P3.size(), -1.0, 1.0);
	genWeights(W4, P4.size(), 100.0, 150.0);
	genWeights(W5, P5.size(), 0.0, 1.0);
	for(size_t i = 0; i < 10; i++)
	{
		P5.push_back(Eigen::Vector2d(100.0, -1000.0));
		P5.push_back(Eigen::Vector2d(-8293.2, 12949.3));
		P5.push_back(Eigen::Vector2d(72.2, 96.3));
		W5.push_back(0.0);
		W5.push_back(0.0);
		W5.push_back(0.0);
	}
	ConicFit::WeightedPoints2D WP1, WP2, WP3, WP4, WP5;
	mergeWeights(WP1, P1, W1);
	mergeWeights(WP2, P2, W2);
	mergeWeights(WP3, P3, W3);
	mergeWeights(WP4, P4, W4);
	mergeWeights(WP5, P5, W5);

	// Sanity check the fitting errors of the generated data
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP1, C1, R1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP2, C2, R2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP3, C3, R3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP4, C4, R4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP5, C5, R5), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P1, W1, C1, R1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P2, W2, C2, R2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P3, W3, C3, R3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P4, W4, C4, R4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P5, W5, C5, R5), TOL_HIGH);

	// Perform weighted circle fitting 2D (WP form)
	FC1.setZero(); FC2.setZero(); FC3.setZero(); FC4.setZero(); FC5.setZero();
	FR1 = FR2 = FR3 = FR4 = FR5 = 0.0;
	ConicFit::fitCircleWeighted(WP1, FC1, FR1);
	ConicFit::fitCircleWeighted(WP2, FC2, FR2);
	ConicFit::fitCircleWeighted(WP3, FC3, FR3);
	ConicFit::fitCircleWeighted(WP4, FC4, FR4);
	ConicFit::fitCircleWeighted(WP5, FC5, FR5);

	// Check weighted circle fitting 2D (WP form)
	EXPECT_NEAR(C1.x(), FC1.x(), TOL_HIGH);
	EXPECT_NEAR(C1.y(), FC1.y(), TOL_HIGH);
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(C2.x(), FC2.x(), TOL_HIGH);
	EXPECT_NEAR(C2.y(), FC2.y(), TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW);
	EXPECT_NEAR(C3.x(), FC3.x(), TOL_HIGH);
	EXPECT_NEAR(C3.y(), FC3.y(), TOL_HIGH);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(C4.x(), FC4.x(), TOL_HIGH);
	EXPECT_NEAR(C4.y(), FC4.y(), TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_HIGH);
	EXPECT_NEAR(C5.x(), FC5.x(), TOL_LOW);
	EXPECT_NEAR(C5.y(), FC5.y(), TOL_LOW);
	EXPECT_NEAR(R5, FR5, TOL_LOW);

	// Check fitting errors 2D (WP form)
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP1, FC1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP2, FC2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP3, FC3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP4, FC4, FR4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP5, FC5, FR5), TOL_LOW);

	// Perform weighted circle fitting 2D (P+W form)
	FC1.setZero(); FC2.setZero(); FC3.setZero(); FC4.setZero(); FC5.setZero();
	FR1 = FR2 = FR3 = FR4 = FR5 = 0.0;
	ConicFit::fitCircleWeighted(P1, W1, FC1, FR1);
	ConicFit::fitCircleWeighted(P2, W2, FC2, FR2);
	ConicFit::fitCircleWeighted(P3, W3, FC3, FR3);
	ConicFit::fitCircleWeighted(P4, W4, FC4, FR4);
	ConicFit::fitCircleWeighted(P5, W5, FC5, FR5);

	// Check weighted circle fitting 2D (P+W form)
	EXPECT_NEAR(C1.x(), FC1.x(), TOL_HIGH);
	EXPECT_NEAR(C1.y(), FC1.y(), TOL_HIGH);
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(C2.x(), FC2.x(), TOL_HIGH);
	EXPECT_NEAR(C2.y(), FC2.y(), TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW);
	EXPECT_NEAR(C3.x(), FC3.x(), TOL_HIGH);
	EXPECT_NEAR(C3.y(), FC3.y(), TOL_HIGH);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(C4.x(), FC4.x(), TOL_HIGH);
	EXPECT_NEAR(C4.y(), FC4.y(), TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_HIGH);
	EXPECT_NEAR(C5.x(), FC5.x(), TOL_LOW);
	EXPECT_NEAR(C5.y(), FC5.y(), TOL_LOW);
	EXPECT_NEAR(R5, FR5, TOL_LOW);

	// Check fitting errors 2D (P+W form)
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P1, W1, FC1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P2, W2, FC2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P3, W3, FC3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P4, W4, FC4, FR4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P5, W5, FC5, FR5), TOL_LOW);

	// Perform weighted circle fitting 2D with a known centre (WP form)
	FR1 = FR2 = FR3 = FR4 = FR5 = 0.0;
	ConicFit::fitCircleCentredWeighted(WP1, C1, FR1);
	ConicFit::fitCircleCentredWeighted(WP2, C2, FR2);
	ConicFit::fitCircleCentredWeighted(WP3, C3, FR3);
	ConicFit::fitCircleCentredWeighted(WP4, C4, FR4);
	ConicFit::fitCircleCentredWeighted(WP5, C5, FR5);

	// Check weighted circle fitting 2D with a known centre (WP form)
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_HIGH);
	EXPECT_NEAR(R5, FR5, TOL_HIGH);

	// Check fitting errors 2D with a known centre (WP form)
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP1, C1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP2, C2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP3, C3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP4, C4, FR4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(WP5, C5, FR5), TOL_HIGH);

	// Perform weighted circle fitting 2D with a known centre (P+W form)
	FR1 = FR2 = FR3 = FR4 = FR5 = 0.0;
	ConicFit::fitCircleCentredWeighted(P1, W1, C1, FR1);
	ConicFit::fitCircleCentredWeighted(P2, W2, C2, FR2);
	ConicFit::fitCircleCentredWeighted(P3, W3, C3, FR3);
	ConicFit::fitCircleCentredWeighted(P4, W4, C4, FR4);
	ConicFit::fitCircleCentredWeighted(P5, W5, C5, FR5);

	// Check weighted circle fitting 2D with a known centre (P+W form)
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_HIGH);
	EXPECT_NEAR(R5, FR5, TOL_HIGH);

	// Check fitting errors 2D with a known centre (P+W form)
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P1, W1, C1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P2, W2, C2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P3, W3, C3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P4, W4, C4, FR4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitCircleErrorWeighted(P5, W5, C5, FR5), TOL_HIGH);
}

// Test ellipse fitting
TEST(ConicFitTest, testFitEllipse)
{
	// Declare variables
	Eigen::Vector2d FC1, FC2, FC3, FC4;
	Eigen::Vector3d FCC1, FCC2, FCC3, FCC4;
	Eigen::Matrix2d FA1, FA2, FA3, FA4;

	// Generate elliptical test data
	Eigen::Vector2d C1(0.6, -0.8), C2(21312.0, -934511.0), C3(0.0, 0.0), C4(-5.6, 6123.0);
	Eigen::Vector2d R1(1.0, 1.0), R2(4.2, 2.5), R3(0.005, 0.4), R4(17.0, 31.2);
	double Ang1 = 0.91, Ang2 = 2.12, Ang3 = 1.41, Ang4 = 5.23;
	ConicFit::Points2D P1, P2, P3, P4;
	ConicFit::genEllipseData(P1, 130, C1, R1, Ang1);
	ConicFit::genEllipseData(P2, 50, C2, R2, Ang2);
	ConicFit::genEllipseData(P3, 90, C3, R3, Ang3);
	ConicFit::genEllipseData(P4, 2000, C4, R4, Ang4);
	double Z1 = 0.3, Z2 = 312.0, Z3 = 0.0, Z4 = -93.0;
	Eigen::Vector3d CC1(C1.x(), C1.y(), Z1), CC2(C2.x(), C2.y(), Z2), CC3(C3.x(), C3.y(), Z3), CC4(C4.x(), C4.y(), Z4);
	ConicFit::Points3D PP1, PP2, PP3, PP4;
	gen3DFrom2D(PP1, P1, Z1);
	gen3DFrom2D(PP2, P2, Z2);
	gen3DFrom2D(PP3, P3, Z3);
	gen3DFrom2D(PP4, P4, Z4);

	// Perform ellipse fitting 2D
	FC1.setZero(); FC2.setZero(); FC3.setZero(); FC4.setZero();
	FA1.setIdentity(); FA2.setIdentity(); FA3.setIdentity(); FA4.setIdentity();
	ConicFit::fitEllipse(P1, FC1, FA1);
	ConicFit::fitEllipse(P2, FC2, FA2);
	ConicFit::fitEllipse(P3, FC3, FA3);
	ConicFit::fitEllipse(P4, FC4, FA4);

	// Check ellipse fitting 2D
	EXPECT_NEAR(0.0, (FC1 - C1).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FC2 - C2).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FC3 - C3).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FC4 - C4).norm(), TOL_MED);
	for(size_t i = 0; i < P1.size(); i++)
	{
		Eigen::Vector2d v = P1[i] - FC1;
		double err = v.transpose() * FA1 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_HIGH);
	}
	for(size_t i = 0; i < P2.size(); i++)
	{
		Eigen::Vector2d v = P2[i] - FC2;
		double err = v.transpose() * FA2 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < P3.size(); i++)
	{
		Eigen::Vector2d v = P3[i] - FC3;
		double err = v.transpose() * FA3 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < P4.size(); i++)
	{
		Eigen::Vector2d v = P4[i] - FC4;
		double err = v.transpose() * FA4 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}

	// Check fitting errors 2D
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P1, C1, FA1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P2, C2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P3, C3, FA3), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P4, C4, FA4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P1, FC1, FA1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P2, FC2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P3, FC3, FA3), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P4, FC4, FA4), TOL_HIGH);

	// Perform ellipse fitting 2D with a known centre
	FA1.setIdentity(); FA2.setIdentity(); FA3.setIdentity(); FA4.setIdentity();
	ConicFit::fitEllipseCentred(P1, C1, FA1);
	ConicFit::fitEllipseCentred(P2, C2, FA2);
	ConicFit::fitEllipseCentred(P3, C3, FA3);
	ConicFit::fitEllipseCentred(P4, C4, FA4);

	// Check ellipse fitting 2D with a known centre
	for(size_t i = 0; i < P1.size(); i++)
	{
		Eigen::Vector2d v = P1[i] - C1;
		double err = v.transpose() * FA1 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_HIGH);
	}
	for(size_t i = 0; i < P2.size(); i++)
	{
		Eigen::Vector2d v = P2[i] - C2;
		double err = v.transpose() * FA2 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < P3.size(); i++)
	{
		Eigen::Vector2d v = P3[i] - C3;
		double err = v.transpose() * FA3 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < P4.size(); i++)
	{
		Eigen::Vector2d v = P4[i] - C4;
		double err = v.transpose() * FA4 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}

	// Check fitting errors 2D with a known centre
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P1, C1, FA1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P2, C2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P3, C3, FA3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(P4, C4, FA4), TOL_HIGH);

	// Perform ellipse fitting 3D
	FCC1.setZero(); FCC2.setZero(); FCC3.setZero(); FCC4.setZero();
	FA1.setIdentity(); FA2.setIdentity(); FA3.setIdentity(); FA4.setIdentity();
	ConicFit::fitEllipse(PP1, FCC1, FA1);
	ConicFit::fitEllipse(PP2, FCC2, FA2);
	ConicFit::fitEllipse(PP3, FCC3, FA3);
	ConicFit::fitEllipse(PP4, FCC4, FA4);

	// Check ellipse fitting 3D
	EXPECT_NEAR(0.0, (FCC1 - CC1).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FCC2 - CC2).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FCC3 - CC3).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FCC4 - CC4).norm(), TOL_MED);
	for(size_t i = 0; i < PP1.size(); i++)
	{
		Eigen::Vector2d v = (PP1[i] - FCC1).head<2>();
		double err = v.transpose() * FA1 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_HIGH);
	}
	for(size_t i = 0; i < PP2.size(); i++)
	{
		Eigen::Vector2d v = (PP2[i] - FCC2).head<2>();
		double err = v.transpose() * FA2 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < PP3.size(); i++)
	{
		Eigen::Vector2d v = (PP3[i] - FCC3).head<2>();
		double err = v.transpose() * FA3 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < PP4.size(); i++)
	{
		Eigen::Vector2d v = (PP4[i] - FCC4).head<2>();
		double err = v.transpose() * FA4 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}

	// Check fitting errors 3D
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP1, CC1, FA1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP2, CC2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP3, CC3, FA3), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP4, CC4, FA4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP1, FCC1, FA1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP2, FCC2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP3, FCC3, FA3), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP4, FCC4, FA4), TOL_HIGH);

	// Perform ellipse fitting 3D with a known centre
	FA1.setIdentity(); FA2.setIdentity(); FA3.setIdentity(); FA4.setIdentity();
	ConicFit::fitEllipseCentred(PP1, CC1, FA1);
	ConicFit::fitEllipseCentred(PP2, CC2, FA2);
	ConicFit::fitEllipseCentred(PP3, CC3, FA3);
	ConicFit::fitEllipseCentred(PP4, CC4, FA4);

	// Check ellipse fitting 3D with a known centre
	for(size_t i = 0; i < PP1.size(); i++)
	{
		Eigen::Vector2d v = (PP1[i] - CC1).head<2>();
		double err = v.transpose() * FA1 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_HIGH);
	}
	for(size_t i = 0; i < PP2.size(); i++)
	{
		Eigen::Vector2d v = (PP2[i] - CC2).head<2>();
		double err = v.transpose() * FA2 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < PP3.size(); i++)
	{
		Eigen::Vector2d v = (PP3[i] - CC3).head<2>();
		double err = v.transpose() * FA3 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < PP4.size(); i++)
	{
		Eigen::Vector2d v = (PP4[i] - CC4).head<2>();
		double err = v.transpose() * FA4 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}

	// Check fitting errors 3D with a known centre
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP1, CC1, FA1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP2, CC2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP3, CC3, FA3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseErrorCoeff(PP4, CC4, FA4), TOL_HIGH);

	// Convert the fitted coefficient matrices to rotation matrices and radii
	Eigen::Matrix2d FQ1, FQ2, FQ3, FQ4;
	Eigen::Vector2d FR1, FR2, FR3, FR4;
	double FAng1, FAng2, FAng3, FAng4;
	ASSERT_TRUE(ConicFit::ellipseMatrixToAxes(FA1, FQ1, FR1, FAng1));
	ASSERT_TRUE(ConicFit::ellipseMatrixToAxes(FA2, FQ2, FR2, FAng2));
	ASSERT_TRUE(ConicFit::ellipseMatrixToAxes(FA3, FQ3, FR3, FAng3));
	ASSERT_TRUE(ConicFit::ellipseMatrixToAxes(FA4, FQ4, FR4, FAng4));

	// Check the rotation matrices and radii
	double cFAng1 = cos(FAng1), sFAng1 = sin(FAng1);
	double cFAng2 = cos(FAng2), sFAng2 = sin(FAng2);
	double cFAng3 = cos(FAng3), sFAng3 = sin(FAng3);
	double cFAng4 = cos(FAng4), sFAng4 = sin(FAng4);
	Eigen::Vector2d lambda1(1.0/(FR1.x()*FR1.x()), 1.0/(FR1.y()*FR1.y()));
	Eigen::Vector2d lambda2(1.0/(FR2.x()*FR2.x()), 1.0/(FR2.y()*FR2.y()));
	Eigen::Vector2d lambda3(1.0/(FR3.x()*FR3.x()), 1.0/(FR3.y()*FR3.y()));
	Eigen::Vector2d lambda4(1.0/(FR4.x()*FR4.x()), 1.0/(FR4.y()*FR4.y()));
	EXPECT_NEAR(0.0, (FQ1 * lambda1.asDiagonal() * FQ1.transpose() - FA1).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ2 * lambda2.asDiagonal() * FQ2.transpose() - FA2).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ3 * lambda3.asDiagonal() * FQ3.transpose() - FA3).norm(), TOL_LOW);
	EXPECT_NEAR(0.0, (FQ4 * lambda4.asDiagonal() * FQ4.transpose() - FA4).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ1 - (Eigen::Matrix2d() << cFAng1, -sFAng1, sFAng1, cFAng1).finished()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ2 - (Eigen::Matrix2d() << cFAng2, -sFAng2, sFAng2, cFAng2).finished()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ3 - (Eigen::Matrix2d() << cFAng3, -sFAng3, sFAng3, cFAng3).finished()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ4 - (Eigen::Matrix2d() << cFAng4, -sFAng4, sFAng4, cFAng4).finished()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ1.inverse() - FQ1.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ2.inverse() - FQ2.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ3.inverse() - FQ3.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ4.inverse() - FQ4.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ1.determinant(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ2.determinant(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ3.determinant(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ4.determinant(), TOL_HIGH);
	EXPECT_GT(FR1.x(), 0.0);
	EXPECT_GT(FR1.y(), 0.0);
	EXPECT_GT(FR2.x(), 0.0);
	EXPECT_GT(FR2.y(), 0.0);
	EXPECT_GT(FR3.x(), 0.0);
	EXPECT_GT(FR3.y(), 0.0);
	EXPECT_GT(FR4.x(), 0.0);
	EXPECT_GT(FR4.y(), 0.0);

	// Convert the rotation matrix and radii to a normalisation matrix
	Eigen::Matrix2d FW1, FW2, FW3, FW4;
	ASSERT_TRUE(ConicFit::ellipseAxesToTransform(FQ1, FR1, FW1));
	ASSERT_TRUE(ConicFit::ellipseAxesToTransform(FQ2, FR2, FW2));
	ASSERT_TRUE(ConicFit::ellipseAxesToTransform(FQ3, FR3, FW3));
	ASSERT_TRUE(ConicFit::ellipseAxesToTransform(FQ4, FR4, FW4));

	// Check the normalisation matrices
	EXPECT_NEAR(0.0, (FW1*FW1 - FA1).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW2*FW2 - FA2).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW3*FW3 - FA3).norm(), TOL_LOW);
	EXPECT_NEAR(0.0, (FW4*FW4 - FA4).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW1 - FW1.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW2 - FW2.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW3 - FW3.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW4 - FW4.transpose()).norm(), TOL_HIGH);
	for(size_t i = 1; i <= 2; i++)
	{
		EXPECT_GT(FW1.topLeftCorner(i,i).determinant(), 0.0); // A matrix is positive definite iff all leading principal minors are positive
		EXPECT_GT(FW2.topLeftCorner(i,i).determinant(), 0.0);
		EXPECT_GT(FW3.topLeftCorner(i,i).determinant(), 0.0);
		EXPECT_GT(FW4.topLeftCorner(i,i).determinant(), 0.0);
	}

	// Check fitting errors
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(P1, FC1, FW1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(P2, FC2, FW2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(P3, FC3, FW3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(P4, FC4, FW4), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(PP1, FCC1, FW1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(PP2, FCC2, FW2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(PP3, FCC3, FW3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipseError(PP4, FCC4, FW4), TOL_HIGH);
}

// Test sphere fitting
TEST(ConicFitTest, testFitSphere)
{
	// Declare variables
	Eigen::Vector3d FC1, FC2, FC3, FC4;
	double FR1, FR2, FR3, FR4;

	// Generate spherical test data
	Eigen::Vector3d C1(0.6, -0.8, 0.4), C2(21312.0, -934511.0, -13123.0), C3(0.0, 0.0, 0.0), C4(-5.6, 6123.0, 9.3);
	double R1 = 1.0, R2 = 4.2, R3 = 0.0, R4 = 17.0;
	ConicFit::Points3D PP1, PP2, PP3, PP4;
	ConicFit::genSphereData(PP1, 13, C1, R1);
	ConicFit::genSphereData(PP2, 15, C2, R2);
	ConicFit::genSphereData(PP3, 10, C3, R3);
	ConicFit::genSphereData(PP4, 45, C4, R4);

	// Sanity check the fitting errors of the generated data
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP1, C1, R1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP2, C2, R2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP3, C3, R3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP4, C4, R4), TOL_HIGH);

	// Perform sphere fitting
	FC1.setZero(); FC2.setZero(); FC3.setZero(); FC4.setZero();
	FR1 = FR2 = FR3 = FR4 = 0.0;
	ConicFit::fitSphere(PP1, FC1, FR1);
	ConicFit::fitSphere(PP2, FC2, FR2);
	ConicFit::fitSphere(PP3, FC3, FR3);
	ConicFit::fitSphere(PP4, FC4, FR4);

	// Check sphere fitting
	EXPECT_NEAR(C1.x(), FC1.x(), TOL_HIGH);
	EXPECT_NEAR(C1.y(), FC1.y(), TOL_HIGH);
	EXPECT_NEAR(C1.z(), FC1.z(), TOL_HIGH);
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(C2.x(), FC2.x(), TOL_HIGH);
	EXPECT_NEAR(C2.y(), FC2.y(), TOL_HIGH);
	EXPECT_NEAR(C2.z(), FC2.z(), TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW); // ULP size of C2
	EXPECT_NEAR(C3.x(), FC3.x(), TOL_HIGH);
	EXPECT_NEAR(C3.y(), FC3.y(), TOL_HIGH);
	EXPECT_NEAR(C3.z(), FC3.z(), TOL_HIGH);
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(C4.x(), FC4.x(), TOL_MED);
	EXPECT_NEAR(C4.y(), FC4.y(), TOL_MED);
	EXPECT_NEAR(C4.z(), FC4.z(), TOL_MED);
	EXPECT_NEAR(R4, FR4, TOL_MED); // ULP size of C4

	// Check fitting errors
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP1, FC1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP2, FC2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP3, FC3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP4, FC4, FR4), TOL_HIGH);

	// Perform sphere fitting with a known centre
	FR1 = FR2 = FR3 = FR4 = 0.0;
	ConicFit::fitSphereCentred(PP1, C1, FR1);
	ConicFit::fitSphereCentred(PP2, C2, FR2);
	ConicFit::fitSphereCentred(PP3, C3, FR3);
	ConicFit::fitSphereCentred(PP4, C4, FR4);

	// Check sphere fitting with a known centre
	EXPECT_NEAR(R1, FR1, TOL_HIGH);
	EXPECT_NEAR(R2, FR2, TOL_LOW); // ULP size of C2
	EXPECT_NEAR(R3, FR3, TOL_HIGH);
	EXPECT_NEAR(R4, FR4, TOL_MED); // ULP size of C4

	// Check fitting errors with a known centre
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP1, C1, FR1), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP2, C2, FR2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP3, C3, FR3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitSphereError(PP4, C4, FR4), TOL_MED);
}

// Test ellipsoid fitting
TEST(ConicFitTest, testFitEllipsoid)
{
	// Declare variables
	Eigen::Vector3d FC1, FC2, FC3, FC4;
	Eigen::Matrix3d FA1, FA2, FA3, FA4;

	// Generate ellipsoidal test data
	Eigen::Vector3d C1(0.6, -0.8, 0.4), C2(21312.0, -934511.0, -13123.0), C3(0.0, 0.0, 0.0), C4(-5.6, 6123.0, 9.3);
	Eigen::Vector3d R1(0.7, 0.5, 1.3), R2(6.0, 3.0, 4.5), R3(16.0, 3.0, 1.0), R4(3.0, 17.0, 8.0);
	Eigen::Matrix3d Q1(Eigen::AngleAxisd(0.83, Eigen::Vector3d(1.0, 1.0, 1.0).normalized()));
	Eigen::Matrix3d Q2(Eigen::AngleAxisd(2.81, Eigen::Vector3d(-0.4, 0.7, 1.0).normalized()));
	Eigen::Matrix3d Q3(Eigen::AngleAxisd(1.27, Eigen::Vector3d(0.0, 1.0, 2.0).normalized()));
	Eigen::Matrix3d Q4(Eigen::AngleAxisd(4.93, Eigen::Vector3d(2.0, -1.0, 0.4).normalized()));
	ConicFit::Points3D PP1, PP2, PP3, PP4;
	ConicFit::genEllipsoidData(PP1, 13, C1, R1, Q1);
	ConicFit::genEllipsoidData(PP2, 15, C2, R2, Q2);
	ConicFit::genEllipsoidData(PP3, 10, C3, R3, Q3);
	ConicFit::genEllipsoidData(PP4, 45, C4, R4, Q4);

	// Perform ellipsoid fitting
	FC1.setZero(); FC2.setZero(); FC3.setZero(); FC4.setZero();
	FA1.setIdentity(); FA2.setIdentity(); FA3.setIdentity(); FA4.setIdentity();
	ConicFit::fitEllipsoid(PP1, FC1, FA1);
	ConicFit::fitEllipsoid(PP2, FC2, FA2);
	ConicFit::fitEllipsoid(PP3, FC3, FA3);
	ConicFit::fitEllipsoid(PP4, FC4, FA4);

	// Check ellipsoid fitting
	EXPECT_NEAR(0.0, (FC1 - C1).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FC2 - C2).norm(), TOL_LOW);
	EXPECT_NEAR(0.0, (FC3 - C3).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FC4 - C4).norm(), TOL_HIGH);
	for(size_t i = 0; i < PP1.size(); i++)
	{
		Eigen::Vector3d v = PP1[i] - FC1;
		double err = v.transpose() * FA1 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_HIGH);
	}
	for(size_t i = 0; i < PP2.size(); i++)
	{
		Eigen::Vector3d v = PP2[i] - FC2;
		double err = v.transpose() * FA2 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < PP3.size(); i++)
	{
		Eigen::Vector3d v = PP3[i] - FC3;
		double err = v.transpose() * FA3 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}
	for(size_t i = 0; i < PP4.size(); i++)
	{
		Eigen::Vector3d v = PP4[i] - FC4;
		double err = v.transpose() * FA4 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}

	// Check fitting errors
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP1, C1, FA1), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP2, C2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP3, C3, FA3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP4, C4, FA4), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP1, FC1, FA1), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP2, FC2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP3, FC3, FA3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP4, FC4, FA4), TOL_MED);

	// Perform ellipsoid fitting with a known centre
	FA1.setIdentity(); FA2.setIdentity(); FA3.setIdentity(); FA4.setIdentity();
	ConicFit::fitEllipsoidCentred(PP1, C1, FA1);
	ConicFit::fitEllipsoidCentred(PP2, C2, FA2);
	ConicFit::fitEllipsoidCentred(PP3, C3, FA3);
	ConicFit::fitEllipsoidCentred(PP4, C4, FA4);

	// Check ellipsoid fitting with a known centre
	for(size_t i = 0; i < PP1.size(); i++)
	{
		Eigen::Vector3d v = PP1[i] - C1;
		double err = v.transpose() * FA1 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_HIGH);
	}
	for(size_t i = 0; i < PP2.size(); i++)
	{
		Eigen::Vector3d v = PP2[i] - C2;
		double err = v.transpose() * FA2 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_LOW);
	}
	for(size_t i = 0; i < PP3.size(); i++)
	{
		Eigen::Vector3d v = PP3[i] - C3;
		double err = v.transpose() * FA3 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}
	for(size_t i = 0; i < PP4.size(); i++)
	{
		Eigen::Vector3d v = PP4[i] - C4;
		double err = v.transpose() * FA4 * v - 1.0;
		EXPECT_NEAR(0.0, err, TOL_MED);
	}

	// Check fitting errors with a known centre
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP1, C1, FA1), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP2, C2, FA2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP3, C3, FA3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidErrorCoeff(PP4, C4, FA4), TOL_MED);

	// Convert the fitted coefficient matrices to rotation matrices and radii
	Eigen::Matrix3d FQ1, FQ2, FQ3, FQ4;
	Eigen::Vector3d FR1, FR2, FR3, FR4;
	ASSERT_TRUE(ConicFit::ellipsoidMatrixToAxes(FA1, FQ1, FR1));
	ASSERT_TRUE(ConicFit::ellipsoidMatrixToAxes(FA2, FQ2, FR2));
	ASSERT_TRUE(ConicFit::ellipsoidMatrixToAxes(FA3, FQ3, FR3));
	ASSERT_TRUE(ConicFit::ellipsoidMatrixToAxes(FA4, FQ4, FR4));

	// Check the rotation matrices and radii
	Eigen::Vector3d lambda1(1.0/(FR1.x()*FR1.x()), 1.0/(FR1.y()*FR1.y()), 1.0/(FR1.z()*FR1.z()));
	Eigen::Vector3d lambda2(1.0/(FR2.x()*FR2.x()), 1.0/(FR2.y()*FR2.y()), 1.0/(FR2.z()*FR2.z()));
	Eigen::Vector3d lambda3(1.0/(FR3.x()*FR3.x()), 1.0/(FR3.y()*FR3.y()), 1.0/(FR3.z()*FR3.z()));
	Eigen::Vector3d lambda4(1.0/(FR4.x()*FR4.x()), 1.0/(FR4.y()*FR4.y()), 1.0/(FR4.z()*FR4.z()));
	EXPECT_NEAR(0.0, (FQ1 * lambda1.asDiagonal() * FQ1.transpose() - FA1).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FQ2 * lambda2.asDiagonal() * FQ2.transpose() - FA2).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FQ3 * lambda3.asDiagonal() * FQ3.transpose() - FA3).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ4 * lambda4.asDiagonal() * FQ4.transpose() - FA4).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ1.inverse() - FQ1.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ2.inverse() - FQ2.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ3.inverse() - FQ3.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FQ4.inverse() - FQ4.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ1.determinant(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ2.determinant(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ3.determinant(), TOL_HIGH);
	EXPECT_NEAR(1.0, FQ4.determinant(), TOL_HIGH);
	EXPECT_GT(FR1.x(), 0.0);
	EXPECT_GT(FR1.y(), 0.0);
	EXPECT_GT(FR1.z(), 0.0);
	EXPECT_GT(FR2.x(), 0.0);
	EXPECT_GT(FR2.y(), 0.0);
	EXPECT_GT(FR2.z(), 0.0);
	EXPECT_GT(FR3.x(), 0.0);
	EXPECT_GT(FR3.y(), 0.0);
	EXPECT_GT(FR3.z(), 0.0);
	EXPECT_GT(FR4.x(), 0.0);
	EXPECT_GT(FR4.y(), 0.0);
	EXPECT_GT(FR4.z(), 0.0);

	// Convert the rotation matrix and radii to a normalisation matrix
	Eigen::Matrix3d FW1, FW2, FW3, FW4;
	ASSERT_TRUE(ConicFit::ellipsoidAxesToTransform(FQ1, FR1, FW1));
	ASSERT_TRUE(ConicFit::ellipsoidAxesToTransform(FQ2, FR2, FW2));
	ASSERT_TRUE(ConicFit::ellipsoidAxesToTransform(FQ3, FR3, FW3));
	ASSERT_TRUE(ConicFit::ellipsoidAxesToTransform(FQ4, FR4, FW4));

	// Check the normalisation matrices
	EXPECT_NEAR(0.0, (FW1*FW1 - FA1).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FW2*FW2 - FA2).norm(), TOL_MED);
	EXPECT_NEAR(0.0, (FW3*FW3 - FA3).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW4*FW4 - FA4).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW1 - FW1.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW2 - FW2.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW3 - FW3.transpose()).norm(), TOL_HIGH);
	EXPECT_NEAR(0.0, (FW4 - FW4.transpose()).norm(), TOL_HIGH);
	for(size_t i = 1; i <= 3; i++)
	{
		EXPECT_GT(FW1.topLeftCorner(i,i).determinant(), 0.0); // A matrix is positive definite iff all leading principal minors are positive
		EXPECT_GT(FW2.topLeftCorner(i,i).determinant(), 0.0);
		EXPECT_GT(FW3.topLeftCorner(i,i).determinant(), 0.0);
		EXPECT_GT(FW4.topLeftCorner(i,i).determinant(), 0.0);
	}

	// Check fitting errors
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidError(PP1, C1, FW1), TOL_MED);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidError(PP2, C2, FW2), TOL_LOW);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidError(PP3, C3, FW3), TOL_HIGH);
	EXPECT_NEAR(0.0, ConicFit::fitEllipsoidError(PP4, C4, FW4), TOL_MED);
}

// Test fitting errors
TEST(ConicFitTest, testFitError)
{
	// Sanity check fitting error for circle
	ConicFit::Points2D P1;
	ConicFit::Weights W1;
	ConicFit::WeightedPoints2D WP1;
	Eigen::Vector2d C1(0.6, -0.8);
	double R1 = 0.5, S1 = 1.3;
	ConicFit::genCircleData(P1, 60, C1, R1*S1);
	genWeights(W1, P1.size(), 0.0, 1.0);
	mergeWeights(WP1, P1, W1);
	EXPECT_NEAR(fabs(S1 - 1.0), ConicFit::fitCircleError(P1, C1, R1), TOL_HIGH);
	EXPECT_NEAR(fabs(S1 - 1.0), ConicFit::fitCircleErrorWeighted(WP1, C1, R1), TOL_HIGH);
	EXPECT_NEAR(fabs(S1 - 1.0), ConicFit::fitCircleErrorWeighted(P1, W1, C1, R1), TOL_HIGH);

	// Sanity check fitting error for ellipse
	Eigen::Vector2d C2(-0.6, 0.8);
	Eigen::Vector2d R2(3.0, 1.2);
	Eigen::Matrix2d W2;
	double Ang2 = 2.19, S2 = 1.7;
	ConicFit::Points2D P2;
	ConicFit::genEllipseData(P2, 60, C2, R2*S2, Ang2);
	ConicFit::ellipseAxesToTransform((Eigen::Matrix2d() << cos(Ang2), -sin(Ang2), sin(Ang2), cos(Ang2)).finished(), R2, W2);
	EXPECT_NEAR(fabs(S2 - 1.0), ConicFit::fitEllipseError(P2, C2, W2), TOL_HIGH);

	// Sanity check fitting error for sphere
	ConicFit::Points3D PP1;
	Eigen::Vector3d CC1(0.6, -0.8, 0.4);
	double RR1 = 7.5, SS1 = 0.6;
	ConicFit::genSphereData(PP1, 20, CC1, RR1*SS1);
	EXPECT_NEAR(fabs(SS1 - 1.0), ConicFit::fitSphereError(PP1, CC1, RR1), TOL_HIGH);

	// Sanity check fitting error for ellipsoid
	Eigen::Vector3d CC2(1.4, -4.3, -0.5);
	Eigen::Vector3d RR2(3.3, 1.8, 0.7);
	Eigen::Matrix3d QQ2(Eigen::AngleAxisd(2.39, Eigen::Vector3d(-0.2, 0.8, 0.4).normalized()));
	Eigen::Matrix3d WW2;
	double SS2 = 0.84;
	ConicFit::Points3D PP2;
	ConicFit::genEllipsoidData(PP2, 15, CC2, RR2*SS2, QQ2);
	ConicFit::ellipsoidAxesToTransform(QQ2, RR2, WW2);
	EXPECT_NEAR(fabs(SS2 - 1.0), ConicFit::fitEllipsoidError(PP2, CC2, WW2), TOL_HIGH);
}

// Main function
int main(int argc, char **argv)
{
	// Ensure repeatability of random number generation
	srand48(0xF9EB283A);

	// Run all the tests
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}
// EOF


//
// $Id: InterpolatorTest.cpp 8434 2012-03-29 16:09:22Z chambm $
//
// Original author: Matt Chambers <matt.chambers .@. vanderbilt.edu>
// Copyright 2011 Vanderbilt University
//
// Licensed under the Code Project Open License, Version 1.02 (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.codeproject.com/info/cpol10.aspx
//
// 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 "pwiz/utility/misc/unit.hpp"
#include "pwiz/utility/misc/Std.hpp"
#include "Interpolator.hpp"
#include <boost/assign.hpp>

using namespace pwiz::util;
using namespace freicore;
using namespace boost::assign;

int main()
{
    try
    {
        {
            vector<double> x; x += 1, 2, 3,  4,  6,  7,  8, 9;
            vector<double> y; y += 1, 8, 27, 64, 64, 27, 8, 1;
            Interpolator test(x, y);

            unit_assert_equal(1, test.interpolate(x, y, 1), 1e-6);
            unit_assert_equal(8, test.interpolate(x, y, 2), 1e-6);
            unit_assert_equal(27, test.interpolate(x, y, 3), 1e-6);
            unit_assert_equal(64, test.interpolate(x, y, 4), 1e-6);
            //unit_assert_equal(64, test.interpolate(x, y, 5), 1e-6);
            unit_assert_equal(64, test.interpolate(x, y, 6), 1e-6);
            unit_assert_equal(27, test.interpolate(x, y, 7), 1e-6);
            unit_assert_equal(1, test.interpolate(x, y, 9), 1e-6);

            test.resample(x, y);
            unit_assert_operator_equal(18, x.size() + y.size());
            unit_assert_equal(1, x[0], 1e-6);
            unit_assert_equal(2, x[1], 1e-6);
            unit_assert_equal(8, x[7], 1e-6);
            unit_assert_equal(9, x[8], 1e-6);

            unit_assert_equal(1, y[0], 1e-6);
            unit_assert_equal(8, y[1], 1e-6);
            unit_assert_equal(27, y[2], 1e-6);
            unit_assert_equal(64, y[3], 1e-6);
            //unit_assert_equal(64, y[4], 1e-6);
            unit_assert_equal(64, y[5], 1e-6);
            unit_assert_equal(27, y[6], 1e-6);
            //unit_assert_equal(8, y[7], 1e-6);
            unit_assert_equal(1, y[8], 1e-6);
        }

        {
            vector<double> x; x += 1, 3,  4,  5,  6,  7,  9;
            vector<double> y; y += 1, 27, 64, 64, 64, 27, 1;
            Interpolator test(x, y);

            unit_assert_operator_equal(1, test.interpolate(x, y, 1));
            unit_assert_operator_equal(8, test.interpolate(x, y, 2));
            unit_assert_operator_equal(64, test.interpolate(x, y, 5));
            unit_assert_operator_equal(27, test.interpolate(x, y, 7));
            unit_assert_operator_equal(8, test.interpolate(x, y, 8));
            unit_assert_operator_equal(1, test.interpolate(x, y, 9));

            test.resample(x, y);
            unit_assert_operator_equal(18, x.size() + y.size());
            unit_assert_operator_equal(1, x[0]);
            unit_assert_operator_equal(2, x[1]);
            unit_assert_operator_equal(8, x[7]);
            unit_assert_operator_equal(9, x[8]);

            unit_assert_operator_equal(1, y[0]);
            unit_assert_operator_equal(8, y[1]);
            unit_assert_operator_equal(27, y[2]);
            unit_assert_operator_equal(64, y[3]);
            unit_assert_operator_equal(64, y[4]);
            unit_assert_operator_equal(64, y[5]);
            unit_assert_operator_equal(27, y[6]);
            unit_assert_operator_equal(8, y[7]);
            unit_assert_operator_equal(1, y[8]);
        }
    }
    catch (exception& e)
    {
        cerr << e.what() << endl;
        return 1;
    }

    return 0;
}


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to 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)

#ifdef TEST_STD_HEADERS
#include <complex>
#else
#include <boost/tr1/complex.hpp>
#endif

#include <boost/test/test_tools.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/mpl/if.hpp>
#include <boost/static_assert.hpp>

#include <iostream>
#include <iomanip>

#ifndef VERBOSE
#undef BOOST_MESSAGE
#define BOOST_MESSAGE(x)
#endif

//
// This test verifies that the complex-algorithms that are
// overloaded for scalar types produce the same result as casting
// the argument to a complex type, and calling the complex version
// of the algorithm.  Relative errors must be within 2e in order for
// the tests to pass.
//

template <class T, class U>
void do_check(const T& t, const U& u)
{
   static const T two = 2;
   static const T factor = std::pow(two, 1-std::numeric_limits<T>::digits) * 200;
   BOOST_STATIC_ASSERT((::boost::is_same<T,U>::value));
   BOOST_CHECK_CLOSE(t, u, factor);
}

template <class T, class U>
void do_check(const std::complex<T>& t, const std::complex<U>& u)
{
   BOOST_STATIC_ASSERT((::boost::is_same<T,U>::value));
   do_check(t.real(), u.real());
   do_check(t.imag(), u.imag());
}

template <class T>
void check_val(const T& val)
{
   typedef typename boost::mpl::if_< boost::is_floating_point<T>, T, double>::type real_type;
   typedef std::complex<real_type> complex_type;

   real_type rval = static_cast<real_type>(val);
   complex_type cval = rval;

   if(val)
   {
      std::cout << "    Testing std::arg.\n";
      do_check(std::arg(cval), std::arg(rval));
      do_check(std::arg(cval), std::arg(val));
   }
   std::cout << "    Testing std::norm.\n";
   do_check(std::norm(cval), std::norm(rval));
   do_check(std::norm(cval), std::norm(val));
   std::cout << "    Testing std::conj.\n";
   do_check(std::conj(cval), std::conj(rval));
   do_check(std::conj(cval), std::conj(val));
   std::cout << "    Testing std::polar.\n";
   do_check(std::polar(val), std::polar(rval));
   do_check(std::polar(val, 0), std::polar(rval, 0));
   do_check(std::polar(val, val), std::polar(rval, rval));
   do_check(std::polar(val, rval), std::polar(rval, val));
   std::cout << "    Testing std::real.\n";
   do_check(std::real(cval), std::real(rval));
   do_check(std::real(cval), std::real(val));
   std::cout << "    Testing std::imaj.\n";
   do_check(std::imag(cval), std::imag(rval));
   do_check(std::imag(cval), std::imag(val));
   if(val && !boost::is_floating_point<T>::value)
   {
      //
      // Note that these tests are not run for floating point
      // types as that would only test the std lib vendor's
      // implementation of pow, not our additional overloads.
      // Note that some std lib's do fail these tests, gcc on
      // Darwin is a particularly bad example !
      //
      std::cout << "    Testing std::pow.\n";
      do_check(std::pow(cval, cval), std::pow(cval, val));
      do_check(std::pow(cval, cval), std::pow(cval, rval));
      do_check(std::pow(cval, cval), std::pow(val, cval));
      do_check(std::pow(cval, cval), std::pow(rval, cval));
   }
}

void do_check(double i)
{
   std::cout << "Checking type double with value " << i << std::endl;
   check_val(i);
   std::cout << "Checking type float with value " << i << std::endl;
   check_val(static_cast<float>(i));
   std::cout << "Checking type long double with value " << i << std::endl;
   check_val(static_cast<long double>(i));
}

void do_check(int i)
{
   std::cout << "Checking type char with value " << i << std::endl;
   check_val(static_cast<char>(i));
   std::cout << "Checking type unsigned char with value " << i << std::endl;
   check_val(static_cast<unsigned char>(i));
   std::cout << "Checking type signed char with value " << i << std::endl;
   check_val(static_cast<signed char>(i));
   std::cout << "Checking type short with value " << i << std::endl;
   check_val(static_cast<short>(i));
   std::cout << "Checking type unsigned short with value " << i << std::endl;
   check_val(static_cast<unsigned short>(i));
   std::cout << "Checking type int with value " << i << std::endl;
   check_val(static_cast<int>(i));
   std::cout << "Checking type unsigned int with value " << i << std::endl;
   check_val(static_cast<unsigned int>(i));
   std::cout << "Checking type long with value " << i << std::endl;
   check_val(static_cast<long>(i));
   std::cout << "Checking type unsigned long with value " << i << std::endl;
   check_val(static_cast<unsigned long>(i));
#ifdef BOOST_HAS_LONG_LONG
   std::cout << "Checking type long long with value " << i << std::endl;
   check_val(static_cast<long long>(i));
   std::cout << "Checking type unsigned long long with value " << i << std::endl;
   check_val(static_cast<unsigned long long>(i));
#elif defined(BOOST_HAS_MS_INT64)
   std::cout << "Checking type __int64 with value " << i << std::endl;
   check_val(static_cast<__int64>(i));
   std::cout << "Checking type unsigned __int64 with value " << i << std::endl;
   check_val(static_cast<unsigned __int64>(i));
#endif
   do_check(static_cast<double>(i));
}

int test_main(int, char*[])
{
   do_check(0);
   do_check(0.0);
   do_check(1);
   do_check(1.5);
   do_check(0.5);
   return 0;
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <math.h>

#include <Eigen/Dense>
#include <unordered_map>
#include <vector>
#include <random>

namespace py = pybind11;

// POSTERIOR PVAL FOR T
// Generate posterior pvals selecting a new or existing table
Eigen::VectorXd posterior_t_cpp(Eigen::VectorXd t_j, Eigen::VectorXd n_jt,
                           Eigen::VectorXd k_jt, Eigen::VectorXd m_k, Eigen::VectorXd fk,
                           double gamma, double V, double alpha){

    // Get table specific topic and counts
    Eigen::VectorXd k_idx = k_jt(t_j);
    Eigen::VectorXd n_idx = n_jt(t_j);

    // If t is not new
    Eigen::VectorXd post_t = n_idx.cwiseProduct(fk(k_idx)); // element-wise product (scalar * vector)

    // If t is new
    double post_t_new = m_k.dot(fk) + gamma/V;
    post_t(0) = post_t_new * alpha / (gamma + m_k.sum());

    post_t /= post_t.sum(); // Normalize to generate pvals

    return post_t;

}

// Conditional density of x_ji given k and all data items except x_j
Eigen::VectorXd fk_cpp(int word, Eigen::MatrixXd  n_kv){

    Eigen::VectorXd lik = n_kv.row(word).cwiseQuotient(n_kv.colwise().sum()); // element-wise division(vec / scalar)
    lik[0] = 0; // First should always be zero

    return lik;
}

// POSTERIOR PVAL FOR K
// Compute explicit posterior multinomial-dirichlet posterior distribution
Eigen::VectorXd posterior_k_cpp(int tbl, Eigen::VectorXd k_jt,
                                Eigen::VectorXd n_jt, Eigen::VectorXd topic_idx, Eigen::MatrixXd n_kv,
                                Eigen::VectorXd m_k, const std::unordered_map<int, int> n_jtw,
                                double gamma, double V, double beta){

    // Topic k of table t
    int k = k_jt(tbl);
    double cnt_t = n_jt(tbl);
    double vbeta = V*beta;

    // Remove all counts associated with topic k in table t, from overall topic counts (n_k)
    Eigen::VectorXd n_k = n_kv.colwise().sum();
    n_k(k) -= cnt_t;
    n_k = n_k(topic_idx);

    // Initialized k posterior in log-form for simplicity, this computes f_k^{-X_ji}
    // has Dirichlet-Multinomial form
    double log_post_k_new = std::log(gamma) + std::lgamma(vbeta) - std::lgamma(vbeta + cnt_t); // for new k

    // for old ks, needs to loop since it's a vector
    m_k = m_k(topic_idx);
    Eigen::VectorXd log_post_k = m_k.array().log();
    for (int i = 0; i < log_post_k.size() ; i++){
        log_post_k(i) += std::lgammaf(n_k(i)) - std::lgammaf(n_k(i) + cnt_t);
    };

    // Remove individual word counts associated with topic k
    // add their contributions to k posterior
    for (auto item : n_jtw){

        if (item.second == 0) continue; //if word count is 0 skip

        //For word w, get counts across topics
        Eigen::VectorXd w_cnt_k = n_kv.row(item.first);

        // For specific topic k, remove count from associated table t
        w_cnt_k(k) -= item.second;
        w_cnt_k = w_cnt_k(topic_idx);
        w_cnt_k(0) = 1;

        // Add contributions of individual observations (words) for k new and k old
        log_post_k_new += std::lgammaf(item.second + beta) - std::lgammaf(beta);

        for (int j = 0; j < w_cnt_k.size(); j++){
            log_post_k(j) += std::lgammaf(w_cnt_k(j) + item.second) - std::lgammaf(w_cnt_k(j));

        };
    };

    // p-val for new k
    log_post_k(0) = log_post_k_new;

    // Bring back to non-log realm
    double pk_max = log_post_k.maxCoeff();

    for (int s = 0; s < log_post_k.size(); s++){
        log_post_k(s) -= pk_max;
    };

    log_post_k = log_post_k.array().exp();

    log_post_k /= log_post_k.sum(); // Normalize

    return log_post_k;

}

// Random Multinomial Sampling in Eigen w/ argmax selection
// Given a set of pvals and N, gives back index most likely category
// Ref:
//   C.S. David, The computer generation of multinomial random variates,
//   Comp. Stat. Data Anal. 16 (1993) 205-217
//

int argmax_multinomial_cpp(int N, Eigen::VectorXd pvals){

    // Init random number generated
    std::random_device rd;
    std::mt19937 re(rd());

    // Allocate memory
    double sum_p = 0.0;
    double norm = 0.0;
    int sum_n = 0;
    int K = pvals.size(); // size of vector (i.e. # of components)
    Eigen::MatrixXd n_vec(1,K);
    Eigen::MatrixXd::Index maxRow, maxCol;

    norm += pvals.sum(); // = 1 if pvals are input (sum of components)

    // For each component (except last one) sample from binomial
    for (int i=0; i < K-1; i++){

        // Binomial sampler varies across loop, pvals and remaining N get adjusted
        std::binomial_distribution<int> binomial(N-sum_n, pvals(i)/(norm - sum_p) );

        if (pvals(i) > 0.0){
            n_vec(0,i) = binomial(re);
        }
        else {
            n_vec(i) = 0;
        };

        sum_p += pvals(i); // adjustment for pvals and remaining n < N
        sum_n += n_vec(0,i);

    };

    // For last component allocate remaining N
    n_vec(0,K-1) = N - sum_n;

    int max = n_vec.maxCoeff(&maxRow, &maxCol); // Get argmax of vector

    return maxCol;

}

// Pybind11 module referencing
PYBIND11_MODULE(hdp_funcs, m) {
    m.doc() = "pybind11 cpp functions used in hierarchical dirichlet processes topic modeling",
    m.def("posterior_t_cpp", &posterior_t_cpp, "Generate posterior pvals to allocate word to table"),
    m.def("fk_cpp", &fk_cpp, "Conditional distribution necessary for posterior sampling"),
    m.def("posterior_k_cpp", &posterior_k_cpp, "Generate posterior pvals to allocate word to topic"),
    m.def("argmax_multinomial_cpp", &argmax_multinomial_cpp, "Vectorized multinomial random sampling");
}


//  (C) Copyright John Maddock 2010.
//  Use, modification and distribution are subject to 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)

#include <gmpxx.h>
#include <boost/math/common_factor.hpp>

template mpz_class boost::math::gcd(const mpz_class&, const mpz_class&);
template mpz_class boost::math::lcm(const mpz_class&, const mpz_class&);

int main()
{
}


#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 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
 */

#include "eigenpy/eigenpy.hpp"
#include "eigenpy/geometry.hpp"
#include <Eigen/Geometry>
#include <iostream>

namespace bp = boost::python;

Eigen::AngleAxisd testOutAngleAxis()
{
  return Eigen::AngleAxisd(.1,Eigen::Vector3d::UnitZ());
}

double testInAngleAxis(Eigen::AngleAxisd aa)
{
  return aa.angle();
}

Eigen::Quaterniond testOutQuaternion()
{
  Eigen::Quaterniond res(1,2,3,4);
  return res;
}
double testInQuaternion( Eigen::Quaterniond q )
{
  return q.norm();
}

BOOST_PYTHON_MODULE(geometry)
{
  eigenpy::enableEigenPy();

  eigenpy::exposeAngleAxis();
  eigenpy::exposeQuaternion();

  bp::def("testOutAngleAxis",&testOutAngleAxis);
  bp::def("testInAngleAxis",&testInAngleAxis);

  bp::def("testOutQuaternion",&testOutQuaternion);
  bp::def("testInQuaternion",&testInQuaternion);

}


#pragma once

#include "mckinnc_ekf/common.hpp"

#include <vector>
#include <Eigen/Dense>

/*! \file
 *  \brief EKF localization algorithm with support for naïve SLAM.
 */

namespace mckinnc_ekf {

//! 3x3 matrix of doubles representing covariance of a 3-vector.
typedef Eigen::Matrix3d Covariance;

class EKF {
  public:
    EKF(const Covariance& modelCovariance,
        const Observation& observationVariance);

    //! Run a single update of the EKF localization.
    /*!
     *  \param control
     *  Linear and angular robot controls for the period since the last run,
     *  and the elapsed time since the last run.
     *  \param observations
     *  Observations of the landmarks.
     *  \param likelyLandmarks
     *  Landmarks that the observations are likely to correspond to.
     *  \param newLandmarkUncertaintyThreshold
     *  Uncertainty above which an observation will be considered a new
     *  landmark, and will not be used for correction. Set to `INFINITY` to
     *  always find a landmark for an observation.
     *  \return
     *  New landmarks from unmatched observations.
     */
    std::vector<Landmark> run(const Control& control,
        const std::vector<Observation>& observations,
        const std::vector<Landmark>& likelyLandmarks,
        double newLandmarkUncertaintyThreshold);

    //! Current mean of the pose estimation.
    const Pose& mean() { return mean_; }
    //! Current covariance of the pose estimation.
    const Covariance& covariance() { return covariance_; }

  protected:
    Pose mean_;
    Covariance covariance_;
    Covariance modelCovariance_;
    Eigen::DiagonalMatrix<double, 3> observationCovariance_;
};

} // namespace