GrabBag/Module/HandEyeCalib/Src/HandEyeCalib.cpp

#ifndef HANDEYECALIB_LIBRARY
#define HANDEYECALIB_LIBRARY
#endif

#include "HandEyeCalib.h"
#include "VrError.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>

HandEyeCalib::HandEyeCalib()
{
}

HandEyeCalib::~HandEyeCalib()
{
}

int HandEyeCalib::CalculateRT(
    const std::vector<HECPoint3D>& eyePoints,
    const std::vector<HECPoint3D>& robotPoints,
    HECCalibResult& result)
{
    // 检查输入参数
    if (eyePoints.size() != robotPoints.size()) {
        return ERR_CODE(APP_ERR_PARAM);
    }

    int N = static_cast<int>(eyePoints.size());
    if (N < 3) {
        return ERR_CODE(APP_ERR_PARAM);  // 至少需要3个点
    }

    // 【1】计算质心
    HECPoint3D p1, p2;
    for (int i = 0; i < N; i++) {
        p1 += eyePoints[i];
        p2 += robotPoints[i];
    }
    p1 = p1 / static_cast<double>(N);
    p2 = p2 / static_cast<double>(N);
    result.centerEye = p1;
    result.centerRobot = p2;

    // 【2】计算去中心坐标
    std::vector<HECPoint3D> q1(N), q2(N);
    for (int i = 0; i < N; i++) {
        q1[i] = eyePoints[i] - p1;
        q2[i] = robotPoints[i] - p2;
    }

    // 【3】计算协方差矩阵 W = sum(q1 * q2^T)
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i++) {
        Eigen::Vector3d v1(q1[i].x, q1[i].y, q1[i].z);
        Eigen::Vector3d v2(q2[i].x, q2[i].y, q2[i].z);
        W += v1 * v2.transpose();
    }

    // 【4】对W进行SVD分解
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 【5】计算旋转矩阵 R = V * M * U^T
    // M用于处理反射情况，确保R是正交旋转矩阵
    double det = (U * V.transpose()).determinant();
    Eigen::Matrix3d M;
    M << 1, 0, 0,
         0, 1, 0,
         0, 0, det;

    Eigen::Matrix3d R_eigen = V * M * U.transpose();

    // 【6】计算平移向量 T = p2 - R * p1
    Eigen::Vector3d p1_eigen(p1.x, p1.y, p1.z);
    Eigen::Vector3d p2_eigen(p2.x, p2.y, p2.z);
    Eigen::Vector3d T_eigen = p2_eigen - R_eigen * p1_eigen;

    // 【7】转换为输出格式
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            result.R.at(i, j) = R_eigen(i, j);
        }
    }
    result.T = HECTranslationVector(T_eigen(0), T_eigen(1), T_eigen(2));

    // 【8】计算标定误差
    result.error = CalculateError(eyePoints, robotPoints, result);

    return SUCCESS;
}

int HandEyeCalib::CalculateRT(
    const std::vector<HECCalibPointPair>& pointPairs,
    HECCalibResult& result)
{
    std::vector<HECPoint3D> eyePoints;
    std::vector<HECPoint3D> robotPoints;

    eyePoints.reserve(pointPairs.size());
    robotPoints.reserve(pointPairs.size());

    for (const auto& pair : pointPairs) {
        eyePoints.push_back(pair.eyePoint);
        robotPoints.push_back(pair.robotPoint);
    }

    return CalculateRT(eyePoints, robotPoints, result);
}

void HandEyeCalib::TransformPoint(
    const HECRotationMatrix& R,
    const HECTranslationVector& T,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint)
{
    // 使用Eigen进行计算
    Eigen::Matrix3d R_eigen;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R_eigen(i, j) = R.at(i, j);
        }
    }
    Eigen::Vector3d T_eigen(T.at(0), T.at(1), T.at(2));
    Eigen::Vector3d src_eigen(srcPoint.x, srcPoint.y, srcPoint.z);

    Eigen::Vector3d dst_eigen = R_eigen * src_eigen + T_eigen;

    dstPoint.x = dst_eigen(0);
    dstPoint.y = dst_eigen(1);
    dstPoint.z = dst_eigen(2);
}

