更新工件孔定位算法

App/WorkpieceHole/WorkpieceHoleApp/Version.h

@@ -5,7 +5,7 @@
 #define WORKPIECEHOLE_APP_NAME "工件孔定位"
 
 // 版本字符串
-#define WORKPIECEHOLE_VERSION_STRING "1.1.2"
+#define WORKPIECEHOLE_VERSION_STRING "1.1.3"
 #define WORKPIECEHOLE_BUILD_STRING "1"
 #define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING
 

App/WorkpieceHole/WorkpieceHoleApp/Version.md

@@ -1,3 +1,7 @@
+# 1.1.3
+## build_1 2026-04-21
+1. 算法更新
+
 # 1.1.2
 ## build_1 2026-04-17
 1. 协议和页面显示倒叙输出

Module/HandEyeCalib/Src/HandEyeCalib.cpp

@@ -376,171 +376,247 @@ int HandEyeCalib::CalculateEyeInHand(
     HECCalibResult& result)
 {
     int N = static_cast<int>(calibData.size());
-    if (N < 2) {
+    if (N < 3) {
         return ERR_CODE(APP_ERR_PARAM);
     }
 
-    // 眼在手上标定使用 AX=XB 问题的求解方法
-    // A = T_end_i^{-1} * T_end_j (两次末端位姿之间的相对变换)
-    // B = T_cam (相机观测到的标定点相对变换)
-    // X = T_cam_to_end (待求的相机到末端的变换)
+    // ================================================================
+    // 预处理：提取末端位姿和相机观测点
+    // ================================================================
+    // 眼在手上：相机安装在机器人末端，标定靶固定
+    // 约束：T_end_i * X * P_cam_i = P_base（对所有 i，P_base 相同）
+    // 其中 X = [R_x | t_x] 是相机到末端的变换（待求）
 
-    // 构建方程组，使用最小二乘法求解
-    // 这里使用简化方法：假设标定点固定，通过多组数据求解
+    std::vector<Eigen::Matrix3d> R_ends(N);
+    std::vector<Eigen::Vector3d> t_ends(N);
+    std::vector<Eigen::Vector3d> p_cams(N);
 
-    // 收集所有相机坐标系下的点，转换到基座坐标系
-    // 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);
+    for (int i = 0; i < N; i++) {
+        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+        for (int r = 0; r < 4; r++)
+            for (int c = 0; c < 4; c++)
+                T(r, c) = calibData[i].endPose.at(r, c);
+        R_ends[i] = T.block<3, 3>(0, 0);
+        t_ends[i] = T.block<3, 1>(0, 3);
+        p_cams[i] = Eigen::Vector3d(
+            calibData[i].targetInCamera.x,
+            calibData[i].targetInCamera.y,
+            calibData[i].targetInCamera.z);
     }
 
-    // 使用 SVD 求解旋转矩阵
-    // 构建 M 矩阵用于求解旋转
-    Eigen::MatrixXd M(9 * (N - 1), 9);
-    M.setZero();
+    int numPairs = N * (N - 1) / 2;
 
-    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];
+    // ================================================================
+    // 第一阶段：线性初始化
+    // ================================================================
+    // 对两组 (i, j)：
+    //   R_i*(R_x*p_i + t_x) + t_i = R_j*(R_x*p_j + t_x) + t_j
+    // 展开：
+    //   (R_i-R_j)*t_x + R_i*M*p_i - R_j*M*p_j = t_j - t_i
+    // 其中 M = R_x，对 [t_x; vec(M)] 线性
 
-        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);
+    Eigen::MatrixXd C(3 * numPairs, 12);
+    Eigen::VectorXd d_vec(3 * numPairs);
+
+    int row = 0;
+    for (int i = 0; i < N; i++) {
+        for (int j = i + 1; j < N; j++) {
+            // (R_i - R_j) * t_x
+            C.block<3, 3>(row, 0) = R_ends[i] - R_ends[j];
+
+            // d(R*M*p)_k / dM_{l,m} = R_{k,l} * p_m (row-major vec)
+            for (int k = 0; k < 3; k++) {
+                for (int l = 0; l < 3; l++) {
+                    for (int m = 0; m < 3; m++) {
+                        C(row + k, 3 + l * 3 + m) =
+                            R_ends[i](k, l) * p_cams[i](m)
+                          - R_ends[j](k, l) * p_cams[j](m);
+                    }
                 }
             }
+
+            d_vec.segment<3>(row) = t_ends[j] - t_ends[i];
+            row += 3;
         }
     }
 
-    // SVD 求解
-    Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullV);
-    Eigen::VectorXd x = svd.matrixV().col(8);
+    Eigen::VectorXd x_lin = C.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d_vec);
 
-    // 重构旋转矩阵
-    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::Vector3d t_x = x_lin.head<3>();
 
