GrabBag/Module/HandEyeCalib/Utils/Inc/CoordinateTransform.h

#ifndef COORDINATE_TRANSFORM_H
#define COORDINATE_TRANSFORM_H

#include "CoordinateTypes.h"

namespace CCoordinateTransform
{
    // ==================== 欧拉角与四元数互转 ====================

    /// 欧拉角转四元数
    /// @param euler 欧拉角 (弧度制)
    /// @return 归一化的四元数
    CTQuaternionD eulerToQuaternion(const CTEulerAngles& euler);

    /// 四元数转欧拉角
    /// @param q 四元数 (会自动归一化)
    /// @param order 欧拉角顺序
    /// @return 欧拉角 (弧度制)
    CTEulerAngles quaternionToEuler(const CTQuaternionD& q, CTEulerOrder order = CTEulerOrder::ZYX);

    // ==================== 旋转矩阵与欧拉角互转 ====================

    /// 欧拉角转旋转矩阵
    /// @param euler 欧拉角 (弧度制)
    /// @return 3x3旋转矩阵
    CTRotationMatrix eulerToRotationMatrix(const CTEulerAngles& euler);

    /// 旋转矩阵转欧拉角
    /// @param R 3x3旋转矩阵
    /// @param order 欧拉角顺序
    /// @return 欧拉角 (弧度制)
    CTEulerAngles rotationMatrixToEuler(const CTRotationMatrix& R, CTEulerOrder order = CTEulerOrder::ZYX);

    // ==================== 旋转矩阵与四元数互转 ====================

    /// 四元数转旋转矩阵
    /// @param q 四元数
    /// @return 3x3旋转矩阵
    CTRotationMatrix quaternionToRotationMatrix(const CTQuaternionD& q);

    /// 旋转矩阵转四元数
    /// @param R 3x3旋转矩阵
    /// @return 归一化的四元数
    CTQuaternionD rotationMatrixToQuaternion(const CTRotationMatrix& R);

    // ==================== 三轴手眼转换 ====================

    /// 三轴手眼转换：相机坐标 -> 机器人坐标
    /// 适用于三轴龙门架等只有XYZ平移和Z轴旋转的设备
    /// @param cameraPt 相机坐标系下的点
    /// @param robotPose 机器人当前位姿 (XYZ + Rz)
    /// @param handEye 三轴手眼标定矩阵
    /// @return 机器人基坐标系下的点
    CTVec3D threeAxisCameraToRobot(const CTVec3D& cameraPt,
                                  const CTRobotPose& robotPose,
                                  const CTThreeAxisCalibResult& handEye);

    /// 三轴手眼转换：机器人坐标 -> 相机坐标
    /// @param robotPt 机器人基坐标系下的点
    /// @param robotPose 机器人当前位姿 (XYZ + Rz)
    /// @param handEye 三轴手眼标定矩阵
    /// @return 相机坐标系下的点
    CTVec3D threeAxisRobotToCamera(const CTVec3D& robotPt,
                                  const CTRobotPose& robotPose,
                                  const CTThreeAxisCalibResult& handEye);

    /// 三轴手眼转换：计算抓取位姿
    /// @param cameraPt 相机坐标系下的目标点
    /// @param cameraAngle 相机坐标系下的目标角度 (弧度)
    /// @param robotPose 机器人当前位姿
    /// @param handEye 三轴手眼标定矩阵
    /// @param[out] targetPose 输出的目标抓取位姿
    void threeAxisCalcGraspPose(const CTVec3D& cameraPt,
                                 double cameraAngle,
                                 const CTRobotPose& robotPose,
                                 const CTThreeAxisCalibResult& handEye,
                                 CTRobotPose& targetPose);

    // ==================== 六轴手眼转换 ====================

    /// 六轴手眼转换 (Eye-in-Hand)：相机坐标 -> 机器人坐标
    /// 相机安装在机器人末端执行器上
    /// @param cameraPt 相机坐标系下的点
    /// @param robotPose 机器人当前位姿 (完整6轴)
    /// @param handEye 六轴手眼标定矩阵 (相机到末端执行器)
    /// @return 机器人基坐标系下的点
    CTVec3D sixAxisEyeInHandCameraToRobot(const CTVec3D& cameraPt,
                                         const CTRobotPose& robotPose,
                                         const CTSixAxisCalibResult& handEye);

