/* +------------------------------------------------------------------------+
   |                     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 "slam-precomp.h"  // Precompiled headers
//
#include <mrpt/config/CConfigFileBase.h>  // MRPT_LOAD_*()
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/tfest.h>

#include <Eigen/Dense>

using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::tfest;
using namespace mrpt::system;
using namespace mrpt::poses;
using namespace std;

CPosePDF::Ptr CICP::AlignPDF(
	const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
	const CPosePDFGaussian& initialEstimationPDF,
	mrpt::optional_ref<TMetricMapAlignmentResult> outInfo)
{
	MRPT_START

	CTicTac tictac;
	TReturnInfo outInfoVal;
	CPosePDF::Ptr resultPDF;

	if (outInfo) tictac.Tic();

	switch (options.ICP_algorithm)
	{
		case icpClassic:
			resultPDF =
				ICP_Method_Classic(m1, mm2, initialEstimationPDF, outInfoVal);
			break;
		case icpLevenbergMarquardt:
			resultPDF =
				ICP_Method_LM(m1, mm2, initialEstimationPDF, outInfoVal);
			break;
		default:
			THROW_EXCEPTION_FMT(
				"Invalid value for ICP_algorithm: %i",
				static_cast<int>(options.ICP_algorithm));
	}  // end switch

	if (outInfo) outInfoVal.executionTime = tictac.Tac();

	// Copy the output info if requested:
	if (outInfo)
		if (auto* o = dynamic_cast<TReturnInfo*>(&outInfo.value().get()); o)
			*o = outInfoVal;

	return resultPDF;

	MRPT_END
}

/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CICP::TConfigParams::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
	MRPT_LOAD_CONFIG_VAR(maxIterations, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(minAbsStep_trans, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(minAbsStep_rot, float, iniFile, section);

	ICP_algorithm = iniFile.read_enum<TICPAlgorithm>(
		section, "ICP_algorithm", ICP_algorithm);
	ICP_covariance_method = iniFile.read_enum<TICPCovarianceMethod>(
		section, "ICP_covariance_method", ICP_covariance_method);

	MRPT_LOAD_CONFIG_VAR(thresholdDist, float, iniFile, section);
	thresholdAng = DEG2RAD(iniFile.read_float(
		section.c_str(), "thresholdAng_DEG", RAD2DEG(thresholdAng)));

	MRPT_LOAD_CONFIG_VAR(ALFA, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(smallestThresholdDist, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(onlyUniqueRobust, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(doRANSAC, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(covariance_varPoints, float, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(ransac_minSetSize, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(ransac_maxSetSize, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(
		ransac_mahalanobisDistanceThreshold, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(ransac_nSimulations, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(normalizationStd, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(ransac_fuseByCorrsMatch, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(ransac_fuseMaxDiffXY, float, iniFile, section);
	ransac_fuseMaxDiffPhi = DEG2RAD(iniFile.read_float(
		section.c_str(), "ransac_fuseMaxDiffPhi_DEG",
		RAD2DEG(ransac_fuseMaxDiffPhi)));

	MRPT_LOAD_CONFIG_VAR(kernel_rho, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(use_kernel, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(Axy_aprox_derivatives, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LM_initial_lambda, float, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(skip_cov_calculation, bool, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(skip_quality_calculation, bool, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(
		corresponding_points_decimation, int, iniFile, section);
}

void CICP::TConfigParams::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		ICP_algorithm, "The ICP algorithm to use (see enum TICPAlgorithm)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		ICP_covariance_method,
		"Method to use for covariance estimation (see enum "
		"TICPCovarianceMethod)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		onlyUniqueRobust,
		"Only the closest correspondence for each reference point will be "
		"kept");
	MRPT_SAVE_CONFIG_VAR_COMMENT(maxIterations, "Iterations");
	MRPT_SAVE_CONFIG_VAR_COMMENT(minAbsStep_trans, "Termination criteria");
	MRPT_SAVE_CONFIG_VAR_COMMENT(minAbsStep_rot, "Termination criteria");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		thresholdDist,
		"Initial threshold distance for two points to be a match");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		thresholdAng,
		"Initial threshold distance for two points to be a match");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		ALFA,
		"The scale factor for thresholds every time convergence is achieved");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		smallestThresholdDist,
		"The size for threshold such that iterations will stop, "
		"since it is considered precise enough.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(covariance_varPoints, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(doRANSAC, "Perform a RANSAC step");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_minSetSize, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_maxSetSize, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_nSimulations, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_mahalanobisDistanceThreshold, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(normalizationStd, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_fuseByCorrsMatch, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_fuseMaxDiffXY, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(ransac_fuseMaxDiffPhi, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(kernel_rho, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(use_kernel, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(Axy_aprox_derivatives, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LM_initial_lambda, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(skip_cov_calculation, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(skip_quality_calculation, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(corresponding_points_decimation, "");
}

float CICP::kernel(float x2, float rho2)
{
	return options.use_kernel ? (x2 / (x2 + rho2)) : x2;
}

/*----------------------------------------------------------------------------

					ICP_Method_Classic

  ----------------------------------------------------------------------------*/