void HandEyeCalib::TransformPointWithCenter(
    const HECRotationMatrix& R,
    const HECTranslationVector& T,
    const HECPoint3D& srcCenter,
    const HECPoint3D& dstCenter,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint)
{
    // 使用Eigen进行计算
    Eigen::Matrix3d R_eigen;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R_eigen(i, j) = R.at(i, j);
        }
    }

    Eigen::Vector3d srcCenter_eigen(srcCenter.x, srcCenter.y, srcCenter.z);
    Eigen::Vector3d dstCenter_eigen(dstCenter.x, dstCenter.y, dstCenter.z);
    Eigen::Vector3d src_eigen(srcPoint.x, srcPoint.y, srcPoint.z);

    // 变换公式: dstPoint = R * (srcPoint - srcCenter) + dstCenter
    Eigen::Vector3d dst_eigen = R_eigen * (src_eigen - srcCenter_eigen) + dstCenter_eigen;

    dstPoint.x = dst_eigen(0);
    dstPoint.y = dst_eigen(1);
    dstPoint.z = dst_eigen(2);
}

void HandEyeCalib::RotatePoint(
    const HECRotationMatrix& R,
    const HECPoint3D& srcPoint,
    HECPoint3D& dstPoint)
{
    // 使用Eigen进行计算
    Eigen::Matrix3d R_eigen;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R_eigen(i, j) = R.at(i, j);
        }
    }
    Eigen::Vector3d src_eigen(srcPoint.x, srcPoint.y, srcPoint.z);

    Eigen::Vector3d dst_eigen = R_eigen * src_eigen;

    dstPoint.x = dst_eigen(0);
    dstPoint.y = dst_eigen(1);
    dstPoint.z = dst_eigen(2);
}

void HandEyeCalib::RotationMatrixToEulerZYX(
    const HECRotationMatrix& R,
    HECEulerAngles& angles)
{
    RotationMatrixToEuler(R, HECEulerOrder::ZYX, angles);
}

void HandEyeCalib::RotationMatrixToEuler(
    const HECRotationMatrix& R,
    HECEulerOrder order,
    HECEulerAngles& angles)
{
    // 将 HECRotationMatrix 转换为 Eigen::Matrix3d
    Eigen::Matrix3d R_eigen;
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            R_eigen(i, j) = R.at(i, j);

    // 外旋 ABC = 内旋 CBA，使用 Eigen 的 eulerAngles（内旋模式）
    Eigen::Vector3d ea;
    switch (order) {
    case HECEulerOrder::XYZ:
        // 外旋 XYZ = 内旋 ZYX
        ea = R_eigen.eulerAngles(2, 1, 0);  // 返回 [yaw, pitch, roll]
        angles.yaw = ea[0]; angles.pitch = ea[1]; angles.roll = ea[2];
        break;
    case HECEulerOrder::XZY:
        // 外旋 XZY = 内旋 YZX
        ea = R_eigen.eulerAngles(1, 2, 0);  // 返回 [pitch, yaw, roll]
        angles.pitch = ea[0]; angles.yaw = ea[1]; angles.roll = ea[2];
        break;
    case HECEulerOrder::YXZ:
        // 外旋 YXZ = 内旋 ZXY
        ea = R_eigen.eulerAngles(2, 0, 1);  // 返回 [yaw, roll, pitch]
        angles.yaw = ea[0]; angles.roll = ea[1]; angles.pitch = ea[2];
        break;
    case HECEulerOrder::YZX:
        // 外旋 YZX = 内旋 XZY
        ea = R_eigen.eulerAngles(0, 2, 1);  // 返回 [roll, yaw, pitch]
        angles.roll = ea[0]; angles.yaw = ea[1]; angles.pitch = ea[2];
        break;
    case HECEulerOrder::ZXY:
        // 外旋 ZXY = 内旋 YXZ
        ea = R_eigen.eulerAngles(1, 0, 2);  // 返回 [pitch, roll, yaw]
        angles.pitch = ea[0]; angles.roll = ea[1]; angles.yaw = ea[2];
        break;
    case HECEulerOrder::ZYX:
        // 外旋 ZYX = 内旋 XYZ
        ea = R_eigen.eulerAngles(0, 1, 2);  // 返回 [roll, pitch, yaw]
        angles.roll = ea[0]; angles.pitch = ea[1]; angles.yaw = ea[2];
        break;
    }
}

void HandEyeCalib::EulerZYXToRotationMatrix(
    const HECEulerAngles& angles,
    HECRotationMatrix& R)
{
    EulerToRotationMatrix(angles, HECEulerOrder::ZYX, R);
}

