GrabBag/Module/HandEyeCalib/Inc/IHandEyeCalib.h

#ifndef IHANDEYECALIB_H
#define IHANDEYECALIB_H

#include <vector>
#include "HandEyeCalib_global.h"
#include "HandEyeCalibTypes.h"

/**
 * @brief 手眼标定接口类
 * 提供基于SVD分解的手眼标定算法，使用Eigen库实现
 * 不依赖OpenCV
 */
class IHandEyeCalib
{
public:
    virtual ~IHandEyeCalib() = default;

    /**
     * @brief 计算旋转平移矩阵 (从眼坐标系到机器人坐标系)
     * 使用SVD分解方法计算两组对应点之间的刚体变换
     * @param eyePoints 眼坐标系（相机坐标系）下的点集
     * @param robotPoints 机器人坐标系下的点集
     * @param result 输出的标定结果 (R, T, 质心等)
     * @return 成功返回0，失败返回错误码
     */
    virtual int CalculateRT(
        const std::vector<HECPoint3D>& eyePoints,
        const std::vector<HECPoint3D>& robotPoints,
        HECCalibResult& result) = 0;

    /**
     * @brief 计算旋转平移矩阵 (使用点对数组)
     * @param pointPairs 对应点对数组
     * @param result 输出的标定结果
     * @return 成功返回0，失败返回错误码
     */
    virtual int CalculateRT(
        const std::vector<HECCalibPointPair>& pointPairs,
        HECCalibResult& result) = 0;

    /**
     * @brief 使用RT矩阵变换点 (完整变换: R*p + T)
     * @param R 旋转矩阵
     * @param T 平移向量
     * @param srcPoint 源点
     * @param dstPoint 输出的变换后的点
     */
    virtual void TransformPoint(
        const HECRotationMatrix& R,
        const HECTranslationVector& T,
        const HECPoint3D& srcPoint,
        HECPoint3D& dstPoint) = 0;

    /**
     * @brief 使用RT矩阵变换点 (基于质心的变换)
     * 变换公式: dstPoint = R * (srcPoint - srcCenter) + dstCenter
     * @param R 旋转矩阵
     * @param T 平移向量
     * @param srcCenter 源坐标系质心
     * @param dstCenter 目标坐标系质心
     * @param srcPoint 源点
     * @param dstPoint 输出的变换后的点
     */
    virtual void TransformPointWithCenter(
        const HECRotationMatrix& R,
        const HECTranslationVector& T,
        const HECPoint3D& srcCenter,
        const HECPoint3D& dstCenter,
        const HECPoint3D& srcPoint,
        HECPoint3D& dstPoint) = 0;

    /**
     * @brief 仅使用旋转矩阵变换点 (不含平移)
     * @param R 旋转矩阵
     * @param srcPoint 源点
     * @param dstPoint 输出的变换后的点
     */
    virtual void RotatePoint(
        const HECRotationMatrix& R,
        const HECPoint3D& srcPoint,
        HECPoint3D& dstPoint) = 0;

    /**
     * @brief 从旋转矩阵计算欧拉角 (ZYX顺序，保持向后兼容)
     * @param R 旋转矩阵
     * @param angles 输出的欧拉角 (弧度制)
     */
    virtual void RotationMatrixToEulerZYX(
        const HECRotationMatrix& R,
        HECEulerAngles& angles) = 0;

    /**
     * @brief 从旋转矩阵计算欧拉角 (支持多种旋转顺序)
     * @param R 旋转矩阵
     * @param order 欧拉角旋转顺序
     * @param angles 输出的欧拉角 (弧度制)
     */
    virtual void RotationMatrixToEuler(
        const HECRotationMatrix& R,
        HECEulerOrder order,
        HECEulerAngles& angles) = 0;

    /**
     * @brief 从欧拉角计算旋转矩阵 (ZYX顺序，保持向后兼容)
     * @param angles 欧拉角 (弧度制)
     * @param R 输出的旋转矩阵
     */
    virtual void EulerZYXToRotationMatrix(
        const HECEulerAngles& angles,
        HECRotationMatrix& R) = 0;

    /**
     * @brief 从欧拉角计算旋转矩阵 (支持多种旋转顺序)
     * @param angles 欧拉角 (弧度制)
     * @param order 欧拉角旋转顺序
     * @param R 输出的旋转矩阵
     */
    virtual void EulerToRotationMatrix(
        const HECEulerAngles& angles,
        HECEulerOrder order,
        HECRotationMatrix& R) = 0;

    /**
     * @brief 计算完整的姿态转换 (位置 + 姿态)
     * 将眼坐标系下的位姿转换到机器人坐标系
     * @param calibResult 标定结果
     * @param eyePoint 眼坐标系下的点
     * @param eyeDirVectors 眼坐标系下的方向向量 (X, Y, Z轴方向)
     * @param invertYZ 是否对Y轴和Z轴方向取反 (坐标系方向调整)
     * @param poseResult 输出的机器人坐标系下的位姿
     */
    virtual void TransformPose(
        const HECCalibResult& calibResult,
        const HECPoint3D& eyePoint,
        const std::vector<HECPoint3D>& eyeDirVectors,
        bool invertYZ,
        HECPoseResult& poseResult) = 0;

