/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/math/transform_gaussian.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DQuatPDFGaussian.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<CPose3DQuatPDFGaussian>;

class Pose3DQuatPDFGaussTests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	static CPose3DQuatPDFGaussian generateRandomPoseQuat3DPDF(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale)
	{
		return CPose3DQuatPDFGaussian(
			generateRandomPose3DPDF(x, y, z, yaw, pitch, roll, std_scale));
	}

	static CPose3DPDFGaussian generateRandomPose3DPDF(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale)
	{
		CMatrixDouble61 r;
		mrpt::random::getRandomGenerator().drawGaussian1DMatrix(
			r, 0, std_scale);
		CMatrixDouble66 cov;
		cov.matProductOf_AAt(r);  // random semi-definite positive matrix:
		for (int i = 0; i < 6; i++)
			cov(i, i) += 1e-7;
		CPose3DPDFGaussian p6pdf(CPose3D(x, y, z, yaw, pitch, roll), cov);
		return p6pdf;
	}

	void test_toFromYPRGauss(
		double yaw, double pitch, double roll, bool useGimbalLockCov = false)
	{
		const bool usesSUT =
			mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION();

		// Random pose:
		CPose3DPDFGaussian p1ypr =
			generateRandomPose3DPDF(1.0, 2.0, 3.0, yaw, pitch, roll, 1e-2);

		// In gimbal-lock, it's impossible to recover uncertainty for
		// pitch/roll, since both are fixed values:
		if (useGimbalLockCov)
		{
			for (int i = 0; i < 6; i++)
				for (int j = 4; j < 6; j++)
					p1ypr.cov(i, j) = p1ypr.cov(j, i) = 0;
		}

		CPose3DQuatPDFGaussian p1quat = CPose3DQuatPDFGaussian(p1ypr);

		// Convert back to a 6x6 representation:
		CPose3DPDFGaussian p2ypr = CPose3DPDFGaussian(p1quat);

		std::stringstream onFailMsg;
		onFailMsg << "p1ypr: " << endl
				  << p1ypr << endl
				  << "p1quat : " << endl
				  << p1quat << endl
				  << "p2ypr : " << endl
				  << p2ypr << endl
				  << "USE_SUT_QUAT2EULER_CONVERSION: "
				  << mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION()
				  << endl;

		EXPECT_NEAR(
			0, (p2ypr.cov - p1ypr.cov).array().abs().maxCoeff(),
			usesSUT ? 1e-2 : 1e-6)
			<< onFailMsg.str();

		EXPECT_NEAR(
			0,
			(p2ypr.mean.asVectorVal() - p1ypr.mean.asVectorVal())
				.array()
				.abs()
				.maxCoeff(),
			usesSUT ? 0.1 : 1e-4)
			<< onFailMsg.str();
	}

	static void func_compose(
		const CVectorFixedDouble<2 * 7>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<7>& Y)
	{
		const CPose3DQuat p1(
			x[0], x[1], x[2], CQuaternionDouble(x[3], x[4], x[5], x[6]));
		const CPose3DQuat p2(
			x[7 + 0], x[7 + 1], x[7 + 2],
			CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
		const CPose3DQuat p = p1 + p2;
		for (int i = 0; i < 7; i++)
			Y[i] = p[i];
	}

	static void func_inv_compose(
		const CVectorFixedDouble<2 * 7>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<7>& Y)
	{
		CQuaternionDouble q(x[3], x[4], x[5], x[6]);
		q.normalize();
		const CPose3DQuat p1(x[0], x[1], x[2], q);
		const CPose3DQuat p2(
			x[7 + 0], x[7 + 1], x[7 + 2],
			CQuaternionDouble(x[7 + 3], x[7 + 4], x[7 + 5], x[7 + 6]));
		const CPose3DQuat p = p1 - p2;
		for (int i = 0; i < 7; i++)
			Y[i] = p[i];
	}

	void testPoseComposition(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale, double x2, double y2, double z2, double yaw2,
		double pitch2, double roll2, double std_scale2)
	{
		CPose3DQuatPDFGaussian p7pdf1 =
			generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
		CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
			x2, y2, z2, yaw2, pitch2, roll2, std_scale2);

		CPose3DQuatPDFGaussian p7_comp = p7pdf1 + p7pdf2;

		// Numeric approximation:
		CVectorFixedDouble<7> y_mean;
		CMatrixFixed<double, 7, 7> y_cov;
		{
			CVectorFixedDouble<2 * 7> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = p7pdf1.mean[i];
			for (int i = 0; i < 7; i++)
				x_mean[7 + i] = p7pdf2.mean[i];

			CMatrixFixed<double, 14, 14> x_cov;
			x_cov.insertMatrix(0, 0, p7pdf1.cov);
			x_cov.insertMatrix(7, 7, p7pdf2.cov);

			double DUMMY = 0;
			CVectorFixedDouble<2 * 7> x_incrs;
			x_incrs.fill(1e-6);
			transform_gaussian_linear(
				x_mean, x_cov, func_compose, DUMMY, y_mean, y_cov, x_incrs);
		}
		// Compare:
		EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().maxCoeff(), 1e-3)
			<< "p1 mean: " << p7pdf1.mean << endl
			<< "p2 mean: " << p7pdf2.mean << endl
			<< "Numeric approximation of covariance: " << endl
			<< y_cov << endl
			<< "Returned covariance: " << endl
			<< p7_comp.cov << endl;
	}

	static void func_inverse(
		const CVectorFixedDouble<7>& x, [[maybe_unused]] const double& dummy,
		CVectorFixedDouble<7>& Y)
	{
		CQuaternionDouble q(x[3], x[4], x[5], x[6]);
		q.normalize();
		const CPose3DQuat p1(x[0], x[1], x[2], q);
		const CPose3DQuat p1_inv(-p1);
		for (int i = 0; i < 7; i++)
			Y[i] = p1_inv[i];
	}

	void testCompositionJacobian(
		double x, double y, double z, double yaw, double pitch, double roll,
		double x2, double y2, double z2, double yaw2, double pitch2,
		double roll2)
	{
		const CPose3DQuat q1(CPose3D(x, y, z, yaw, pitch, roll));
		const CPose3DQuat q2(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));

		// Theoretical Jacobians:
		CMatrixDouble77 df_dx(UNINITIALIZED_MATRIX),
			df_du(UNINITIALIZED_MATRIX);
		CPose3DQuatPDF::jacobiansPoseComposition(
			q1,	 // x
			q2,	 // u
			df_dx, df_du);

		// Numerical approximation:
		CMatrixDouble77 num_df_dx(UNINITIALIZED_MATRIX),
			num_df_du(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<2 * 7> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = q1[i];
			for (int i = 0; i < 7; i++)
				x_mean[7 + i] = q2[i];

			double DUMMY = 0;
			CVectorFixedDouble<2 * 7> x_incrs;
			x_incrs.fill(1e-7);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<2 * 7>& x, const double& dummy,
					CVectorFixedDouble<7>& Y)>(&func_compose),
				x_incrs, DUMMY, numJacobs);

			num_df_dx = numJacobs.block<7, 7>(0, 0);
			num_df_du = numJacobs.block<7, 7>(0, 7);
		}

		// Compare:
		EXPECT_NEAR(0, (df_dx - num_df_dx).array().abs().maxCoeff(), 1e-6)
			<< "q1: " << q1 << endl
			<< "q2: " << q2 << endl
			<< "Numeric approximation of df_dx: " << endl
			<< num_df_dx << endl
			<< "Implemented method: " << endl
			<< df_dx << endl
			<< "Error: " << endl
			<< df_dx - num_df_dx << endl;

		EXPECT_NEAR(0, (df_du - num_df_du).array().abs().maxCoeff(), 1e-6)
			<< "q1: " << q1 << endl
			<< "q2: " << q2 << endl
			<< "Numeric approximation of df_du: " << endl
			<< num_df_du << endl
			<< "Implemented method: " << endl
			<< df_du << endl
			<< "Error: " << endl
			<< df_du - num_df_du << endl;
	}

	void testInverse(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale)
	{
		CPose3DQuatPDFGaussian p7pdf1 =
			generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);

		CPose3DQuatPDFGaussian p7_inv = -p7pdf1;

		// Numeric approximation:
		CVectorFixedDouble<7> y_mean;
		CMatrixFixed<double, 7, 7> y_cov;
		{
			CVectorFixedDouble<7> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = p7pdf1.mean[i];

			CMatrixFixed<double, 7, 7> x_cov;
			x_cov.insertMatrix(0, 0, p7pdf1.cov);

			double DUMMY = 0;
			CVectorFixedDouble<7> x_incrs;
			x_incrs.fill(1e-6);
			transform_gaussian_linear(
				x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
		}

		// Compare:
		EXPECT_NEAR(0, (y_cov - p7_inv.cov).array().abs().maxCoeff(), 1e-6)
			<< "p1 mean: " << p7pdf1.mean << endl
			<< "inv mean: " << p7_inv.mean << endl
			<< "Numeric approximation of covariance: " << endl
			<< y_cov << endl
			<< "Returned covariance: " << endl
			<< p7_inv.cov << endl
			<< "Error: " << endl
			<< y_cov - p7_inv.cov << endl;
	}

	void testPoseInverseComposition(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale, double x2, double y2, double z2, double yaw2,
		double pitch2, double roll2, double std_scale2)
	{
		CPose3DQuatPDFGaussian p7pdf1 =
			generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
		CPose3DQuatPDFGaussian p7pdf2 = generateRandomPoseQuat3DPDF(
			x2, y2, z2, yaw2, pitch2, roll2, std_scale2);

		CPose3DQuatPDFGaussian p7_comp = p7pdf1 - p7pdf2;

		// Numeric approximation:
		CVectorFixedDouble<7> y_mean;
		CMatrixFixed<double, 7, 7> y_cov;
		{
			CVectorFixedDouble<2 * 7> x_mean;
			for (int i = 0; i < 7; i++)
				x_mean[i] = p7pdf1.mean[i];
			for (int i = 0; i < 7; i++)
				x_mean[7 + i] = p7pdf2.mean[i];

			CMatrixFixed<double, 14, 14> x_cov;
			x_cov.insertMatrix(0, 0, p7pdf1.cov);
			x_cov.insertMatrix(7, 7, p7pdf2.cov);

			double DUMMY = 0;
			CVectorFixedDouble<2 * 7> x_incrs;
			x_incrs.fill(1e-6);
			transform_gaussian_linear(
				x_mean, x_cov, func_inv_compose, DUMMY, y_mean, y_cov, x_incrs);
		}
		// Compare:
		EXPECT_NEAR(0, (y_cov - p7_comp.cov).array().abs().maxCoeff(), 1e-6)
			<< "p1 mean: " << p7pdf1.mean << endl
			<< "p2 mean: " << p7pdf2.mean << endl
			<< "Numeric approximation of covariance: " << endl
			<< y_cov << endl
			<< "Returned covariance: " << endl
			<< p7_comp.cov << endl;
	}

	void testInverseCompositionCrossCorrelation(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale, double x2, double y2, double z2, double yaw2,
		double pitch2, double roll2, double std_scale2)
	{
		CPose3DQuatPDFGaussian p7pdf1 =
			generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);
		CPose3DQuatPDFGaussian p7_displacement = generateRandomPoseQuat3DPDF(
			x2, y2, z2, yaw2, pitch2, roll2, std_scale2);

		CPose3DQuatPDFGaussian p7pdf2 = p7pdf1 + p7_displacement;

		CPose3DQuatPDFGaussian p7_displacement_computed =
			p7pdf1.inverseCompositionCrossCorrelation(p7pdf2);

		// Compare:
		EXPECT_NEAR(
			0,
			(p7_displacement_computed.cov - p7_displacement.cov)
				.array()
				.abs()
				.maxCoeff(),
			1e-6)
			<< "p1 mean: " << p7pdf1.mean << endl
			<< "p2 mean: " << p7pdf2.mean << endl
			<< "Computed displacement covariance: " << endl
			<< p7_displacement_computed.cov << endl
			<< "True displacement covariance: " << endl
			<< p7_displacement.cov << endl;
	}

	void testChangeCoordsRef(
		double x, double y, double z, double yaw, double pitch, double roll,
		double std_scale, double x2, double y2, double z2, double yaw2,
		double pitch2, double roll2)
	{
		CPose3DQuatPDFGaussian p7pdf1 =
			generateRandomPoseQuat3DPDF(x, y, z, yaw, pitch, roll, std_scale);

		const CPose3DQuat new_base =
			CPose3DQuat(CPose3D(x2, y2, z2, yaw2, pitch2, roll2));
		const CPose3DQuatPDFGaussian new_base_pdf(
			new_base, CMatrixDouble77());  // COV = Zeros

		const CPose3DQuatPDFGaussian p7_new_base_pdf = new_base_pdf + p7pdf1;
		p7pdf1.changeCoordinatesReference(new_base);

		// Compare:
		EXPECT_NEAR(
			0, (p7_new_base_pdf.cov - p7pdf1.cov).array().abs().maxCoeff(),
			1e-2)
			<< "p1 mean: " << p7pdf1.mean << endl
			<< "new_base: " << new_base << endl;
		EXPECT_NEAR(
			0,
			(p7_new_base_pdf.mean.asVectorVal() - p7pdf1.mean.asVectorVal())
				.array()
				.abs()
				.mean(),
			1e-2)
			<< "p1 mean: " << p7pdf1.mean << endl
			<< "new_base: " << new_base << endl;
	}
};