void HandEyeCalib::EulerToRotationMatrix(
    const HECEulerAngles& angles,
    HECEulerOrder order,
    HECRotationMatrix& R)
{
    double roll  = angles.roll;   // 绕X轴
    double pitch = angles.pitch;  // 绕Y轴
    double yaw   = angles.yaw;   // 绕Z轴

    // 使用 Eigen::AngleAxisd 构造各轴旋转矩阵
    Eigen::Matrix3d Rx = Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX()).toRotationMatrix();
    Eigen::Matrix3d Ry = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()).toRotationMatrix();
    Eigen::Matrix3d Rz = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ()).toRotationMatrix();

    // 外旋 ABC：矩阵公式为 R = Rc · Rb · Ra（右乘，反向）
    Eigen::Matrix3d R_eigen;
    switch (order) {
    case HECEulerOrder::XYZ: R_eigen = Rz * Ry * Rx; break;  // 外旋 XYZ
    case HECEulerOrder::XZY: R_eigen = Ry * Rz * Rx; break;  // 外旋 XZY
    case HECEulerOrder::YXZ: R_eigen = Rz * Rx * Ry; break;  // 外旋 YXZ
    case HECEulerOrder::YZX: R_eigen = Rx * Rz * Ry; break;  // 外旋 YZX
    case HECEulerOrder::ZXY: R_eigen = Ry * Rx * Rz; break;  // 外旋 ZXY
    case HECEulerOrder::ZYX: R_eigen = Rx * Ry * Rz; break;  // 外旋 ZYX
    }

    // 转换回 HECRotationMatrix
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            R.at(i, j) = R_eigen(i, j);
}

void HandEyeCalib::TransformPose(
    const HECCalibResult& calibResult,
    const HECPoint3D& eyePoint,
    const std::vector<HECPoint3D>& eyeDirVectors,
    bool invertYZ,
    HECPoseResult& poseResult)
{
    // 1. 位置转换: P_robot = R * P_eye + T
    TransformPoint(calibResult.R, calibResult.T, eyePoint, poseResult.position);

    // 2. 姿态转换
    if (eyeDirVectors.size() < 3) {
        // 如果没有方向向量，姿态设为0
        poseResult.angles = HECEulerAngles();
        return;
    }

    // 复制方向向量
    std::vector<HECPoint3D> dirVectors = eyeDirVectors;

    // 如果需要，对Y轴和Z轴方向取反（坐标系方向调整）
    if (invertYZ) {
        dirVectors[1] = dirVectors[1] * (-1.0);
        dirVectors[2] = dirVectors[2] * (-1.0);
    }

    // 对每个方向向量应用旋转矩阵
    std::vector<HECPoint3D> robotDirVectors(3);
    for (int i = 0; i < 3; i++) {
        RotatePoint(calibResult.R, dirVectors[i], robotDirVectors[i]);
    }

    // 构建旋转矩阵（列向量形式）
    HECRotationMatrix R_pose;
    R_pose.at(0, 0) = robotDirVectors[0].x;
    R_pose.at(0, 1) = robotDirVectors[1].x;
    R_pose.at(0, 2) = robotDirVectors[2].x;
    R_pose.at(1, 0) = robotDirVectors[0].y;
    R_pose.at(1, 1) = robotDirVectors[1].y;
    R_pose.at(1, 2) = robotDirVectors[2].y;
    R_pose.at(2, 0) = robotDirVectors[0].z;
    R_pose.at(2, 1) = robotDirVectors[1].z;
    R_pose.at(2, 2) = robotDirVectors[2].z;

    // 从旋转矩阵提取欧拉角
    RotationMatrixToEulerZYX(R_pose, poseResult.angles);
}

double HandEyeCalib::CalculateError(
    const std::vector<HECPoint3D>& eyePoints,
    const std::vector<HECPoint3D>& robotPoints,
    const HECCalibResult& calibResult)
{
    if (eyePoints.size() != robotPoints.size() || eyePoints.empty()) {
        return -1.0;
    }

    double totalError = 0.0;
    int N = static_cast<int>(eyePoints.size());

    for (int i = 0; i < N; i++) {
        HECPoint3D transformedPoint;
        TransformPoint(calibResult.R, calibResult.T, eyePoints[i], transformedPoint);

        HECPoint3D diff = transformedPoint - robotPoints[i];
        totalError += diff.norm();
    }

    return totalError / N;
}