    /// 六轴手眼转换 (Eye-in-Hand)：机器人坐标 -> 相机坐标
    /// @param robotPt 机器人基坐标系下的点
    /// @param robotPose 机器人当前位姿
    /// @param handEye 六轴手眼标定矩阵
    /// @return 相机坐标系下的点
    CTVec3D sixAxisEyeInHandRobotToCamera(const CTVec3D& robotPt,
                                         const CTRobotPose& robotPose,
                                         const CTSixAxisCalibResult& handEye);

    /// 六轴手眼转换 (Eye-to-Hand)：相机坐标 -> 机器人坐标
    /// 相机固定在外部，观察机器人工作空间
    /// @param cameraPt 相机坐标系下的点
    /// @param cameraToBase 相机到机器人基座的标定矩阵
    /// @return 机器人基坐标系下的点
    CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
                                         const CTSixAxisCalibResult& cameraToBase);

    /// 六轴手眼转换 (Eye-to-Hand)：相机坐标 -> 机器人坐标 (直接使用齐次矩阵)
    /// @param cameraPt 相机坐标系下的点
    /// @param cameraToBaseMatrix 相机到机器人基座的4x4齐次变换矩阵
    /// @return 机器人基坐标系下的点
    CTVec3D sixAxisEyeToHandCameraToRobot(const CTVec3D& cameraPt,
                                         const CTHomogeneousMatrix& cameraToBaseMatrix);

    /// 六轴手眼转换 (Eye-to-Hand)：机器人坐标 -> 相机坐标
    /// @param robotPt 机器人基坐标系下的点
    /// @param cameraToBase 相机到机器人基座的标定矩阵
    /// @return 相机坐标系下的点
    CTVec3D sixAxisEyeToHandRobotToCamera(const CTVec3D& robotPt,
                                         const CTSixAxisCalibResult& cameraToBase);

    /// 六轴手眼转换：计算抓取位姿 (Eye-in-Hand)
    /// @param cameraPose 相机坐标系下的目标位姿
    /// @param robotPose 机器人当前位姿
    /// @param handEye 六轴手眼标定矩阵
    /// @param[out] targetPose 输出的目标抓取位姿
    void sixAxisEyeInHandCalcGraspPose(const CTCameraPose& cameraPose,
                                        const CTRobotPose& robotPose,
                                        const CTSixAxisCalibResult& handEye,
                                        CTRobotPose& targetPose);

    /// 六轴手眼转换：计算抓取位姿 (Eye-to-Hand)
    /// @param cameraPose 相机坐标系下的目标位姿
    /// @param cameraToBase 相机到机器人基座的标定矩阵
    /// @param[out] targetPose 输出的目标抓取位姿
    void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
                                        const CTSixAxisCalibResult& cameraToBase,
                                        CTRobotPose& targetPose);

    /// 六轴手眼转换：计算抓取位姿 (Eye-to-Hand) - 直接使用齐次矩阵
    /// @param cameraPose 相机坐标系下的目标位姿
    /// @param cameraToBaseMatrix 相机到机器人基座的4x4齐次变换矩阵
    /// @param eulerOrder 输出欧拉角的顺序 (默认XYZ，对应外旋ZYX即RZ-RY-RX)
    /// @param[out] targetPose 输出的目标抓取位姿
    void sixAxisEyeToHandCalcGraspPose(const CTCameraPose& cameraPose,
                                        const CTHomogeneousMatrix& cameraToBaseMatrix,
                                        CTEulerOrder eulerOrder,
                                        CTRobotPose& targetPose);

    // ==================== 辅助函数 ====================

    /// 角度转弧度
    inline double degToRad(double deg) { return deg * M_PI / 180.0; }

    /// 弧度转角度
    inline double radToDeg(double rad) { return rad * 180.0 / M_PI; }

    /// 归一化角度到 [-π, π]
    double normalizeAngle(double angle);

    /// 计算两个四元数之间的角度差 (弧度)
    double quaternionAngleDiff(const CTQuaternionD& q1, const CTQuaternionD& q2);

    /// 四元数球面线性插值 (SLERP)
    CTQuaternionD quaternionSlerp(const CTQuaternionD& q1, const CTQuaternionD& q2, double t);

} // namespace CCoordinateTransform

#endif // COORDINATE_TRANSFORM_H