/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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/poses/CPoint3D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>

#include <Eigen/Dense>

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

template class mrpt::CTraitsTest<CPose3D>;

class Pose3DTests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	void test_inverse(const CPose3D& p1)
	{
		const auto HM = p1.getHomogeneousMatrixVal<CMatrixDouble44>();
		CMatrixDouble44 HMi;
		p1.getInverseHomogeneousMatrix(HMi);

		auto I4 = CMatrixDouble44::Identity();

		EXPECT_NEAR(
			(HM.asEigen() * HMi.asEigen() - I4.asEigen()).array().abs().sum(),
			0, 1e-3)
			<< "HM:\n"
			<< HM << "inv(HM):\n"
			<< HMi << "inv(HM)*HM:\n"
			<< (HM * HMi) << endl;

		CPose3D p1_inv_inv = p1;

		p1_inv_inv.inverse();
		const auto HMi_from_p1_inv =
			p1_inv_inv.getHomogeneousMatrixVal<CMatrixDouble44>();

		p1_inv_inv.inverse();

		EXPECT_NEAR(
			(p1.asVectorVal() - p1_inv_inv.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			0, 1e-3)
			<< "p1: " << p1 << "p1_inv_inv: " << p1_inv_inv << endl;

		EXPECT_NEAR((HMi_from_p1_inv - HMi).sum_abs(), 0, 1e-4)
			<< "HMi_from_p1_inv:\n"
			<< HMi_from_p1_inv << "HMi:\n"
			<< HMi << endl;
	}

	void test_compose(const CPose3D& p1, const CPose3D& p2)
	{
		const CPose3D p1_c_p2 = p1 + p2;
		const CPose3D p1_i_p2 = p1 - p2;

		const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1;	// should be -> p2
		const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2;	// Should be -> p1

		EXPECT_NEAR(
			0,
			(p1_c_p2_i_p2.asVectorVal() - p2.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p2          : " << p2 << endl
			<< "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;

		EXPECT_NEAR(
			0,
			(p2_c_p1_i_p2.asVectorVal() - p1.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5)
			<< "p1          : " << p1 << endl
			<< "p2          : " << p2 << endl
			<< "p2 matrix   : " << endl
			<< p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "p1_i_p2     : " << p1_i_p2 << endl
			<< "p1_i_p2 matrix: " << endl
			<< p1_i_p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
			<< "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;

		// Test + operator: trg new var
		{
			CPose3D C = p1;
			CPose3D A = C + p2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - p1_c_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-6);
		}
		// Test + operator: trg same var
		{
			CPose3D A = p1;
			A = A + p2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - p1_c_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-6);
		}
		// Test =+ operator
		{
			CPose3D A = p1;
			A += p2;
			EXPECT_NEAR(
				0,
				(A.asVectorVal() - p1_c_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-6);
		}
	}

	void test_to_from_2d(double x, double y, double phi)
	{
		const CPose2D p2d = CPose2D(x, y, phi);
		const CPose3D p3d = CPose3D(p2d);

		const CPose2D p2d_bis = CPose2D(p3d);

		EXPECT_DOUBLE_EQ(p2d.x(), p2d_bis.x()) << "p2d: " << p2d << endl;
		EXPECT_DOUBLE_EQ(p2d.y(), p2d_bis.y()) << "p2d: " << p2d << endl;
		EXPECT_DOUBLE_EQ(p2d.phi(), p2d_bis.phi()) << "p2d: " << p2d << endl;

		EXPECT_DOUBLE_EQ(p2d.phi(), p3d.yaw()) << "p2d: " << p2d << endl;
	}

	void test_composeFrom(const CPose3D& p1, const CPose3D& p2)
	{
		const CPose3D p1_plus_p2 = p1 + p2;

		{
			CPose3D p1_plus_p2bis;
			p1_plus_p2bis.composeFrom(p1, p2);

			EXPECT_NEAR(
				0,
				(p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-5)
				<< "p2 : " << p2 << endl
				<< "p1 : " << p1 << endl
				<< "p1_plus_p2    : " << p1_plus_p2 << endl
				<< "p1_plus_p2bis : " << p1_plus_p2bis << endl;
		}

		{
			CPose3D p1_plus_p2bis = p1;
			p1_plus_p2bis.composeFrom(p1_plus_p2bis, p2);

			EXPECT_NEAR(
				0,
				(p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-5)
				<< "p2 : " << p2 << endl
				<< "p1 : " << p1 << endl
				<< "p1_plus_p2    : " << p1_plus_p2 << endl
				<< "p1_plus_p2bis : " << p1_plus_p2bis << endl;
		}

		{
			CPose3D p1_plus_p2bis = p2;
			p1_plus_p2bis.composeFrom(p1, p1_plus_p2bis);

			EXPECT_NEAR(
				0,
				(p1_plus_p2bis.asVectorVal() - p1_plus_p2.asVectorVal())
					.asEigen()
					.array()
					.abs()
					.sum(),
				1e-5)
				<< "p2 : " << p2 << endl
				<< "p1 : " << p1 << endl
				<< "p1_plus_p2    : " << p1_plus_p2 << endl
				<< "p1_plus_p2bis : " << p1_plus_p2bis << endl;
		}
	}

	void test_composePoint(const CPose3D& p1, double x, double y, double z)
	{
		const CPoint3D p(x, y, z);
		CPoint3D p1_plus_p = p1 + p;

		CPoint3D p1_plus_p2;
		p1.composePoint(
			p.x(), p.y(), p.z(), p1_plus_p2.x(), p1_plus_p2.y(),
			p1_plus_p2.z());

		EXPECT_NEAR(
			0,
			(p1_plus_p2.asVectorVal() - p1_plus_p.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5);

		// Repeat using same input/output variables:
		{
			double lx = p.x();
			double ly = p.y();
			double lz = p.z();

			p1.composePoint(lx, ly, lz, lx, ly, lz);
			EXPECT_NEAR(
				0,
				std::abs(lx - p1_plus_p.x()) + std::abs(ly - p1_plus_p.y()) +
					std::abs(lz - p1_plus_p.z()),
				1e-5);
		}

		// Inverse:
		CPoint3D p_recov = p1_plus_p - p1;
		CPoint3D p_recov2;
		p1.inverseComposePoint(
			p1_plus_p.x(), p1_plus_p.y(), p1_plus_p.z(), p_recov2.x(),
			p_recov2.y(), p_recov2.z());

		EXPECT_NEAR(
			0,
			(p_recov2.asVectorVal() - p_recov.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5);

		EXPECT_NEAR(
			0,
			(p.asVectorVal() - p_recov.asVectorVal())
				.asEigen()
				.array()
				.abs()
				.sum(),
			1e-5);
	}

	static void func_compose_point(
		const CVectorFixedDouble<6 + 3>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
	{
		CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
		const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
		const CPoint3D pp = q + p;
		for (int i = 0; i < 3; i++)
			Y[i] = pp[i];
	}

	static void func_inv_compose_point(
		const CVectorFixedDouble<6 + 3>& x,
		[[maybe_unused]] const double& dummy, CVectorFixedDouble<3>& Y)
	{
		CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
		const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
		const CPoint3D pp = p - q;
		Y[0] = pp.x();
		Y[1] = pp.y();
		Y[2] = pp.z();
	}

	void test_composePointJacob(
		const CPose3D& p1, double x, double y, double z, bool use_aprox = false)
	{
		const CPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint;
		CMatrixFixed<double, 3, 6> df_dpose;

		TPoint3D pp;
		p1.composePoint(
			x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose, std::nullopt,
			use_aprox);

		// Numerical approx:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6 + 3> x_mean;
			for (int i = 0; i < 6; i++)
				x_mean[i] = p1[i];
			x_mean[6 + 0] = x;
			x_mean[6 + 1] = y;
			x_mean[6 + 2] = z;

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

			num_df_dpose = numJacobs.block<3, 6>(0, 0);
			num_df_dpoint = numJacobs.block<3, 3>(0, 6);
		}

		const double max_error = use_aprox ? 0.1 : 3e-3;

		EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).sum_abs(), max_error)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint << endl
			<< "Implemented method: " << endl
			<< df_dpoint << endl
			<< "Error: " << endl
			<< df_dpoint - num_df_dpoint << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			max_error)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	void test_ExpLnEqual(const CPose3D& p1)
	{
		const CPose3D p2 = Lie::SE<3>::exp(Lie::SE<3>::log(p1));
		EXPECT_NEAR((p1.asVectorVal() - p2.asVectorVal()).sum_abs(), 0, 1e-5)
			<< "p1: " << p1 << endl;
	}

	void test_invComposePointJacob(
		const CPose3D& p1, double x, double y, double z)
	{
		const CPoint3D p(x, y, z);

		CMatrixFixed<double, 3, 3> df_dpoint;
		CMatrixFixed<double, 3, 6> df_dpose;

		TPoint3D pp;
		p1.inverseComposePoint(x, y, z, pp.x, pp.y, pp.z, df_dpoint, df_dpose);

		// Numerical approx:
		CMatrixFixed<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
		CMatrixFixed<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6 + 3> x_mean;
			for (int i = 0; i < 6; i++)
				x_mean[i] = p1[i];
			x_mean[6 + 0] = x;
			x_mean[6 + 1] = y;
			x_mean[6 + 2] = z;

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

			num_df_dpose = numJacobs.block<3, 6>(0, 0);
			num_df_dpoint = numJacobs.block<3, 3>(0, 6);
		}

		EXPECT_NEAR(
			0, (df_dpoint - num_df_dpoint).asEigen().array().abs().sum(), 3e-3)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpoint: " << endl
			<< num_df_dpoint << endl
			<< "Implemented method: " << endl
			<< df_dpoint << endl
			<< "Error: " << endl
			<< df_dpoint - num_df_dpoint << endl;

		EXPECT_NEAR(
			0,
			(df_dpose.asEigen() - num_df_dpose.asEigen()).array().abs().sum(),
			3e-3)
			<< "p1: " << p1 << endl
			<< "p:  " << p << endl
			<< "Numeric approximation of df_dpose: " << endl
			<< num_df_dpose.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dpose.asEigen() << endl
			<< "Error: " << endl
			<< df_dpose.asEigen() - num_df_dpose.asEigen() << endl;
	}

	void test_default_values(const CPose3D& p, const std::string& label)
	{
		EXPECT_EQ(p.x(), 0);
		EXPECT_EQ(p.y(), 0);
		EXPECT_EQ(p.z(), 0);
		EXPECT_EQ(p.yaw(), 0);
		EXPECT_EQ(p.pitch(), 0);
		EXPECT_EQ(p.roll(), 0);
		for (size_t i = 0; i < 4; i++)
			for (size_t j = 0; j < 4; j++)
				EXPECT_NEAR(
					p.getHomogeneousMatrixVal<CMatrixDouble44>()(i, j),
					i == j ? 1.0 : 0.0, 1e-8)
					<< "Failed for (i,j)=" << i << "," << j << endl
					<< "Matrix is: " << endl
					<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
					<< "case was: " << label << endl;
	}

	static void func_compose_point_se3(
		const CVectorFixedDouble<6>& x, const CVectorFixedDouble<3>& P,
		CVectorFixedDouble<3>& Y)
	{
		CPose3D q = Lie::SE<3>::exp(x);
		const CPoint3D p(P[0], P[1], P[2]);
		const CPoint3D pp = q + p;
		for (int i = 0; i < 3; i++)
			Y[i] = pp[i];
	}

	static void func_invcompose_point_se3(
		const CVectorFixedDouble<6>& x, const CVectorFixedDouble<3>& P,
		CVectorFixedDouble<3>& Y)
	{
		CPose3D q = Lie::SE<3>::exp(x);
		const CPoint3D p(P[0], P[1], P[2]);
		const CPoint3D pp = p - q;
		for (int i = 0; i < 3; i++)
			Y[i] = pp[i];
	}

	void test_composePointJacob_se3(const CPose3D& p, const TPoint3D x_l)
	{
		CMatrixFixed<double, 3, 6> df_dse3;

		TPoint3D pp;
		p.composePoint(
			x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
			df_dse3);

		// Numerical approx:
		CMatrixFixed<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6> x_mean;
			for (int i = 0; i < 6; i++)
				x_mean[i] = 0;

			CVectorFixedDouble<3> P;
			for (int i = 0; i < 3; i++)
				P[i] = pp[i];

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-9);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& x,
					const CVectorFixedDouble<3>& P, CVectorFixedDouble<3>& Y)>(
					&func_compose_point_se3),
				x_incrs, P, num_df_dse3);
		}

		EXPECT_NEAR(
			0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
			3e-3)
			<< "p: " << p << endl
			<< "x_l:  " << x_l << endl
			<< "Numeric approximation of df_dse3: " << endl
			<< num_df_dse3.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dse3.asEigen() << endl
			<< "Error: " << endl
			<< df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
	}

	void test_invComposePointJacob_se3(const CPose3D& p, const TPoint3D x_g)
	{
		CMatrixFixed<double, 3, 6> df_dse3;

		TPoint3D pp;
		p.inverseComposePoint(
			x_g.x, x_g.y, x_g.z, pp.x, pp.y, pp.z, std::nullopt, std::nullopt,
			df_dse3);

		// Numerical approx:
		CMatrixFixed<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
		{
			CVectorFixedDouble<6> x_mean;
			for (int i = 0; i < 6; i++)
				x_mean[i] = 0;

			CVectorFixedDouble<3> P;
			for (int i = 0; i < 3; i++)
				P[i] = pp[i];

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-9);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& x,
					const CVectorFixedDouble<3>& P, CVectorFixedDouble<3>& Y)>(
					&func_invcompose_point_se3),
				x_incrs, P, num_df_dse3);
		}

		EXPECT_NEAR(
			0, (df_dse3.asEigen() - num_df_dse3.asEigen()).array().abs().sum(),
			3e-3)
			<< "p: " << p << endl
			<< "x_g:  " << x_g << endl
			<< "Numeric approximation of df_dse3: " << endl
			<< num_df_dse3.asEigen() << endl
			<< "Implemented method: " << endl
			<< df_dse3.asEigen() << endl
			<< "Error: " << endl
			<< df_dse3.asEigen() - num_df_dse3.asEigen() << endl;
	}

	static void func_jacob_expe_e(
		const CVectorFixedDouble<6>& x, [[maybe_unused]] const double& dummy,
		CVectorFixedDouble<12>& Y)
	{
		const CPose3D p = Lie::SE<3>::exp(x);
		// const CMatrixDouble44 R =
		// p.getHomogeneousMatrixVal<CMatrixDouble44>();
		p.getAs12Vector(Y);
	}

	// Check dexp(e)_de
	void check_jacob_expe_e_at_0()
	{
		CVectorFixedDouble<6> x_mean;
		for (int i = 0; i < 6; i++)
			x_mean[i] = 0;

		const double& dummy = 0.;
		CVectorFixedDouble<6> x_incrs;
		x_incrs.fill(1e-9);
		CMatrixDouble numJacobs;
		mrpt::math::estimateJacobian(
			x_mean,
			std::function<void(
				const CVectorFixedDouble<6>& x, const double& dummy,
				CVectorFixedDouble<12>& Y)>(&func_jacob_expe_e),
			x_incrs, dummy, numJacobs);

		// Theoretical matrix:
		// [ 0   -[e1]_x ]
		// [ 0   -[e2]_x ]
		// [ 0   -[e3]_x ]
		// [ I_3    0    ]
		double vals[12 * 6] = {
			0, 0, 0, 0, 0, 0,  0, 0, 0, 0,	0, 1, 0, 0, 0, 0, -1, 0,

			0, 0, 0, 0, 0, -1, 0, 0, 0, 0,	0, 0, 0, 0, 0, 1, 0,  0,

			0, 0, 0, 0, 1, 0,  0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0,  0,

			1, 0, 0, 0, 0, 0,  0, 1, 0, 0,	0, 0, 0, 0, 1, 0, 0,  0};
		CMatrixFixed<double, 12, 6> M(vals);

		EXPECT_NEAR(
			(numJacobs.asEigen() - M.asEigen()).array().abs().maxCoeff(), 0,
			1e-5)
			<< "M:\n"
			<< M.asEigen() << "numJacobs:\n"
			<< numJacobs << "\n";
	}

	static void func_jacob_LnT_T(
		const CVectorFixedDouble<12>& x, [[maybe_unused]] const double& dummy,
		CVectorFixedDouble<6>& Y)
	{
		CPose3D p;

		p.setFrom12Vector(x);
		// ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
		auto R = p.getRotationMatrix();
		const auto Rsvd =
			R.asEigen().jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
		R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
		p.setRotationMatrix(R);

		Y = Lie::SE<3>::log(p);
	}

	// Jacobian of Ln(T) wrt T
	void check_jacob_LnT_T(const CPose3D& p)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dlogv_dv(p);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<12> x_mean;
			p.getAs12Vector(x_mean);

			const double& dummy = 0.;
			CVectorFixedDouble<12> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<12>& x, const double& dummy,
					CVectorFixedDouble<6>& Y)>(&func_jacob_LnT_T),
				x_incrs, dummy, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen()).array().abs().sum(),
			0, 1e-3)
			<< "Pose: " << p << endl
			<< "Pose matrix:\n"
			<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}

	static void func_jacob_expe_D(
		const CVectorFixedDouble<6>& eps, const CPose3D& D,
		CVectorFixedDouble<12>& Y)
	{
		const CPose3D incr = Lie::SE<3>::exp(eps);
		const CPose3D expe_D = incr + D;
		expe_D.getAs12Vector(Y);
	}

	// Test Jacobian: d exp(e)*D / d e
	// 10.3.3 in tech report
	void test_Jacob_dexpeD_de(const CPose3D& p)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dexpeD_de(p);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<6> x_mean;
			x_mean.setZero();

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& eps, const CPose3D& D,
					CVectorFixedDouble<12>& Y)>(&func_jacob_expe_D),
				x_incrs, p, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen())
				.array()
				.abs()
				.maxCoeff(),
			0, 1e-3)
			<< "Pose: " << p << endl
			<< "Pose matrix:\n"
			<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}

	static void func_jacob_D_expe(
		const CVectorFixedDouble<6>& eps, const CPose3D& D,
		CVectorFixedDouble<12>& Y)
	{
		const CPose3D incr = Lie::SE<3>::exp(eps);
		const CPose3D expe_D = D + incr;
		expe_D.getAs12Vector(Y);
	}

	// Test Jacobian: d D*exp(e) / d e
	// 10.3.4 in tech report
	void test_Jacob_dDexpe_de(const CPose3D& p)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dDexpe_de(p);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<6> x_mean;
			x_mean.setZero();

			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& eps, const CPose3D& D,
					CVectorFixedDouble<12>& Y)>(&func_jacob_D_expe),
				x_incrs, p, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen())
				.array()
				.abs()
				.maxCoeff(),
			0, 1e-3)
			<< "Pose: " << p << endl
			<< "Pose matrix:\n"
			<< p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}

	struct TParams_func_jacob_Aexpe_D
	{
		CPose3D A, D;
	};

	static void func_jacob_Aexpe_D(
		const CVectorFixedDouble<6>& eps,
		const TParams_func_jacob_Aexpe_D& params, CVectorFixedDouble<12>& Y)
	{
		const CPose3D incr = Lie::SE<3>::exp(eps);
		const CPose3D res = params.A + incr + params.D;
		res.getAs12Vector(Y);
	}

	// Test Jacobian: d A*exp(e)*D / d e
	// 10.3.7 in tech report
	// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
	void test_Jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D)
	{
		const auto theor_jacob = Lie::SE<3>::jacob_dAexpeD_de(A, D);

		CMatrixDouble numJacobs;
		{
			CVectorFixedDouble<6> x_mean;
			x_mean.setZero();

			TParams_func_jacob_Aexpe_D params;
			params.A = A;
			params.D = D;
			CVectorFixedDouble<6> x_incrs;
			x_incrs.fill(1e-6);
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<6>& eps,
					const TParams_func_jacob_Aexpe_D& params,
					CVectorFixedDouble<12>& Y)>(&func_jacob_Aexpe_D),
				x_incrs, params, numJacobs);
		}

		EXPECT_NEAR(
			(numJacobs.asEigen() - theor_jacob.asEigen())
				.array()
				.abs()
				.maxCoeff(),
			0, 1e-3)
			<< "Pose A: " << A << endl
			<< "Pose D: " << D << endl
			<< "Num. Jacob:\n"
			<< numJacobs << endl
			<< "Theor. Jacob:\n"
			<< theor_jacob.asEigen() << endl
			<< "ERR:\n"
			<< theor_jacob.asEigen() - numJacobs.asEigen() << endl;
	}
};