int HandEyeCalib::CalculateEyeInHand(
    const std::vector<HECEyeInHandData>& calibData,
    HECCalibResult& result)
{
    int N = static_cast<int>(calibData.size());
    if (N < 2) {
        return ERR_CODE(APP_ERR_PARAM);
    }

    // 眼在手上标定使用 AX=XB 问题的求解方法
    // A = T_end_i^{-1} * T_end_j (两次末端位姿之间的相对变换)
    // B = T_cam (相机观测到的标定点相对变换)
    // X = T_cam_to_end (待求的相机到末端的变换)

    // 构建方程组，使用最小二乘法求解
    // 这里使用简化方法：假设标定点固定，通过多组数据求解

    // 收集所有相机坐标系下的点，转换到基座坐标系
    // P_base = T_end * T_cam * P_cam
    // 对于固定标定点，所有 P_base 应该相同

    // 使用迭代优化方法求解
    // 初始估计：使用第一组数据
    Eigen::Matrix4d T_cam = Eigen::Matrix4d::Identity();

    // 构建超定方程组
    // 对于每对数据 (i, j)，有：
    // T_end_i * T_cam * P_cam_i = T_end_j * T_cam * P_cam_j
    // 即 T_end_i^{-1} * T_end_j * T_cam * P_cam_j = T_cam * P_cam_i

    // 使用 Tsai-Lenz 方法求解旋转部分
    std::vector<Eigen::Matrix3d> A_rot, B_rot;
    std::vector<Eigen::Vector3d> A_trans, B_trans;

    for (int i = 0; i < N - 1; i++) {
        // 获取末端位姿
        Eigen::Matrix4d T_end_i = Eigen::Matrix4d::Identity();
        Eigen::Matrix4d T_end_j = Eigen::Matrix4d::Identity();

        for (int r = 0; r < 4; r++) {
            for (int c = 0; c < 4; c++) {
                T_end_i(r, c) = calibData[i].endPose.at(r, c);
                T_end_j(r, c) = calibData[i + 1].endPose.at(r, c);
            }
        }

        // A = T_end_i^{-1} * T_end_j
        Eigen::Matrix4d A = T_end_i.inverse() * T_end_j;

        // 相机观测点
        Eigen::Vector3d P_cam_i(calibData[i].targetInCamera.x,
                                 calibData[i].targetInCamera.y,
                                 calibData[i].targetInCamera.z);
        Eigen::Vector3d P_cam_j(calibData[i + 1].targetInCamera.x,
                                 calibData[i + 1].targetInCamera.y,
                                 calibData[i + 1].targetInCamera.z);

        A_rot.push_back(A.block<3, 3>(0, 0));
        A_trans.push_back(A.block<3, 1>(0, 3));

        // B 矩阵从相机观测构建（假设标定点固定）
        // 这里简化处理，直接使用点的差异
        B_rot.push_back(Eigen::Matrix3d::Identity());
        B_trans.push_back(P_cam_i - P_cam_j);
    }

    // 使用 SVD 求解旋转矩阵
    // 构建 M 矩阵用于求解旋转
    Eigen::MatrixXd M(9 * (N - 1), 9);
    M.setZero();

    for (int i = 0; i < N - 1; i++) {
        // (A_rot ⊗ I - I ⊗ B_rot^T) * vec(X_rot) = 0
        Eigen::Matrix3d Ai = A_rot[i];
        Eigen::Matrix3d Bi = B_rot[i];

        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) {
                // Kronecker product 展开
                int row = i * 9 + r * 3 + c;
                for (int k = 0; k < 3; k++) {
                    M(row, r * 3 + k) += Ai(c, k);
                    M(row, k * 3 + c) -= Bi(r, k);
                }
            }
        }
    }

    // SVD 求解
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullV);
    Eigen::VectorXd x = svd.matrixV().col(8);

    // 重构旋转矩阵
    Eigen::Matrix3d R_raw;
    R_raw << x(0), x(1), x(2),
             x(3), x(4), x(5),
             x(6), x(7), x(8);

    // 正交化旋转矩阵
    Eigen::JacobiSVD<Eigen::Matrix3d> svd_R(R_raw, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d R_cam = svd_R.matrixU() * svd_R.matrixV().transpose();

    // 确保行列式为1
    if (R_cam.determinant() < 0) {
        R_cam = -R_cam;
    }

    // 求解平移向量
    // 使用最小二乘法: (A_rot - I) * t_cam = R_cam * B_trans - A_trans
    Eigen::MatrixXd C(3 * (N - 1), 3);
    Eigen::VectorXd d(3 * (N - 1));

    for (int i = 0; i < N - 1; i++) {
        C.block<3, 3>(i * 3, 0) = A_rot[i] - Eigen::Matrix3d::Identity();
        d.segment<3>(i * 3) = R_cam * B_trans[i] - A_trans[i];
    }

    // 最小二乘求解
    Eigen::Vector3d t_cam = C.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d);

    // 输出结果
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            result.R.at(i, j) = R_cam(i, j);
        }
    }
    result.T = HECTranslationVector(t_cam(0), t_cam(1), t_cam(2));

    // 计算误差
    double totalError = 0.0;
    Eigen::Matrix4d T_result = Eigen::Matrix4d::Identity();
    T_result.block<3, 3>(0, 0) = R_cam;
    T_result.block<3, 1>(0, 3) = t_cam;

    // 计算标定点在基座坐标系下的位置（应该一致）
    std::vector<Eigen::Vector3d> basePoints;
    for (int i = 0; i < N; i++) {
        Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity();
        for (int r = 0; r < 4; r++) {
            for (int c = 0; c < 4; c++) {
                T_end(r, c) = calibData[i].endPose.at(r, c);
            }
        }

        Eigen::Vector4d P_cam(calibData[i].targetInCamera.x,
                               calibData[i].targetInCamera.y,
                               calibData[i].targetInCamera.z, 1.0);
        Eigen::Vector4d P_base = T_end * T_result * P_cam;
        basePoints.push_back(P_base.head<3>());
    }

    // 计算基座点的离散程度作为误差
    Eigen::Vector3d meanBase = Eigen::Vector3d::Zero();
    for (const auto& p : basePoints) {
        meanBase += p;
    }
    meanBase /= N;

    for (const auto& p : basePoints) {
        totalError += (p - meanBase).norm();
    }
    result.error = totalError / N;

    result.centerEye = HECPoint3D(0, 0, 0);
    result.centerRobot = HECPoint3D(meanBase(0), meanBase(1), meanBase(2));

    return SUCCESS;
}