    /**
     * @brief Convert eye-frame center + XZ seed directions to robot-frame pose.
     *
     * Builds a right-handed frame from xSeed (X axis) and zSeed (Z axis),
     * applies the dirVectorInvert flip, transforms position and orientation through
     * the hand-eye calibration, and returns the result in degrees.
     *
     * @param calibResult     Hand-eye calibration result (R, T)
     * @param eyeCenter       Object center in eye (camera) frame
     * @param xSeed           X-axis seed direction in eye frame
     * @param zSeed           Z-axis seed direction in eye frame
     * @param dirVectorInvert Axis-flip mode: 0=none, 1=flip XY, 2=flip XZ, 3=flip YZ
     * @param eulerOrder      Euler angle decomposition order
     * @param poseResult      Output robot-frame position (mm) and Euler angles (degrees)
     * @return true if the seed vectors form a valid frame, false otherwise
     */
    virtual bool TransformPose(
        const HECCalibResult& calibResult,
        const HECPoint3D& eyeCenter,
        const HECPoint3D& longAxisDir,
        const HECPoint3D& normalDir,
        int dirVectorInvert,
        HECEulerOrder eulerOrder,
        HECLongAxisDir longAxisMapping,
        HECPoseResult& poseResult) = 0;

    /**
     * @brief 计算标定误差
     * @param eyePoints 眼坐标系下的点集
     * @param robotPoints 机器人坐标系下的点集
     * @param calibResult 标定结果
     * @return 平均误差 (mm)
     */
    virtual double CalculateError(
        const std::vector<HECPoint3D>& eyePoints,
        const std::vector<HECPoint3D>& robotPoints,
        const HECCalibResult& calibResult) = 0;

    /**
     * @brief 眼在手上标定 (Eye-In-Hand)
     * 相机安装在机器人末端，求相机坐标系到末端坐标系的变换
     *
     * 原理：对于固定的标定点P_base（在基座坐标系下），有：
     *   P_base = T_end * T_cam * P_cam
     * 其中 T_end 是末端位姿，T_cam 是待求的相机到末端的变换
     *
     * 通过多组数据，将问题转化为 AX=XB 形式求解
     *
     * @param calibData 标定数据数组，每组包含末端位姿和相机观测点
     * @param result 输出的标定结果 (相机到末端的变换)
     * @return 成功返回0，失败返回错误码
     */
    virtual int CalculateEyeInHand(
        const std::vector<HECEyeInHandData>& calibData,
        HECCalibResult& result) = 0;

    /**
     * @brief 眼在手上标定 - 使用已知的固定标定点
     * 当标定点在基座坐标系下的位置已知时使用此方法
     *
     * @param calibData 标定数据数组
     * @param targetInBase 标定点在基座坐标系下的已知坐标
     * @param result 输出的标定结果
     * @return 成功返回0，失败返回错误码
     */
    virtual int CalculateEyeInHandWithTarget(
        const std::vector<HECEyeInHandData>& calibData,
        const HECPoint3D& targetInBase,
        HECCalibResult& result) = 0;

    /**
     * @brief TCP 标定（支持 3-DOF 和 6-DOF）
     * @param data TCP 标定输入数据
     * @return TCP 标定结果
     */
    virtual HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) = 0;

    /**
     * @brief Eye-To-Hand 标定（使用完整位姿信息 - Park 方法）
     * 相机固定，标定板安装在机械臂末端，求相机到基座的变换
     *
     * 使用 Park/Tsai-Lenz 方法，利用完整的位姿信息（位置+姿态）
     * 求解 AX=XB 方程，其中：
     * A = T_end_i^{-1} * T_end_j (末端位姿的相对变换)
     * B = T_board_i^{-1} * T_board_j (标定板在相机坐标系的相对变换)
     * X = T_cam_to_base (相机到基座的变换，待求)
     *
     * @param calibData 标定数据数组，每组包含末端位姿和标定板在相机坐标系的完整位姿
     * @param result 输出的标定结果（相机到基座的变换）
     * @return 成功返回0，失败返回错误码
     */
    virtual int CalculateEyeToHandWithPose(
        const std::vector<HECEyeToHandData>& calibData,
        HECCalibResult& result) = 0;
};

/**
 * @brief 创建手眼标定实例
 * @return 手眼标定实例指针
 */
IHandEyeCalib* CreateHandEyeCalibInstance();

/**
 * @brief 销毁手眼标定实例
 * @param instance 实例指针
 */
void DestroyHandEyeCalibInstance(IHandEyeCalib* instance);

#endif // IHANDEYECALIB_H