CPosePDF::Ptr CICP::ICP_Method_Classic(
	const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
	const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
{
	MRPT_START

	// The result can be either a Gaussian or a SOG:
	CPosePDF::Ptr resultPDF;
	CPosePDFGaussian::Ptr gaussPdf;
	CPosePDFSOG::Ptr SOG;

	size_t nCorrespondences = 0;
	bool keepApproaching;
	CPose2D grossEst = initialEstimationPDF.mean;
	mrpt::tfest::TMatchingPairList correspondences, old_correspondences;
	CPose2D lastMeanPose;

	// Assure the class of the maps:
	const mrpt::maps::CMetricMap* m2 = mm2;

	// Asserts:
	// -----------------
	ASSERT_(options.ALFA >= 0 && options.ALFA < 1);

	// The algorithm output auxiliar info:
	// -------------------------------------------------
	outInfo.nIterations = 0;
	outInfo.goodness = 1;
	outInfo.quality = 0;

	// The gaussian PDF to estimate:
	// ------------------------------------------------------
	gaussPdf = std::make_shared<CPosePDFGaussian>();

	// First gross approximation:
	gaussPdf->mean = grossEst;

	// Initial thresholds:
	mrpt::maps::TMatchingParams matchParams;
	mrpt::maps::TMatchingExtraResults matchExtraResults;

	matchParams.maxDistForCorrespondence =
		options.thresholdDist;	// Distance threshold.
	matchParams.maxAngularDistForCorrespondence =
		options.thresholdAng;  // Angular threshold.
	// Option onlyClosestCorrespondences removed in MRPT 2.0.
	matchParams.onlyKeepTheClosest = true;
	matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
	matchParams.decimation_other_map_points =
		options.corresponding_points_decimation;

	// Ensure maps are not empty!
	// ------------------------------------------------------
	if (!m2->isEmpty())
	{
		matchParams.offset_other_map_points = 0;

		// ------------------------------------------------------
		//					The ICP loop
		// ------------------------------------------------------
		do
		{
			// ------------------------------------------------------
			//		Find the matching (for a points map)
			// ------------------------------------------------------
			// Pivot point for angular measurements
			matchParams.angularDistPivotPoint = gaussPdf->mean.translation();

			m1->determineMatching2D(
				m2,	 // The other map
				gaussPdf->mean,	 // The other map pose
				correspondences, matchParams, matchExtraResults);

			nCorrespondences = correspondences.size();

			// ***DEBUG***
			//				correspondences.dumpToFile("debug_correspondences.txt");

			if (!nCorrespondences)
			{
				// Nothing we can do !!
				keepApproaching = false;
			}
			else
			{
				// Compute the estimated pose.
				//  (Method from paper of J.Gonzalez, Martinez y Morales)
				// ----------------------------------------------------------------------
				mrpt::math::TPose2D est_mean;
				mrpt::tfest::se2_l2(correspondences, est_mean);

				gaussPdf->mean = mrpt::poses::CPose2D(est_mean);

				// If matching has not changed, decrease the thresholds:
				// --------------------------------------------------------
				keepApproaching = true;
				if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
						  options.minAbsStep_trans ||
					  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
						  options.minAbsStep_trans ||
					  fabs(math::wrapToPi(
						  lastMeanPose.phi() - gaussPdf->mean.phi())) >
						  options.minAbsStep_rot))
				{
					matchParams.maxDistForCorrespondence *= options.ALFA;
					matchParams.maxAngularDistForCorrespondence *= options.ALFA;
					if (matchParams.maxDistForCorrespondence <
						options.smallestThresholdDist)
						keepApproaching = false;

					if (++matchParams.offset_other_map_points >=
						options.corresponding_points_decimation)
						matchParams.offset_other_map_points = 0;
				}

				lastMeanPose = gaussPdf->mean;

			}  // end of "else, there are correspondences"

			// Next iteration:
			outInfo.nIterations++;

			if (outInfo.nIterations >= options.maxIterations &&
				matchParams.maxDistForCorrespondence >
					options.smallestThresholdDist)
			{
				matchParams.maxDistForCorrespondence *= options.ALFA;
			}

		} while (
			(keepApproaching && outInfo.nIterations < options.maxIterations) ||
			(outInfo.nIterations >= options.maxIterations &&
			 matchParams.maxDistForCorrespondence >
				 options.smallestThresholdDist));

		// -------------------------------------------------
		//   Obtain the covariance matrix of the estimation
		// -------------------------------------------------
		if (!options.skip_cov_calculation && nCorrespondences)
		{
			switch (options.ICP_covariance_method)
			{
				// ----------------------------------------------
				// METHOD: MSE linear estimation
				// ----------------------------------------------
				case icpCovLinealMSE:
					mrpt::tfest::se2_l2(correspondences, *gaussPdf);
					// Scale covariance:
					gaussPdf->cov *= options.covariance_varPoints;
					break;

				// ----------------------------------------------
				// METHOD: Method from Oxford MRG's "OXSMV2"
				//
				//  It is the equivalent covariance resulting
				//   from a Levenberg-Maquardt optimization stage.
				// ----------------------------------------------
				case icpCovFiniteDifferences:
				{
					CMatrixDouble D(3, nCorrespondences);

					const TPose2D transf = gaussPdf->mean.asTPose();

					double ccos = cos(transf.phi);
					double csin = sin(transf.phi);

					double w1, w2, w3;
					double q1, q2, q3;
					double A, B;
					double Axy = 0.01;

					// Fill out D:
					double rho2 = square(options.kernel_rho);
					mrpt::tfest::TMatchingPairList::iterator it;
					size_t i;
					for (i = 0, it = correspondences.begin();
						 i < nCorrespondences; ++i, ++it)
					{
						float other_x_trans =
							transf.x + ccos * it->local.x - csin * it->local.y;
						float other_y_trans =
							transf.y + csin * it->local.x + ccos * it->local.y;

						// Jacobian: dR2_dx
						// --------------------------------------
						w1 = other_x_trans - Axy;
						q1 = kernel(
							square(it->global.x - w1) +
								square(it->global.y - other_y_trans),
							rho2);

						w2 = other_x_trans;
						q2 = kernel(
							square(it->global.x - w2) +
								square(it->global.y - other_y_trans),
							rho2);

						w3 = other_x_trans + Axy;
						q3 = kernel(
							square(it->global.x - w3) +
								square(it->global.y - other_y_trans),
							rho2);

						// interpolate
						A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
							((q1 - q2) / ((w1 - w2) * (w3 - w1)));
						B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
							(w1 - w2);

						D(0, i) = (2 * A * other_x_trans) + B;

						// Jacobian: dR2_dy
						// --------------------------------------
						w1 = other_y_trans - Axy;
						q1 = kernel(
							square(it->global.x - other_x_trans) +
								square(it->global.y - w1),
							rho2);

						w2 = other_y_trans;
						q2 = kernel(
							square(it->global.x - other_x_trans) +
								square(it->global.y - w2),
							rho2);

						w3 = other_y_trans + Axy;
						q3 = kernel(
							square(it->global.x - other_x_trans) +
								square(it->global.y - w3),
							rho2);

						// interpolate
						A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
							((q1 - q2) / ((w1 - w2) * (w3 - w1)));
						B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) /
							(w1 - w2);

						D(1, i) = (2 * A * other_y_trans) + B;

						// Jacobian: dR_dphi
						// --------------------------------------
						D(2, i) = D(0, i) *
								(-csin * it->local.x - ccos * it->local.y) +
							D(1, i) * (ccos * it->local.x - csin * it->local.y);

					}  // end for each corresp.

					// COV = ( D*D^T + lamba*I )^-1
					CMatrixDouble33 DDt;
					DDt.matProductOf_AAt(D);

					for (i = 0; i < 3; i++)
						DDt(i, i) += 1e-6;	// Just to make sure the matrix is
					// not singular, while not changing
					// its covariance significantly.

					gaussPdf->cov = DDt.inverse_LLt();
				}
				break;
				default:
					THROW_EXCEPTION_FMT(
						"Invalid value for ICP_covariance_method: %i",
						static_cast<int>(options.ICP_covariance_method));
			}
		}

		outInfo.goodness = matchExtraResults.correspondencesRatio;

		if (!nCorrespondences || options.skip_quality_calculation)
		{
			outInfo.quality = matchExtraResults.correspondencesRatio;
		}
		else
		{
			// Compute a crude estimate of the quality of scan matching at this
			// local minimum:
			// -----------------------------------------------------------------------------------
			float Axy = matchParams.maxDistForCorrespondence * 0.9f;

			CPose2D P0(gaussPdf->mean);
			CPose2D PX1(P0);
			PX1.x_incr(-Axy);
			CPose2D PX2(P0);
			PX2.x_incr(+Axy);
			CPose2D PY1(P0);
			PY1.y_incr(-Axy);
			CPose2D PY2(P0);
			PY2.y_incr(+Axy);

			// Pivot point for angular measurements
			matchParams.angularDistPivotPoint = gaussPdf->mean.translation();

			m1->determineMatching2D(
				m2,	 // The other map
				P0,	 // The other map pose
				correspondences, matchParams, matchExtraResults);
			const float E0 = matchExtraResults.correspondencesRatio;

			m1->determineMatching2D(
				m2,	 // The other map
				PX1,  // The other map pose
				correspondences, matchParams, matchExtraResults);
			const float EX1 = matchExtraResults.correspondencesRatio;

			m1->determineMatching2D(
				m2,	 // The other map
				PX2,  // The other map pose
				correspondences, matchParams, matchExtraResults);
			const float EX2 = matchExtraResults.correspondencesRatio;

			m1->determineMatching2D(
				m2,	 // The other map
				PY1,  // The other map pose
				correspondences, matchParams, matchExtraResults);
			const float EY1 = matchExtraResults.correspondencesRatio;
			m1->determineMatching2D(
				m2,	 // The other map
				PY2,  // The other map pose
				correspondences, matchParams, matchExtraResults);
			const float EY2 = matchExtraResults.correspondencesRatio;

			outInfo.quality =
				-max(EX1 - E0, max(EX2 - E0, max(EY1 - E0, EY2 - E0))) /
				(E0 + 1e-1);
		}

	}  // end of "if m2 is not empty"

	// We'll return a CPosePDFGaussian or a CPosePDFSOG if RANSAC is enabled:
	// -----------------------------------------------------------------------

	// RANSAC?
	if (options.doRANSAC)
	{
		mrpt::tfest::TSE2RobustParams params;
		params.ransac_minSetSize = options.ransac_minSetSize;
		params.ransac_maxSetSize = options.ransac_maxSetSize;
		params.ransac_mahalanobisDistanceThreshold =
			options.ransac_mahalanobisDistanceThreshold;
		params.ransac_nSimulations = options.ransac_nSimulations;
		params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
		params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
		params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
		params.ransac_algorithmForLandmarks = false;

		mrpt::tfest::TSE2RobustResult results;
		mrpt::tfest::se2_l2_robust(
			correspondences, options.normalizationStd, params, results);

		SOG = std::make_shared<CPosePDFSOG>();
		*SOG = results.transformation;

		// And return the SOG:
		resultPDF = SOG;
	}
	else
	{
		// Return the gaussian distribution:
		resultPDF = gaussPdf;
	}

	return resultPDF;

	MRPT_END
}