int HandEyeCalib::CalculateEyeInHandWithTarget(
    const std::vector<HECEyeInHandData>& calibData,
    const HECPoint3D& targetInBase,
    HECCalibResult& result)
{
    int N = static_cast<int>(calibData.size());
    if (N < 3) {
        return ERR_CODE(APP_ERR_PARAM);
    }

    // 当标定点在基座坐标系下的位置已知时
    // P_base = T_end * T_cam * P_cam
    // T_cam = T_end^{-1} * T_base_to_cam
    // 其中 T_base_to_cam 将 P_cam 变换到 P_base

    // 收集相机坐标系下的点和对应的末端逆变换后的基座点
    std::vector<HECPoint3D> camPoints;
    std::vector<HECPoint3D> endPoints;

    Eigen::Vector3d P_base(targetInBase.x, targetInBase.y, targetInBase.z);

    for (int i = 0; i < N; i++) {
        // 获取末端位姿的逆
        Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity();
        for (int r = 0; r < 4; r++) {
            for (int c = 0; c < 4; c++) {
                T_end(r, c) = calibData[i].endPose.at(r, c);
            }
        }
        Eigen::Matrix4d T_end_inv = T_end.inverse();

        // 将基座点变换到末端坐标系
        Eigen::Vector4d P_base_h(P_base(0), P_base(1), P_base(2), 1.0);
        Eigen::Vector4d P_end = T_end_inv * P_base_h;

        camPoints.push_back(calibData[i].targetInCamera);
        endPoints.push_back(HECPoint3D(P_end(0), P_end(1), P_end(2)));
    }

    // 使用 SVD 方法求解相机到末端的变换
    // 这与 EyeToHand 的 CalculateRT 方法相同
    return CalculateRT(camPoints, endPoints, result);
}

Eigen::Matrix3d HandEyeCalib::eulerToRotationMatrix(double rx, double ry, double rz, HECEulerOrder order)
{
    // 将角度转为弧度
    const double deg2rad = M_PI / 180.0;
    double roll  = rx * deg2rad;
    double pitch = ry * deg2rad;
    double yaw   = rz * deg2rad;

    // 复用现有的 EulerToRotationMatrix 逻辑
    HECEulerAngles angles(roll, pitch, yaw);
    HECRotationMatrix R;
    EulerToRotationMatrix(angles, order, R);

    // 转换为 Eigen 矩阵
    Eigen::Matrix3d result;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            result(i, j) = R.at(i, j);
        }
    }
    return result;
}