// Elemental tests:
TEST_F(Pose3DTests, DefaultValues)
{
	{
		CPose3D p;
		test_default_values(p, "Default");
	}
	{
		CPose3D p2;
		CPose3D p = p2;
		test_default_values(p, "p=p2");
	}
	{
		CPose3D p1, p2;
		test_default_values(p1 + p2, "p1+p2");
		CPose3D p = p1 + p2;
		test_default_values(p, "p=p1+p2");
	}
	{
		CPose3D p1, p2;
		CPose3D p = p1 - p2;
		test_default_values(p, "p1-p2");
	}
}

TEST_F(Pose3DTests, Initialization)
{
	CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
	EXPECT_NEAR(p.x(), 1, 1e-7);
	EXPECT_NEAR(p.y(), 2, 1e-7);
	EXPECT_NEAR(p.z(), 3, 1e-7);
	EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
	EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
	EXPECT_NEAR(p.roll(), 0.4, 1e-7);
}

TEST_F(Pose3DTests, OperatorBracket)
{
	CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
	EXPECT_NEAR(p[0], 1, 1e-7);
	EXPECT_NEAR(p[1], 2, 1e-7);
	EXPECT_NEAR(p[2], 3, 1e-7);
	EXPECT_NEAR(p[3], 0.2, 1e-7);
	EXPECT_NEAR(p[4], 0.3, 1e-7);
	EXPECT_NEAR(p[5], 0.4, 1e-7);
}