-    // 正交化旋转矩阵
-    Eigen::JacobiSVD<Eigen::Matrix3d> svd_R(R_raw, Eigen::ComputeFullU | Eigen::ComputeFullV);
-    Eigen::Matrix3d R_cam = svd_R.matrixU() * svd_R.matrixV().transpose();
+    // 提取 M 并投影到最近旋转矩阵
+    Eigen::Matrix3d M_mat;
+    for (int l = 0; l < 3; l++)
+        for (int m = 0; m < 3; m++)
+            M_mat(l, m) = x_lin(3 + l * 3 + m);
 
-    // 确保行列式为1
-    if (R_cam.determinant() < 0) {
-        R_cam = -R_cam;
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd_M(M_mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    Eigen::Matrix3d R_x = svd_M.matrixU() * svd_M.matrixV().transpose();
+    if (R_x.determinant() < 0) {
+        Eigen::Matrix3d V = svd_M.matrixV();
+        V.col(2) *= -1;
+        R_x = svd_M.matrixU() * V.transpose();
     }
 
-    // 求解平移向量
-    // 使用最小二乘法: (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;
-    double maxErr = 0.0;
-    double minErr = std::numeric_limits<double>::max();
-    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;
+    // 已知旋转后重新线性求解平移
+    Eigen::MatrixXd C_t(3 * numPairs, 3);
+    Eigen::VectorXd d_t(3 * numPairs);
+    row = 0;
     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);
+        for (int j = i + 1; j < N; j++) {
+            C_t.block<3, 3>(row, 0) = R_ends[i] - R_ends[j];
+            d_t.segment<3>(row) = t_ends[j] - t_ends[i]
+                                 - R_ends[i] * R_x * p_cams[i]
+                                 + R_ends[j] * R_x * p_cams[j];
+            row += 3;
+        }
+    }
+    t_x = C_t.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d_t);
+
+    // ================================================================
+    // 第二阶段：Levenberg-Marquardt 非线性优化
+    // ================================================================
+    // 参数化：角轴 w(3) + 平移 t(3) = 6 参数
+    // 残差：r_ij = T_end_i * X * p_i - T_end_j * X * p_j
+    // 最小化 sum ||r_ij||^2
+
+    auto skew = [](const Eigen::Vector3d& v) -> Eigen::Matrix3d {
+        Eigen::Matrix3d S;
+        S <<     0, -v(2),  v(1),
+             v(2),     0, -v(0),
+            -v(1),  v(0),     0;
+        return S;
+    };
+
+    auto paramsToRot = [](const Eigen::Vector3d& w) -> Eigen::Matrix3d {
+        double theta = w.norm();
+        if (theta < 1e-10) return Eigen::Matrix3d::Identity();
+        return Eigen::AngleAxisd(theta, w / theta).toRotationMatrix();
+    };
+
+    auto computeCost = [&](const Eigen::Matrix3d& R, const Eigen::Vector3d& t) -> double {
+        double cost = 0;
+        for (int i = 0; i < N; i++) {
+            Eigen::Vector3d base_i = R_ends[i] * (R * p_cams[i] + t) + t_ends[i];
+            for (int j = i + 1; j < N; j++) {
+                Eigen::Vector3d base_j = R_ends[j] * (R * p_cams[j] + t) + t_ends[j];
+                cost += (base_i - base_j).squaredNorm();
+            }
+        }
+        return 0.5 * cost;
+    };
+
+    // 初始化参数
+    Eigen::AngleAxisd aa_init(R_x);
+    Eigen::VectorXd params(6);
+    if (aa_init.angle() > 1e-10) {
+        params.head<3>() = aa_init.angle() * aa_init.axis();
+    } else {
+        params.head<3>().setZero();
+    }
+    params.tail<3>() = t_x;
+
+    double lambda = 1e-3;
+
+    for (int iter = 0; iter < 100; iter++) {
+        Eigen::Matrix3d R_curr = paramsToRot(params.head<3>());
+        Eigen::Vector3d t_curr = params.tail<3>();
+
+        // 残差和 Jacobian
+        Eigen::VectorXd residual(3 * numPairs);
+        Eigen::MatrixXd J(3 * numPairs, 6);
+
+        row = 0;
+        for (int i = 0; i < N; i++) {
+            Eigen::Vector3d Rp_i = R_curr * p_cams[i];
+            Eigen::Vector3d base_i = R_ends[i] * (Rp_i + t_curr) + t_ends[i];
+
+            for (int j = i + 1; j < N; j++) {
+                Eigen::Vector3d Rp_j = R_curr * p_cams[j];
+                Eigen::Vector3d base_j = R_ends[j] * (Rp_j + t_curr) + t_ends[j];
+
+                residual.segment<3>(row) = base_i - base_j;
+
+                // dr/dw = -R_i * skew(Rp_i) + R_j * skew(Rp_j)
+                J.block<3, 3>(row, 0) = -R_ends[i] * skew(Rp_i) + R_ends[j] * skew(Rp_j);
+                // dr/dt = R_i - R_j
+                J.block<3, 3>(row, 3) = R_ends[i] - R_ends[j];
+
+                row += 3;
             }
         }
 
-        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>());
+        double cost = 0.5 * residual.squaredNorm();
+        Eigen::MatrixXd JtJ = J.transpose() * J;
+        Eigen::VectorXd Jtr = J.transpose() * residual;
+        Eigen::VectorXd prev_params = params;
+
+        // LM 自适应阻尼更新
+        bool accepted = false;
+        for (int lm = 0; lm < 8; lm++) {
+            Eigen::MatrixXd damped = JtJ;
+            damped.diagonal() += lambda * JtJ.diagonal();
+            Eigen::VectorXd delta = damped.ldlt().solve(Jtr);
+            Eigen::VectorXd trial = prev_params - delta;
+
+            Eigen::Matrix3d R_trial = paramsToRot(trial.head<3>());
+            double newCost = computeCost(R_trial, trial.tail<3>());
+
+            if (newCost < cost) {
+                params = trial;
+                lambda = std::max(lambda * 0.1, 1e-12);
+                accepted = true;
+                break;
+            } else {
+                lambda *= 10;
+            }
+        }
+
+        if (!accepted) break;
+        if ((params - prev_params).norm() < 1e-10) break;
     }
 