HECTCPCalibResult HandEyeCalib::CalculateTCP(const HECTCPCalibData& data)
{
    HECTCPCalibResult result;

    // 参数校验
    int N = static_cast<int>(data.poses.size());
    if (N < 3) {
        result.success = false;
        result.errorMessage = "至少需要3组法兰位姿数据";
        return result;
    }

    // 构建每组位姿的旋转矩阵和位置向量
    std::vector<Eigen::Matrix3d> rotations(N);
    std::vector<Eigen::Vector3d> positions(N);

    for (int i = 0; i < N; ++i) {
        const auto& pose = data.poses[i];
        rotations[i] = eulerToRotationMatrix(pose.rx, pose.ry, pose.rz, pose.eulerOrder);
        positions[i] = Eigen::Vector3d(pose.x, pose.y, pose.z);
    }

    // === 3-DOF 位置求解 ===
    // 约束：R_i * t_tcp + p_i = P_tcp（常数）
    // 对相邻位姿对：(R_i - R_j) * t_tcp = p_j - p_i
    int numPairs = N - 1;
    Eigen::MatrixXd A(3 * numPairs, 3);
    Eigen::VectorXd b(3 * numPairs);

    for (int i = 0; i < numPairs; ++i) {
        Eigen::Matrix3d dR = rotations[i] - rotations[i + 1];
        Eigen::Vector3d dp = positions[i + 1] - positions[i];

        A.block<3, 3>(i * 3, 0) = dR;
        b.segment<3>(i * 3) = dp;
    }

    // SVD 最小二乘求解
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // 检查奇异值，判断退化情况
    Eigen::VectorXd singularValues = svd.singularValues();
    if (singularValues.size() < 3 || singularValues(2) < 1e-6) {
        result.success = false;
        result.errorMessage = "位姿数据退化，无法求解（可能所有位姿过于相似）";
        return result;
    }

    Eigen::Vector3d t_tcp = svd.solve(b);

    result.tx = t_tcp(0);
    result.ty = t_tcp(1);
    result.tz = t_tcp(2);

    // 计算残差：所有位姿计算 R_i * t_tcp + p_i，应趋近同一点
    Eigen::Vector3d meanTCPPoint = Eigen::Vector3d::Zero();
    for (int i = 0; i < N; ++i) {
        meanTCPPoint += rotations[i] * t_tcp + positions[i];
    }
    meanTCPPoint /= N;

    double totalResidual = 0.0;
    for (int i = 0; i < N; ++i) {
        Eigen::Vector3d tcpPoint = rotations[i] * t_tcp + positions[i];
        totalResidual += (tcpPoint - meanTCPPoint).squaredNorm();
    }
    result.residualError = std::sqrt(totalResidual / N);

    // === 6-DOF 姿态求解 ===
    if (data.mode == HECTCPCalibMode::Full6DOF) {
        int refIdx = data.referencePoseIndex;
        if (refIdx < 0 || refIdx >= N) {
            result.success = false;
            result.errorMessage = "参考位姿索引越界";
            return result;
        }

        // 参考位姿的法兰旋转矩阵
        Eigen::Matrix3d R_ref = rotations[refIdx];

        // 期望的世界坐标系朝向旋转矩阵
        Eigen::Matrix3d R_world = eulerToRotationMatrix(
            data.worldRx, data.worldRy, data.worldRz, data.worldEulerOrder);

        // TCP 相对于法兰的旋转：R_tcp = R_ref^T * R_world
        Eigen::Matrix3d R_tcp = R_ref.transpose() * R_world;

        // 将 R_tcp 转换为欧拉角（度）输出
        HECRotationMatrix R_out;
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                R_out.at(i, j) = R_tcp(i, j);
            }
        }

        HECEulerAngles tcpAngles;
        RotationMatrixToEuler(R_out, data.worldEulerOrder, tcpAngles);

        double rxDeg, ryDeg, rzDeg;
        tcpAngles.toDegrees(rxDeg, ryDeg, rzDeg);
        result.rx = rxDeg;
        result.ry = ryDeg;
        result.rz = rzDeg;
    }

    result.success = true;
    return result;
}

// 导出函数实现
IHandEyeCalib* CreateHandEyeCalibInstance()
{
    return new HandEyeCalib();
}

void DestroyHandEyeCalibInstance(IHandEyeCalib* instance)
{
    delete instance;
}