/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/math_frwds.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/tfest/TMatchingPair.h>
#include <mrpt/tfest/indiv-compat-decls.h>

namespace mrpt::tfest
{
/** \addtogroup mrpt_tfest_grp
 * @{ */

/** Least-squares (L2 norm) solution to finding the optimal SE(3) transform
 * between two reference frames using the "quaternion" or Horn's method
 * \cite horn1987closed
 *
 *  The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
 * \f$, that is, the
 *  transformation of frame `other` with respect to `this`.
 *
 *  \image html tfest_frames.png
 *
 * \param[in]  in_correspondences  The coordinates of the input points for the
 * two coordinate systems "this" and "other"
 * \param[out] out_transform       The output transformation
 * \param[out] out_scale           The computed scale of the optimal
 * transformation (will be 1.0 for a perfectly rigid translation + rotation).
 * \param[in]  forceScaleToUnity   Whether or not force the scale employed to
 * rotate the coordinate systems to one (rigid transformation)
 * \note [New in MRPT 1.3.0] This function replaces
 * mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC() and
 * mrpt::scanmatching::HornMethod()
 * \sa se2_l2, se3_l2_robust
 */
bool se3_l2(
	const mrpt::tfest::TMatchingPairList& in_correspondences,
	mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
	bool forceScaleToUnity = false);

/// \overload (for double precision points)
bool se3_l2(
	const mrpt::tfest::TMatchingPairList_d& in_correspondences,
	mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
	bool forceScaleToUnity = false);

/** \overload
 *
 * This version accepts corresponding points as two vectors of TPoint3D (must
 * have identical length).
 */
bool se3_l2(
	const std::vector<mrpt::math::TPoint3D>& in_points_this,
	const std::vector<mrpt::math::TPoint3D>& in_points_other,
	mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
	bool forceScaleToUnity = false);

/** Parameters for se3_l2_robust(). See function for more details */
struct TSE3RobustParams
{
	/** (Default=5)  The minimum amount of points in a set to start a consensus
	 * set. \sa ransac_maxSetSizePct */
	unsigned int ransac_minSetSize{5};
	/** (Default=50) The maximum number of iterations of the RANSAC algorithm */
	unsigned int ransac_nmaxSimulations{50};
	/** (Default=0.5) The minimum ratio (0.0 - 1.0) of the input set that is
	 * considered to be inliers. *Important*: The minimum size of a consensus
	 * set to be accepted will be "INPUT_CORRESPONDENCES*ransac_maxSetSizePct".
	 */
	double ransac_maxSetSizePct{0.5};
	/** (Default=0.05) The maximum distance in X,Y,Z for a solution to be
	 * considered as matching a candidate solution (In meters) */
	double ransac_threshold_lin{0.05};
	/** (Default=1 deg) The maximum angle (yaw,pitch,roll) for a solution to be
	 * considered as matching a candidate solution (In radians) */
	double ransac_threshold_ang{mrpt::DEG2RAD(1.)};
	/** (Default=0.03) The maximum difference in scale for a solution to be
	 * considered as matching a candidate solution (dimensionless) */
	double ransac_threshold_scale{0.03};
	/** (Default=true)  */
	bool forceScaleToUnity{true};
	/** (Default=false) */
	bool verbose{false};

	/** If provided, this user callback will be invoked to determine the
	 * individual compatibility between each potential pair
	 * of elements. Can check image descriptors, geometrical properties, etc.
	 * \return Must return true if the pair is a potential match, false
	 * otherwise.
	 */
	TFunctorCheckPotentialMatch user_individual_compat_callback;
};

/** Output placeholder for se3_l2_robust() */
struct TSE3RobustResult
{
	/** The best transformation found */
	mrpt::poses::CPose3DQuat transformation;
	/** The estimated scale of the rigid transformation (should be very close to
	 * 1.0) */
	double scale{.0};
	/** Indexes within the `in_correspondences` list which corresponds with
	 * inliers */
	std::vector<uint32_t> inliers_idx;
};

/** Least-squares (L2 norm) solution to finding the optimal SE(3) transform
 * between two reference frames using RANSAC and the "quaternion" or Horn's
 * method \cite horn1987closed
 *
 *  The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
 * \f$, that is, the
 *  transformation of frame `other` with respect to `this`.
 *
 *  \image html tfest_frames.png
 *
 * \param[in] in_correspondences The set of correspondences.
 * \param[in] in_params Method parameters (see docs for TSE3RobustParams)
 * \param[out] out_results Results: transformation, scale, etc.
 *
 * \return True if the minimum number of correspondences was found, false
 * otherwise.
 * \note Implemented by FAMD, 2008. Re-factored by JLBC, 2015.
 * \note [New in MRPT 1.3.0] This function replaces
 * mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC()
 * \sa se2_l2, se3_l2
 */
bool se3_l2_robust(
	const mrpt::tfest::TMatchingPairList& in_correspondences,
	const TSE3RobustParams& in_params, TSE3RobustResult& out_results);

/** @} */  // end of grouping
}  // namespace mrpt::tfest