TEST_F(Pose3DQuatPDFGaussTests, ToYPRGaussPDFAndBack)
{
	for (int USE_SUT = 0; USE_SUT <= 1; USE_SUT++)
	{
		mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION(USE_SUT != 0);

		test_toFromYPRGauss(-30.0_deg, 10.0_deg, 60.0_deg);
		test_toFromYPRGauss(30.0_deg, 88.0_deg, 0.0_deg);
		test_toFromYPRGauss(30.0_deg, 89.5_deg, 0.0_deg);
		test_toFromYPRGauss(20.0_deg, 89.999_deg, 0.0_deg, true /*gimbal*/);
		test_toFromYPRGauss(20.0_deg, -89.999_deg, 0.0_deg, true /*gimbal*/);
	}
}

TEST_F(Pose3DQuatPDFGaussTests, CompositionJacobian)
{
	testCompositionJacobian(
		0, 0, 0, 2.0_deg, 0.0_deg, 0.0_deg, 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
	testCompositionJacobian(
		1, 2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg);
	testCompositionJacobian(
		1, -2, 3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg);
	testCompositionJacobian(
		1, 2, -3, 2.0_deg, 0.0_deg, 0.0_deg, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg);
	testCompositionJacobian(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
		30.0_deg);
	testCompositionJacobian(
		1, 2, 3, 20.0_deg, -80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
		30.0_deg);
	testCompositionJacobian(
		1, 2, 3, 20.0_deg, 80.0_deg, -70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
		30.0_deg);
	testCompositionJacobian(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, -50.0_deg, -10.0_deg,
		30.0_deg);
	testCompositionJacobian(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, 10.0_deg,
		30.0_deg);
	testCompositionJacobian(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, -8, 45, 10, 50.0_deg, -10.0_deg,
		-30.0_deg);
}

TEST_F(Pose3DQuatPDFGaussTests, Inverse)
{
	testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(0, 0, 0, 10.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(0, 0, 0, 0.0_deg, 10.0_deg, 0.0_deg, 0.1);
	testInverse(0, 0, 0, 0.0_deg, 0.0_deg, 10.0_deg, 0.1);

	testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.2);

	testInverse(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, -3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, -3, 30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, 3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, 3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, -3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, -3, -30.0_deg, 0.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, -3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, -3, 0.0_deg, 30.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, 3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, 3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, -3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
	testInverse(-1, 2, -3, 0.0_deg, -30.0_deg, 0.0_deg, 0.1);
	testInverse(1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
	testInverse(-1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
	testInverse(1, 2, -3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
	testInverse(-1, 2, -3, 0.0_deg, 0.0_deg, 30.0_deg, 0.1);
	testInverse(1, 2, 3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
	testInverse(-1, 2, 3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
	testInverse(1, 2, -3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
	testInverse(-1, 2, -3, 0.0_deg, 0.0_deg, -30.0_deg, 0.1);
}

TEST_F(Pose3DQuatPDFGaussTests, Composition)
{
	testPoseComposition(
		0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);

	testPoseComposition(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg, 0.2);

	testPoseComposition(
		1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
		0.0_deg, 0.1);
	testPoseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		10.0_deg, 0.1);
}

TEST_F(Pose3DQuatPDFGaussTests, InverseComposition)
{
	testPoseInverseComposition(
		0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);

	testPoseInverseComposition(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg, 0.2);

	testPoseInverseComposition(
		1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
		0.0_deg, 0.1);
	testPoseInverseComposition(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		10.0_deg, 0.1);
}

TEST_F(Pose3DQuatPDFGaussTests, RelativeDisplacement)
{
	testInverseCompositionCrossCorrelation(
		0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);

	testInverseCompositionCrossCorrelation(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg, 0.2);

	testInverseCompositionCrossCorrelation(
		1, 2, 3, 10.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 0.0_deg, 10.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 0.0_deg, 0.0_deg, 10.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 10.0_deg, 0.0_deg,
		0.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 10.0_deg,
		0.0_deg, 0.1);
	testInverseCompositionCrossCorrelation(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		10.0_deg, 0.1);
}

TEST_F(Pose3DQuatPDFGaussTests, ChangeCoordsRef)
{
	testChangeCoordsRef(
		0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, 0, 0, 0, 0.0_deg, 0.0_deg,
		0.0_deg);
	testChangeCoordsRef(
		1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg, 0.1, -8, 45, 10, 0.0_deg, 0.0_deg,
		0.0_deg);

	testChangeCoordsRef(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.1, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg);
	testChangeCoordsRef(
		1, 2, 3, 20.0_deg, 80.0_deg, 70.0_deg, 0.2, -8, 45, 10, 50.0_deg,
		-10.0_deg, 30.0_deg);
}