// List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
// degrees)
static const std::vector<mrpt::poses::CPose3D> ptc = {
	{.0, .0, .0, .0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
	{1.0, 2.0, 3.0, 80.0_deg, 5.0_deg, 5.0_deg},
	{1.0, 2.0, 3.0, -20.0_deg, -30.0_deg, -40.0_deg},
	{1.0, 2.0, 3.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 2.0, 3.0, 40.0_deg, -5.0_deg, 25.0_deg},
	{1.0, 2.0, 3.0, 40.0_deg, 20.0_deg, -15.0_deg},
	{-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{6.0, -5.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{6.0, 2.0, -9.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{0.0, 8.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 0.0, 5.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg}};

// More complex tests:
TEST_F(Pose3DTests, InverseHM)
{
	for (const auto& p : ptc)
		test_inverse(p);
}

TEST_F(Pose3DTests, Compose)
{
	for (const auto& p1 : ptc)
		for (const auto& p2 : ptc)
			test_compose(p1, p2);
}
TEST_F(Pose3DTests, composeFrom)
{
	for (const auto& p1 : ptc)
		for (const auto& p2 : ptc)
			test_composeFrom(p1, p2);
}

TEST_F(Pose3DTests, ToFromCPose2D)
{
	for (const auto& p : ptc)
		test_to_from_2d(p.x(), p.y(), p.yaw());
}

TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
{
	for (const auto& p : ptc)
	{
		test_composePoint(p, 10, 11, 12);
		test_composePoint(p, -5, 1, 2);
		test_composePoint(p, 5, -1, 2);
		test_composePoint(p, 5, 1, -2);
	}
}

TEST_F(Pose3DTests, ComposePointJacob)
{
	for (const auto& p : ptc)
	{
		test_composePointJacob(p, 10, 11, 12);
		test_composePointJacob(p, -5, 1, 2);
	}
}

TEST_F(Pose3DTests, ComposePointJacobApprox)
{  // Test approximated Jacobians for very small rotations
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.0_deg), 10, 11,
		12, true);
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.1_deg, 0.0_deg, 0.0_deg), 10, 11,
		12, true);
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.1_deg, 0.0_deg), 10, 11,
		12, true);
	test_composePointJacob(
		mrpt::poses::CPose3D(1.0, 2.0, 3.0, 0.0_deg, 0.0_deg, 0.1_deg), 10, 11,
		12, true);
}