/*----------------------------------------------------------------------------

						ICP_Method_LM

  ----------------------------------------------------------------------------*/
CPosePDF::Ptr CICP::ICP_Method_LM(
	const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* m2,
	const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
{
	MRPT_START

	// The result can be either a Gaussian or a SOG:
	size_t nCorrespondences = 0;
	bool keepIteratingICP;
	CPose2D grossEst = initialEstimationPDF.mean;
	TMatchingPairList correspondences, old_correspondences;
	CPose2D lastMeanPose;
	std::vector<float> other_xs_trans, other_ys_trans;	// temporary container
	// of "other" map (map2)
	// transformed by "q"
	CMatrixFloat dJ_dq;	 // The jacobian
	CPose2D q;	// The updated 2D transformation estimate
	CPose2D q_new;

	const bool onlyUniqueRobust = options.onlyUniqueRobust;

	// Assure the class of the maps:
	ASSERT_(mm1->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
	const auto* m1 = dynamic_cast<const CPointsMap*>(mm1);

	// Asserts:
	// -----------------
	ASSERT_(options.ALFA > 0 && options.ALFA < 1);

	// The algorithm output auxiliar info:
	// -------------------------------------------------
	outInfo.nIterations = 0;
	outInfo.goodness = 1.0f;

	TMatchingParams matchParams;
	TMatchingExtraResults matchExtraResults;

	matchParams.maxDistForCorrespondence =
		options.thresholdDist;	// Distance threshold
	matchParams.maxAngularDistForCorrespondence =
		options.thresholdAng;  // Angular threshold
	matchParams.onlyKeepTheClosest = true;
	matchParams.onlyUniqueRobust = onlyUniqueRobust;
	matchParams.decimation_other_map_points =
		options.corresponding_points_decimation;

	// The gaussian PDF to estimate:
	// ------------------------------------------------------
	// First gross approximation:
	q = grossEst;

	// For LM inverse
	CMatrixFixed<float, 3, 3> C;
	CMatrixFixed<float, 3, 3>
		C_inv;	// This will keep the cov. matrix at the end

	// Ensure maps are not empty!
	// ------------------------------------------------------
	if (!m2->isEmpty())
	{
		matchParams.offset_other_map_points = 0;
		// ------------------------------------------------------
		//					The ICP loop
		// ------------------------------------------------------
		do
		{
			// ------------------------------------------------------
			//		Find the matching (for a points map)
			// ------------------------------------------------------
			// Pivot point for angular measurements:
			matchParams.angularDistPivotPoint = q.translation();

			m1->determineMatching2D(
				m2,	 // The other map
				q,	// The other map pose
				correspondences, matchParams, matchExtraResults);

			nCorrespondences = correspondences.size();

			if (!nCorrespondences)
			{
				// Nothing we can do !!
				keepIteratingICP = false;
			}
			else
			{
				// Compute the estimated pose through iterative least-squares:
				// Levenberg-Marquardt
				// ----------------------------------------------------------------------
				dJ_dq.setSize(3, nCorrespondences);	 // The jacobian of the
				// error function wrt the
				// transformation q

				double lambda = options.LM_initial_lambda;	// The LM parameter

				double ccos = cos(q.phi());
				double csin = sin(q.phi());

				double w1, w2, w3;
				double q1, q2, q3;
				double A, B;
				const double Axy =
					options.Axy_aprox_derivatives;	// For approximating the
				// derivatives

				// Compute at once the square errors for each point with the
				// current "q" and the transformed points:
				std::vector<float> sq_errors;
				correspondences.squareErrorVector(
					q, sq_errors, other_xs_trans, other_ys_trans);
				double OSE_initial = math::sum(sq_errors);

				// Compute "dJ_dq"
				// ------------------------------------
				double rho2 = square(options.kernel_rho);
				mrpt::tfest::TMatchingPairList::iterator it;
				std::vector<float>::const_iterator other_x_trans, other_y_trans;
				size_t i;

				for (i = 0, it = correspondences.begin(),
					other_x_trans = other_xs_trans.begin(),
					other_y_trans = other_ys_trans.begin();
					 i < nCorrespondences;
					 ++i, ++it, ++other_x_trans, ++other_y_trans)
				{
					// Jacobian: dJ_dx
					// --------------------------------------
					//#define ICP_DISTANCES_TO_LINE

#ifndef ICP_DISTANCES_TO_LINE
					w1 = *other_x_trans - Axy;
					q1 = m1->squareDistanceToClosestCorrespondence(
						w1, *other_y_trans);
					q1 = kernel(q1, rho2);

					w2 = *other_x_trans;
					q2 = m1->squareDistanceToClosestCorrespondence(
						w2, *other_y_trans);
					q2 = kernel(q2, rho2);

					w3 = *other_x_trans + Axy;
					q3 = m1->squareDistanceToClosestCorrespondence(
						w3, *other_y_trans);
					q3 = kernel(q3, rho2);
#else
					// The distance to the line that interpolates the TWO
					// closest points:
					float x1, y1, x2, y2, d1, d2;
					m1->kdTreeTwoClosestPoint2D(
						*other_x_trans, *other_y_trans,	 // The query
						x1, y1,	 // Closest point #1
						x2, y2,	 // Closest point #2
						d1, d2);

					w1 = *other_x_trans - Axy;
					q1 = math::squaredDistancePointToLine(
						w1, *other_y_trans, x1, y1, x2, y2);
					q1 = kernel(q1, rho2);

					w2 = *other_x_trans;
					q2 = math::squaredDistancePointToLine(
						w2, *other_y_trans, x1, y1, x2, y2);
					q2 = kernel(q2, rho2);

					w3 = *other_x_trans + Axy;
					q3 = math::squaredDistancePointToLine(
						w3, *other_y_trans, x1, y1, x2, y2);
					q3 = kernel(q3, rho2);
#endif
					// interpolate
					A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
						((q1 - q2) / ((w1 - w2) * (w3 - w1)));
					B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);

					dJ_dq(0, i) = (2 * A * *other_x_trans) + B;

					// Jacobian: dJ_dy
					// --------------------------------------
					w1 = *other_y_trans - Axy;
#ifdef ICP_DISTANCES_TO_LINE
					q1 = math::squaredDistancePointToLine(
						*other_x_trans, w1, x1, y1, x2, y2);
					q1 = kernel(q1, rho2);
#else
					q1 = kernel(
						square(it->global.x - *other_x_trans) +
							square(it->global.y - w1),
						rho2);
#endif

					w2 = *other_y_trans;
					// q2 is alreay computed from above!
					// q2 = m1->squareDistanceToClosestCorrespondence(
					// *other_x_trans, w2 );
					// q2= kernel( square(it->global.x - *other_x_trans)+
					// square( it->global.y - w2 ),  rho2 );

					w3 = *other_y_trans + Axy;
#ifdef ICP_DISTANCES_TO_LINE
					q3 = math::squaredDistancePointToLine(
						*other_x_trans, w3, x1, y1, x2, y2);
					q3 = kernel(q3, rho2);
#else
					q3 = kernel(
						square(it->global.x - *other_x_trans) +
							square(it->global.y - w3),
						rho2);
#endif

					// interpolate
					A = ((q3 - q2) / ((w3 - w2) * (w3 - w1))) -
						((q1 - q2) / ((w1 - w2) * (w3 - w1)));
					B = ((q1 - q2) + (A * ((w2 * w2) - (w1 * w1)))) / (w1 - w2);

					dJ_dq(1, i) = (2 * A * *other_y_trans) + B;

					// Jacobian: dR_dphi
					// --------------------------------------
					dJ_dq(2, i) = dJ_dq(0, i) *
							(-csin * it->local.x - ccos * it->local.y) +
						dJ_dq(1, i) * (ccos * it->local.x - csin * it->local.y);

				}  // end for each corresp.

				// Now we have the Jacobian in dJ_dq.

				// Compute the Hessian matrix H = dJ_dq * dJ_dq^T
				CMatrixFloat H_(3, 3);
				H_.matProductOf_AAt(dJ_dq);

				CMatrixFixed<float, 3, 3> H;
				H = H_;

				bool keepIteratingLM = true;

				// ---------------------------------------------------
				// Iterate the inner LM loop until convergence:
				// ---------------------------------------------------
				q_new = q;

				std::vector<float> new_sq_errors, new_other_xs_trans,
					new_other_ys_trans;
				size_t nLMiters = 0;
				const size_t maxLMiters = 100;

				while (keepIteratingLM && ++nLMiters < maxLMiters)
				{
					// The LM heuristic is:
					//  x_{k+1} = x_k  - ( H + \lambda diag(H) )^-1 * grad(J)
					//  grad(J) = dJ_dq * e (vector of errors)
					C = H;
					for (i = 0; i < 3; i++)
						C(i, i) *=
							(1 + lambda);  // Levenberg-Maquardt heuristic

					C_inv = C.inverse_LLt();

					// LM_delta = C_inv * dJ_dq * sq_errors
					const Eigen::Vector3f LM_delta =
						(C_inv.asEigen() * dJ_dq.asEigen() *
						 Eigen::Map<Eigen::VectorXf>(
							 &sq_errors[0], sq_errors.size()))
							.eval();

					q_new.x(q.x() - LM_delta[0]);
					q_new.y(q.y() - LM_delta[1]);
					q_new.phi(q.phi() - LM_delta[2]);

					// Compute the new square errors:
					// ---------------------------------------
					correspondences.squareErrorVector(
						q_new, new_sq_errors, new_other_xs_trans,
						new_other_ys_trans);

					float OSE_new = math::sum(new_sq_errors);

					bool improved = OSE_new < OSE_initial;

					keepIteratingLM =
						fabs(LM_delta[0]) > options.minAbsStep_trans ||
						fabs(LM_delta[1]) > options.minAbsStep_trans ||
						fabs(LM_delta[2]) > options.minAbsStep_rot;

					if (improved)
					{
						// If resids have gone down, keep change and make lambda
						// smaller by factor of 10
						lambda /= 10;
						q = q_new;
						OSE_initial = OSE_new;
					}
					else
					{
						// Discard movement and try with larger lambda:
						lambda *= 10;
					}

				}  // end iterative LM

#if 0  // Debuggin'
				cout << "ICP loop: " << lastMeanPose  << " -> " << q << " LM iters: " << nLMiters  << " threshold: " << matchParams.maxDistForCorrespondence << endl;
#endif
				// --------------------------------------------------------
				// now the conditions for the outer ICP loop
				// --------------------------------------------------------
				keepIteratingICP = true;
				if (fabs(lastMeanPose.x() - q.x()) < options.minAbsStep_trans &&
					fabs(lastMeanPose.y() - q.y()) < options.minAbsStep_trans &&
					fabs(math::wrapToPi(lastMeanPose.phi() - q.phi())) <
						options.minAbsStep_rot)
				{
					matchParams.maxDistForCorrespondence *= options.ALFA;
					matchParams.maxAngularDistForCorrespondence *= options.ALFA;
					if (matchParams.maxDistForCorrespondence <
						options.smallestThresholdDist)
						keepIteratingICP = false;

					if (++matchParams.offset_other_map_points >=
						options.corresponding_points_decimation)
						matchParams.offset_other_map_points = 0;
				}
				lastMeanPose = q;
			}  // end of "else, there are correspondences"

			// Next iteration:
			outInfo.nIterations++;

			if (outInfo.nIterations >= options.maxIterations &&
				matchParams.maxDistForCorrespondence >
					options.smallestThresholdDist)
			{
				matchParams.maxDistForCorrespondence *= options.ALFA;
			}

		} while (
			(keepIteratingICP && outInfo.nIterations < options.maxIterations) ||
			(outInfo.nIterations >= options.maxIterations &&
			 matchParams.maxDistForCorrespondence >
				 options.smallestThresholdDist));

		outInfo.goodness = matchExtraResults.correspondencesRatio;

		// if (!options.skip_quality_calculation)
		{
			outInfo.quality = matchExtraResults.correspondencesRatio;
		}

	}  // end of "if m2 is not empty"

	return CPosePDFGaussian::Create(q, C_inv.cast_double());
	MRPT_END
}

CPose3DPDF::Ptr CICP::Align3DPDF(
	const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
	const CPose3DPDFGaussian& initialEstimationPDF,
	mrpt::optional_ref<TMetricMapAlignmentResult> outInfo)
{
	MRPT_START

	static CTicTac tictac;
	TReturnInfo outInfoVal;
	CPose3DPDF::Ptr resultPDF;

	if (outInfo) tictac.Tic();

	switch (options.ICP_algorithm)
	{
		case icpClassic:
			resultPDF =
				ICP3D_Method_Classic(m1, mm2, initialEstimationPDF, outInfoVal);
			break;
		case icpLevenbergMarquardt:
			THROW_EXCEPTION("Only icpClassic is implemented for ICP-3D");
			break;
		default:
			THROW_EXCEPTION_FMT(
				"Invalid value for ICP_algorithm: %i",
				static_cast<int>(options.ICP_algorithm));
	}  // end switch

	if (outInfo) outInfoVal.executionTime = tictac.Tac();

	// Copy the output info if requested:
	if (outInfo)
		if (auto* o = dynamic_cast<TReturnInfo*>(&outInfo.value().get()); o)
			*o = outInfoVal;

	return resultPDF;

	MRPT_END
}

CPose3DPDF::Ptr CICP::ICP3D_Method_Classic(
	const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* mm2,
	const CPose3DPDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
{
	MRPT_START

	// The result can be either a Gaussian or a SOG:
	CPose3DPDF::Ptr resultPDF;
	CPose3DPDFGaussian::Ptr gaussPdf;

	size_t nCorrespondences = 0;
	bool keepApproaching;
	CPose3D grossEst = initialEstimationPDF.mean;
	mrpt::tfest::TMatchingPairList correspondences, old_correspondences;
	CPose3D lastMeanPose;

	// Assure the class of the maps:
	ASSERT_(mm2->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
	const CPointsMap* m2 = (CPointsMap*)mm2;

	// Asserts:
	// -----------------
	ASSERT_(options.ALFA > 0 && options.ALFA < 1);

	// The algorithm output auxiliar info:
	// -------------------------------------------------
	outInfo.nIterations = 0;
	outInfo.goodness = 1;
	outInfo.quality = 0;

	// The gaussian PDF to estimate:
	// ------------------------------------------------------
	gaussPdf = std::make_shared<CPose3DPDFGaussian>();

	// First gross approximation:
	gaussPdf->mean = grossEst;

	// Initial thresholds:
	TMatchingParams matchParams;
	TMatchingExtraResults matchExtraResults;

	matchParams.maxDistForCorrespondence =
		options.thresholdDist;	// Distance threshold
	matchParams.maxAngularDistForCorrespondence =
		options.thresholdAng;  // Angular threshold
	matchParams.onlyKeepTheClosest = true;
	matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
	matchParams.decimation_other_map_points =
		options.corresponding_points_decimation;

	// Ensure maps are not empty!
	// ------------------------------------------------------
	if (!m2->isEmpty())
	{
		matchParams.offset_other_map_points = 0;

		// ------------------------------------------------------
		//					The ICP loop
		// ------------------------------------------------------
		do
		{
			// Pivot point for angular measurements:
			matchParams.angularDistPivotPoint = gaussPdf->mean.translation();

			// ------------------------------------------------------
			//		Find the matching (for a points map)
			// ------------------------------------------------------
			m1->determineMatching3D(
				m2,	 // The other map
				gaussPdf->mean,	 // The other map pose
				correspondences, matchParams, matchExtraResults);

			nCorrespondences = correspondences.size();

			if (!nCorrespondences)
			{
				// Nothing we can do !!
				keepApproaching = false;
			}
			else
			{
				// Compute the estimated pose, using Horn's method.
				// ----------------------------------------------------------------------
				mrpt::poses::CPose3DQuat estPoseQuat;
				double transf_scale;
				mrpt::tfest::se3_l2(
					correspondences, estPoseQuat, transf_scale,
					false /* dont force unit scale */);
				gaussPdf->mean = mrpt::poses::CPose3D(estPoseQuat);

				// If matching has not changed, decrease the thresholds:
				// --------------------------------------------------------
				keepApproaching = true;
				if (!(fabs(lastMeanPose.x() - gaussPdf->mean.x()) >
						  options.minAbsStep_trans ||
					  fabs(lastMeanPose.y() - gaussPdf->mean.y()) >
						  options.minAbsStep_trans ||
					  fabs(lastMeanPose.z() - gaussPdf->mean.z()) >
						  options.minAbsStep_trans ||
					  fabs(math::wrapToPi(
						  lastMeanPose.yaw() - gaussPdf->mean.yaw())) >
						  options.minAbsStep_rot ||
					  fabs(math::wrapToPi(
						  lastMeanPose.pitch() - gaussPdf->mean.pitch())) >
						  options.minAbsStep_rot ||
					  fabs(math::wrapToPi(
						  lastMeanPose.roll() - gaussPdf->mean.roll())) >
						  options.minAbsStep_rot))
				{
					matchParams.maxDistForCorrespondence *= options.ALFA;
					matchParams.maxAngularDistForCorrespondence *= options.ALFA;
					if (matchParams.maxDistForCorrespondence <
						options.smallestThresholdDist)
						keepApproaching = false;

					if (++matchParams.offset_other_map_points >=
						options.corresponding_points_decimation)
						matchParams.offset_other_map_points = 0;
				}

				lastMeanPose = gaussPdf->mean;

			}  // end of "else, there are correspondences"

			// Next iteration:
			outInfo.nIterations++;

			if (outInfo.nIterations >= options.maxIterations &&
				matchParams.maxDistForCorrespondence >
					options.smallestThresholdDist)
			{
				matchParams.maxDistForCorrespondence *= options.ALFA;
			}

		} while (
			(keepApproaching && outInfo.nIterations < options.maxIterations) ||
			(outInfo.nIterations >= options.maxIterations &&
			 matchParams.maxDistForCorrespondence >
				 options.smallestThresholdDist));

		// -------------------------------------------------
		//   Obtain the covariance matrix of the estimation
		// -------------------------------------------------
		if (!options.skip_cov_calculation && nCorrespondences)
		{
			// ...
		}

		//
		outInfo.goodness = matchExtraResults.correspondencesRatio;

	}  // end of "if m2 is not empty"

	// Return the gaussian distribution:
	resultPDF = gaussPdf;

	return resultPDF;

	MRPT_END
}