-    // 计算基座点的离散程度作为误差
-    Eigen::Vector3d meanBase = Eigen::Vector3d::Zero();
-    for (const auto& p : basePoints) {
-        meanBase += p;
+    // 提取最终结果
+    Eigen::Matrix3d R_final = paramsToRot(params.head<3>());
+    Eigen::Vector3d t_final = params.tail<3>();
+
+    for (int i = 0; i < 3; i++)
+        for (int j = 0; j < 3; j++)
+            result.R.at(i, j) = R_final(i, j);
+    result.T = HECTranslationVector(t_final(0), t_final(1), t_final(2));
+
+    // ================================================================
+    // 误差计算：各观测映射到基座坐标系的离散程度
+    // ================================================================
+    Eigen::Matrix4d T_result = Eigen::Matrix4d::Identity();
+    T_result.block<3, 3>(0, 0) = R_final;
+    T_result.block<3, 1>(0, 3) = t_final;
+
+    std::vector<Eigen::Vector3d> basePoints(N);
+    for (int i = 0; i < N; i++) {
+        Eigen::Vector4d ph(p_cams[i](0), p_cams[i](1), p_cams[i](2), 1.0);
+        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);
+        basePoints[i] = (T_end * T_result * ph).head<3>();
     }
+
+    Eigen::Vector3d meanBase = Eigen::Vector3d::Zero();
+    for (const auto& p : basePoints) meanBase += p;
     meanBase /= N;
 
+    double totalError = 0, maxErr = 0, minErr = std::numeric_limits<double>::max();
     for (const auto& p : basePoints) {
-        double error = (p - meanBase).norm();
-        totalError += error;
-        if (error > maxErr) maxErr = error;
-        if (error < minErr) minErr = error;
+        double err = (p - meanBase).norm();
+        totalError += err;
+        if (err > maxErr) maxErr = err;
+        if (err < minErr) minErr = err;
     }
     result.error = totalError / N;
     result.maxError = maxErr;
     result.minError = minErr;
-
     result.centerEye = HECPoint3D(0, 0, 0);
     result.centerRobot = HECPoint3D(meanBase(0), meanBase(1), meanBase(2));
 

Tools/CalibView/Src/CalibDataWidget.cpp

@@ -465,12 +465,13 @@ void CalibDataWidget::getEyeInHandData(std::vector<HECEyeInHandData>& calibData)
             m_tableHandEye->item(row, 1)->text().toDouble() : 0;
         double endZ = m_tableHandEye->item(row, 2) ?
             m_tableHandEye->item(row, 2)->text().toDouble() : 0;
+        // 机器人姿态是角度，需要转换为弧度
         double endRoll = m_tableHandEye->item(row, 3) ?
-            m_tableHandEye->item(row, 3)->text().toDouble() : 0;
+            m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0;
         double endPitch = m_tableHandEye->item(row, 4) ?
-            m_tableHandEye->item(row, 4)->text().toDouble() : 0;
+            m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0;
         double endYaw = m_tableHandEye->item(row, 5) ?
-            m_tableHandEye->item(row, 5)->text().toDouble() : 0;
+            m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0;
 
         // 使用标定接口统一转换欧拉角到旋转矩阵
         HECEulerAngles endAngles(endRoll, endPitch, endYaw);