diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.h b/App/WorkpieceHole/WorkpieceHoleApp/Version.h index cb12ef40..ff7ad10f 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.h +++ b/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 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.md b/App/WorkpieceHole/WorkpieceHoleApp/Version.md index 7f408ace..7fcbf391 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.md +++ b/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. 协议和页面显示倒叙输出 diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so index 69efab54..b4494e23 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so differ diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so index 570cf783..42c8fce6 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll index 79bb1c51..2b87b39b 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib index be5f2797..4966ca62 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb index 563db752..3cab1706 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll index 7c3c00ec..e420341b 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb index 81576875..efee67c1 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll index ef9c835a..5898d96a 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib index ab0fd44d..0a90c88c 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb index 0016ec31..f1cc1e1d 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll index 5a91af7a..21e745e6 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb index 52a2e861..00cf6604 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb differ diff --git a/Module/HandEyeCalib/Src/HandEyeCalib.cpp b/Module/HandEyeCalib/Src/HandEyeCalib.cpp index 5b23658b..f55af349 100644 --- a/Module/HandEyeCalib/Src/HandEyeCalib.cpp +++ b/Module/HandEyeCalib/Src/HandEyeCalib.cpp @@ -376,171 +376,247 @@ int HandEyeCalib::CalculateEyeInHand( HECCalibResult& result) { int N = static_cast(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 R_ends(N); + std::vector t_ends(N); + std::vector 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 A_rot, B_rot; - std::vector 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 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 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 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::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 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 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::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)); diff --git a/Tools/CalibView/Src/CalibDataWidget.cpp b/Tools/CalibView/Src/CalibDataWidget.cpp index fe58f547..2b312242 100644 --- a/Tools/CalibView/Src/CalibDataWidget.cpp +++ b/Tools/CalibView/Src/CalibDataWidget.cpp @@ -465,12 +465,13 @@ void CalibDataWidget::getEyeInHandData(std::vector& 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);