TEST_F(Pose3DTests, InvComposePointJacob)
{
	for (const auto& p : ptc)
	{
		test_invComposePointJacob(p, 10, 11, 12);
		test_invComposePointJacob(p, -5, 1, 2);
		test_invComposePointJacob(p, 5, -1, 2);
		test_invComposePointJacob(p, 5, 1, -2);
	}
}

TEST_F(Pose3DTests, ComposePointJacob_se3)
{
	for (const auto& p : ptc)
	{
		test_composePointJacob_se3(p, TPoint3D(0, 0, 0));
		test_composePointJacob_se3(p, TPoint3D(10, 11, 12));
		test_composePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
	}
}
TEST_F(Pose3DTests, InvComposePointJacob_se3)
{
	for (const auto& p : ptc)
	{
		test_invComposePointJacob_se3(p, TPoint3D(0, 0, 0));
		test_invComposePointJacob_se3(p, TPoint3D(10, 11, 12));
		test_invComposePointJacob_se3(p, TPoint3D(-5.0, -15.0, 8.0));
	}
}

TEST_F(Pose3DTests, ExpLnEqual)
{
	for (const auto& p : ptc)
		test_ExpLnEqual(p);
}

TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
TEST_F(Pose3DTests, Jacob_dLnT_dT)
{
	check_jacob_LnT_T(mrpt::poses::CPose3D(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg));
	// JL NOTE:
	//  This function cannot be properly tested numerically, since the logm()
	//  implementation
	//  is not generic and does NOT depends on all matrix entries, thus the
	//  numerical Jacobian
	//  contains entire columns of zeros, even if the theorethical doesn't.
	//	check_jacob_LnT_T(1.0,0,0, 0.0_deg,0.0_deg,0.0_deg ); ...
}

TEST_F(Pose3DTests, Jacob_dexpeD_de)
{
	for (const auto& p : ptc)
		test_Jacob_dexpeD_de(p);
}

TEST_F(Pose3DTests, Jacob_dDexpe_de)
{
	for (const auto& p : ptc)
		test_Jacob_dDexpe_de(p);
}

TEST_F(Pose3DTests, Jacob_dAexpeD_de)
{
	for (const auto& p1 : ptc)
		for (const auto& p2 : ptc)
			test_Jacob_dAexpeD_de(p1, p2);
}

TEST_F(Pose3DTests, translation)
{
	const auto p =
		mrpt::poses::CPose3D::FromXYZYawPitchRoll(1.0, 2.0, 3.0, 0, 0, 0);
	const mrpt::math::TPoint3D t = p.translation();
	EXPECT_EQ(t, mrpt::math::TPoint3D(1, 2, 3));
}
