diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h index 34789db..0936d2f 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h @@ -22,7 +22,10 @@ struct RobotPose6D; * * eulerOrder 同时用作机器人法兰姿态的输入分解 (rx/ry/rz → 旋转矩阵) * 以及工具姿态输出的合成 (旋转矩阵 → roll/pitch/yaw)； - * rotX/Y/Z 在 Eye 坐标系下按 Rx*Ry*Rz 顺序对工具三轴做补偿旋转。 + * rotX/Y/Z 在 Eye 坐标系下按 Rx*Ry*Rz 顺序对工具三轴做补偿旋转（手眼变换之前）。 + * outRotX/Y/Z 在 Robot 坐标系下按 Rx*Ry*Rz 顺序对工具三轴做补偿旋转（手眼变换之后、提欧拉角之前）， + * 用于把"算法定义的工具系"修正到"机器人期望的 TCP 系"，同时让输出位姿远离万向节锁。 + * 默认 0 不启用；具体值应由示教位姿反推得到，不要照搬某次测量值。 * approachOffset 是机械臂接近点相对目标点沿目标姿态反方向的偏移量（mm）。 */ struct HandEyeExtrinsic @@ -32,6 +35,9 @@ struct HandEyeExtrinsic double rotY = 0.0; double rotZ = 0.0; double approachOffset = 0.0; + double outRotX = 0.0; + double outRotY = 0.0; + double outRotZ = 15.0; }; class DetectPresenter diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp index 1c421e9..f63c6db 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp @@ -458,8 +458,9 @@ int DetectPresenter::DetectScrew( growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum, growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", filterParam.continuityTh, filterParam.outlierTh); - LOG_INFO("[Algo Thread] Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f\n", - extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ); + LOG_INFO("[Algo Thread] Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f, outRotX=%.3f, outRotY=%.3f, outRotZ=%.3f\n", + extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ, + extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); } int errCode = 0; @@ -550,6 +551,9 @@ int DetectPresenter::DetectScrew( LOG_WARNING("[Algo Thread] Screw %zu: 轴变换到机器人坐标系失败，已跳过\n", i); continue; } + // Robot 系下对工具三轴做后补偿：把"算法工具系"修正到"机器人期望 TCP 系"， + // 同时让最终输出欧拉角远离万向节锁；默认 0 不启用。 + ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes); ScrewPosition pos; @@ -626,8 +630,9 @@ int DetectPresenter::DetectToolDisk( LOG_INFO("[Algo Thread] ToolDisk Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f\n", cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z); - LOG_INFO("[Algo Thread] ToolDisk Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f\n", - extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ); + LOG_INFO("[Algo Thread] ToolDisk Pose Config: eulerOrder=%d, poseOutputOrder=%d, rotX=%.3f, rotY=%.3f, rotZ=%.3f, outRotX=%.3f, outRotY=%.3f, outRotZ=%.3f\n", + extrinsic.eulerOrder, poseOutputOrder, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ, + extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); } int errCode = 0; @@ -691,6 +696,9 @@ int DetectPresenter::DetectToolDisk( LOG_WARNING("[Algo Thread] ToolDisk: 轴变换到机器人坐标系失败\n"); return ERR_CODE(DEV_DATA_INVALID); } + // Robot 系下对工具三轴做后补偿：把"算法工具系"修正到"机器人期望 TCP 系"， + // 同时让最终输出欧拉角远离万向节锁；默认 0 不启用。 + ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes); RotationMatrixToConfiguredEulerDegrees(robotRotation, order, pos.roll, pos.pitch, pos.yaw); // 拍照姿态作参考，把 gimbal lock / 多解区的解拉回拍照姿态附近，减少机械臂旋转量。 diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp index 2496d5b..f4f3e5b 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp @@ -236,7 +236,7 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vectorDoubleAttribute("rotZ"); if (handEyeElement->Attribute("approachOffset")) calibMatrix.approachOffset = handEyeElement->DoubleAttribute("approachOffset"); + if (handEyeElement->Attribute("outRotX")) + calibMatrix.outRotX = handEyeElement->DoubleAttribute("outRotX"); + if (handEyeElement->Attribute("outRotY")) + calibMatrix.outRotY = handEyeElement->DoubleAttribute("outRotY"); + if (handEyeElement->Attribute("outRotZ")) + calibMatrix.outRotZ = handEyeElement->DoubleAttribute("outRotZ"); } // ============ 手眼标定矩阵 ============ @@ -98,6 +104,9 @@ void SaveHandEyeCalibMatrixs(XMLDocument& doc, handEyeElement->SetAttribute("rotX", calibMatrix.rotX); handEyeElement->SetAttribute("rotY", calibMatrix.rotY); handEyeElement->SetAttribute("rotZ", calibMatrix.rotZ); + handEyeElement->SetAttribute("outRotX", calibMatrix.outRotX); + handEyeElement->SetAttribute("outRotY", calibMatrix.outRotY); + handEyeElement->SetAttribute("outRotZ", calibMatrix.outRotZ); handEyeElement->SetAttribute("approachOffset", calibMatrix.approachOffset); containerElement->InsertEndChild(handEyeElement); } diff --git a/GrabBagPrj/AppList.md b/GrabBagPrj/AppList.md index 8f4973f..8db003d 100644 --- a/GrabBagPrj/AppList.md +++ b/GrabBagPrj/AppList.md @@ -15,8 +15,8 @@ | 9 | 颗粒尺寸检测 | ParticleSize | 1.0.0.0 | | 10 | 双目标记检测 | BinocularMarkServer | 1.0.0.4 | | 11 | 铁路隧道槽道测量 | TunnelChannel | 1.0.0.3 | -| 12 | 螺杆定位 | ScrewPosition | 1.1.5.2 | +| 12 | 螺杆定位 | ScrewPosition | 1.1.5.3 | | 13 | 包裹拆线位置定位 | BagThreadPosition | 1.0.0.4 | -| 14 | 工件孔定位 | WorkpieceHole | 1.1.2.1 | +| 14 | 工件孔定位 | WorkpieceHole | 1.1.3.1 | | 16 | 坑孔定位 | HolePitPosition | 无 | diff --git a/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h b/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h index ed6df6b..78a55e4 100644 --- a/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h +++ b/Module/HandEyeCalib/Inc/HandEyeCalibTypes.h @@ -337,6 +337,20 @@ struct HECEyeInHandData : endPose(pose), targetInCamera(target) {} }; +/** + * @brief 眼在手上标定输入数据（完整位姿） + * 包含机器人末端位姿和标定板在相机坐标系的完整位姿 + */ +struct HECEyeInHandPoseData +{ + HECHomogeneousMatrix endPose; // 机器人末端位姿 (相对于基座) + HECHomogeneousMatrix boardInCamera; // 标定板在相机坐标系的位姿 + + HECEyeInHandPoseData() = default; + HECEyeInHandPoseData(const HECHomogeneousMatrix& end, const HECHomogeneousMatrix& board) + : endPose(end), boardInCamera(board) {} +}; + /** * @brief 眼在手外标定输入数据（完整位姿） * 包含机器人末端位姿和标定板在相机坐标系的完整位姿 diff --git a/Module/HandEyeCalib/Inc/IHandEyeCalib.h b/Module/HandEyeCalib/Inc/IHandEyeCalib.h index 847d913..9fc797b 100644 --- a/Module/HandEyeCalib/Inc/IHandEyeCalib.h +++ b/Module/HandEyeCalib/Inc/IHandEyeCalib.h @@ -53,6 +53,9 @@ public: virtual int CalculateEyeInHand(const std::vector& calibData, HECCalibResult& result) = 0; + virtual int CalculateEyeInHandWithPose(const std::vector& calibData, + HECCalibResult& result) = 0; + virtual HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) = 0; virtual int CalculateEyeToHandWithPose(const std::vector& calibData, diff --git a/Module/HandEyeCalib/Src/HandEyeCalib.cpp b/Module/HandEyeCalib/Src/HandEyeCalib.cpp index f55af34..23a2fcf 100644 --- a/Module/HandEyeCalib/Src/HandEyeCalib.cpp +++ b/Module/HandEyeCalib/Src/HandEyeCalib.cpp @@ -1,7 +1,3 @@ -#ifndef HANDEYECALIB_LIBRARY -#define HANDEYECALIB_LIBRARY -#endif - #include "HandEyeCalib.h" #include "VrError.h" #include "VrLog.h" @@ -623,6 +619,162 @@ int HandEyeCalib::CalculateEyeInHand( return SUCCESS; } +int HandEyeCalib::CalculateEyeInHandWithPose( + const std::vector& calibData, + HECCalibResult& result) +{ + const int N = static_cast(calibData.size()); + if (N < 3) { + return ERR_CODE(APP_ERR_PARAM); + } + + // Eye-In-Hand: + // T_base_to_end_i * X * T_board_in_cam_i = T_base_to_board + // X 为相机到末端的固定变换，构造 A_i_j * X = X * B_i_j. + std::vector A_rot, B_rot; + std::vector A_trans, B_trans; + + for (int i = 0; i < N; ++i) { + for (int j = i + 1; j < N; ++j) { + Eigen::Matrix4d T_end_i = Eigen::Matrix4d::Identity(); + Eigen::Matrix4d T_end_j = Eigen::Matrix4d::Identity(); + Eigen::Matrix4d T_board_i = Eigen::Matrix4d::Identity(); + Eigen::Matrix4d T_board_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[j].endPose.at(r, c); + T_board_i(r, c) = calibData[i].boardInCamera.at(r, c); + T_board_j(r, c) = calibData[j].boardInCamera.at(r, c); + } + } + + const Eigen::Matrix4d A = T_end_i.inverse() * T_end_j; + const Eigen::Matrix4d B = T_board_i * T_board_j.inverse(); + + A_rot.push_back(A.block<3, 3>(0, 0)); + A_trans.push_back(A.block<3, 1>(0, 3)); + B_rot.push_back(B.block<3, 3>(0, 0)); + B_trans.push_back(B.block<3, 1>(0, 3)); + } + } + + const int numPairs = static_cast(A_rot.size()); + Eigen::MatrixXd M(9 * numPairs, 9); + M.setZero(); + + for (int i = 0; i < numPairs; ++i) { + const Eigen::Matrix3d& Ai = A_rot[i]; + const Eigen::Matrix3d& Bi = B_rot[i]; + + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + const int row = i * 9 + r * 3 + c; + for (int k = 0; k < 3; ++k) { + M(row, k * 3 + c) += Ai(r, k); + M(row, r * 3 + k) -= Bi(k, c); + } + } + } + } + + Eigen::JacobiSVD svd(M, Eigen::ComputeFullV); + const 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 svd_R(R_raw, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d R_cam_to_end = svd_R.matrixU() * svd_R.matrixV().transpose(); + if (R_cam_to_end.determinant() < 0) { + R_cam_to_end = -R_cam_to_end; + } + + Eigen::MatrixXd C(3 * numPairs, 3); + Eigen::VectorXd d(3 * numPairs); + + for (int i = 0; i < numPairs; ++i) { + C.block<3, 3>(i * 3, 0) = A_rot[i] - Eigen::Matrix3d::Identity(); + d.segment<3>(i * 3) = R_cam_to_end * B_trans[i] - A_trans[i]; + } + + const Eigen::Vector3d t_cam_to_end = + C.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(d); + + double totalError = 0.0; + double maxErr = 0.0; + double minErr = std::numeric_limits::max(); + + Eigen::Matrix4d T_cam_to_end = Eigen::Matrix4d::Identity(); + T_cam_to_end.block<3, 3>(0, 0) = R_cam_to_end; + T_cam_to_end.block<3, 1>(0, 3) = t_cam_to_end; + + std::vector boardInBasePoints; + boardInBasePoints.reserve(N); + + for (int i = 0; i < N; ++i) { + Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity(); + Eigen::Matrix4d T_board = 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); + T_board(r, c) = calibData[i].boardInCamera.at(r, c); + } + } + + const Eigen::Matrix4d T_board_in_base = T_end * T_cam_to_end * T_board; + boardInBasePoints.push_back(T_board_in_base.block<3, 1>(0, 3)); + } + + Eigen::Vector3d meanBoardInBase = Eigen::Vector3d::Zero(); + for (const auto& p : boardInBasePoints) { + meanBoardInBase += p; + } + meanBoardInBase /= N; + + for (const auto& p : boardInBasePoints) { + const double error = (p - meanBoardInBase).norm(); + totalError += error; + if (error > maxErr) maxErr = error; + if (error < minErr) minErr = error; + } + + result.error = totalError / N; + result.maxError = maxErr; + result.minError = minErr; + result.centerEye = HECPoint3D(0, 0, 0); + result.centerRobot = HECPoint3D(meanBoardInBase(0), meanBoardInBase(1), meanBoardInBase(2)); + +#if 0 + + // 统一对外返回 camera -> end，与点法和业务侧 sixAxisEyeInHandBuildTransform 保持一致。 + const Eigen::Matrix3d R_cam_to_end = R_end_to_cam.transpose(); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + result.R.at(i, j) = R_cam_to_end(i, j); + } + } + result.T = HECTranslationVector(t_cam_to_end(0), t_cam_to_end(1), t_cam_to_end(2)); + + return SUCCESS; +} + +#endif + // Return the hand-eye matrix in camera -> end form. + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + result.R.at(i, j) = R_cam_to_end(i, j); + } + } + result.T = HECTranslationVector(t_cam_to_end(0), t_cam_to_end(1), t_cam_to_end(2)); + + return SUCCESS; +} + Eigen::Matrix3d HandEyeCalib::eulerToRotationMatrix(double rx, double ry, double rz, HECEulerOrder order) { // 将角度转为弧度 diff --git a/Module/HandEyeCalib/_Inc/HandEyeCalib.h b/Module/HandEyeCalib/_Inc/HandEyeCalib.h index 5d3386a..8c45bfe 100644 --- a/Module/HandEyeCalib/_Inc/HandEyeCalib.h +++ b/Module/HandEyeCalib/_Inc/HandEyeCalib.h @@ -51,6 +51,9 @@ public: int CalculateEyeInHand(const std::vector& calibData, HECCalibResult& result) override; + int CalculateEyeInHandWithPose(const std::vector& calibData, + HECCalibResult& result) override; + HECTCPCalibResult CalculateTCP(const HECTCPCalibData& data) override; int CalculateEyeToHandWithPose(const std::vector& calibData, diff --git a/Tools/CalibView/Inc/BatchVerifyDialog.h b/Tools/CalibView/Inc/BatchVerifyDialog.h index f50a8e3..5770886 100644 --- a/Tools/CalibView/Inc/BatchVerifyDialog.h +++ b/Tools/CalibView/Inc/BatchVerifyDialog.h @@ -9,8 +9,10 @@ #include #include #include +#include #include #include +#include "IHandEyeCalib.h" class IChessboardDetector; class IHandEyeCalib; @@ -23,7 +25,8 @@ struct BatchVerifyItem QString leftImagePath; // 左目图像路径 QString rightImagePath; // 右目图像路径 double robotX, robotY, robotZ; // 机械臂坐标 - double robotRx, robotRy, robotRz; // 机械臂姿态 + double robotRx, robotRy, robotRz; // 机械臂姿态(欧拉角,度) + double robotQw, robotQx, robotQy, robotQz; // 机械臂姿态(四元数) // 检测结果 bool detected; @@ -33,6 +36,16 @@ struct BatchVerifyItem // 验证结果 double errorX, errorY, errorZ; // 误差 double errorTotal; // 总误差 + + BatchVerifyItem() + : robotX(0), robotY(0), robotZ(0) + , robotRx(0), robotRy(0), robotRz(0) + , robotQw(1), robotQx(0), robotQy(0), robotQz(0) + , detected(false) + , camX(0), camY(0), camZ(0) + , camRx(0), camRy(0), camRz(0) + , errorX(0), errorY(0), errorZ(0) + , errorTotal(0) {} }; /** @@ -97,10 +110,21 @@ private: bool detectImagePair(BatchVerifyItem& item); /** - * @brief 计算验证误差 + * @brief 计算验证误差（直接比较模式） */ void calculateError(BatchVerifyItem& item); + /** + * @brief 执行 Eye-In-Hand 标定并计算一致性误差 + */ + void calculateEyeInHandErrors(); + + /** + * @brief 四元数转旋转矩阵 + */ + static void quatToRotationMatrix(double qw, double qx, double qy, double qz, + HECRotationMatrix& R); + /** * @brief 更新表格显示 */ @@ -136,6 +160,13 @@ private: QDoubleSpinBox* m_sbCx; QDoubleSpinBox* m_sbCy; + // 标定类型选择 + QComboBox* m_cbCalibType; + + // 标定结果 + HECCalibResult m_calibResult; + bool m_calibSuccess; + // 数据 std::vector m_items; QString m_currentDirectory; diff --git a/Tools/CalibView/Inc/CalibDataWidget.h b/Tools/CalibView/Inc/CalibDataWidget.h index 72f3e55..84022e4 100644 --- a/Tools/CalibView/Inc/CalibDataWidget.h +++ b/Tools/CalibView/Inc/CalibDataWidget.h @@ -42,6 +42,25 @@ public: */ void getEyeInHandData(std::vector& calibData) const; + /** + * @brief 获取 Eye-In-Hand 完整位姿数据（用于 Park 方法） + */ + void getEyeInHandPoseData(std::vector& calibData) const; + + /** + * @brief 按指定机器人欧拉角顺序获取 Eye-In-Hand 完整位姿数据 + */ + void getEyeInHandPoseData(std::vector& calibData, + HECEulerOrder robotEulerOrder) const; + + /** + * @brief 按指定机器人/标定板欧拉角顺序与位姿方向获取 Eye-In-Hand 完整位姿数据 + */ + void getEyeInHandPoseData(std::vector& calibData, + HECEulerOrder robotEulerOrder, + HECEulerOrder boardEulerOrder, + bool invertBoardPose) const; + /** * @brief 获取当前标定模式 */ diff --git a/Tools/CalibView/Src/BatchVerifyDialog.cpp b/Tools/CalibView/Src/BatchVerifyDialog.cpp index 7e35e81..a369d5b 100644 --- a/Tools/CalibView/Src/BatchVerifyDialog.cpp +++ b/Tools/CalibView/Src/BatchVerifyDialog.cpp @@ -45,6 +45,8 @@ BatchVerifyDialog::BatchVerifyDialog(IChessboardDetector* detector, , m_sbFy(nullptr) , m_sbCx(nullptr) , m_sbCy(nullptr) + , m_cbCalibType(nullptr) + , m_calibSuccess(false) , m_isVerifying(false) { setupUI(); @@ -123,6 +125,13 @@ void BatchVerifyDialog::setupUI() m_sbCy->setValue(512.0); paramLayout->addWidget(m_sbCy, 2, 1); + // 标定类型选择 + paramLayout->addWidget(new QLabel("标定类型:", this), 2, 2); + m_cbCalibType = new QComboBox(this); + m_cbCalibType->addItem("Eye-In-Hand (眼在手上)"); + m_cbCalibType->addItem("Eye-To-Hand (眼在手外)"); + paramLayout->addWidget(m_cbCalibType, 2, 3, 1, 3); + mainLayout->addWidget(paramGroup); // 控制按钮 @@ -287,6 +296,11 @@ bool BatchVerifyDialog::scanDirectory(const QString& dirPath) double qy = rotation["y"].toDouble(); double qz = rotation["z"].toDouble(); + item.robotQw = qw; + item.robotQx = qx; + item.robotQy = qy; + item.robotQz = qz; + // 四元数转欧拉角 (ZYX顺序，单位：度) // Roll (X轴旋转) double sinr_cosp = 2.0 * (qw * qx + qy * qz); @@ -332,11 +346,14 @@ void BatchVerifyDialog::onStartVerify() } m_isVerifying = true; + m_calibSuccess = false; m_btnStartVerify->setEnabled(false); m_btnStopVerify->setEnabled(true); m_btnExport->setEnabled(false); - appendLog("开始批量验证..."); + int calibType = m_cbCalibType->currentIndex(); + appendLog(QString("开始批量验证 (模式: %1)...") + .arg(calibType == 0 ? "Eye-In-Hand" : "Eye-To-Hand")); // 设置检测参数 m_detector->SetDetectionFlags(true, true, false); @@ -344,33 +361,50 @@ void BatchVerifyDialog::onStartVerify() int successCount = 0; int totalCount = m_items.size(); + // 阶段1：检测所有图像对 for (size_t i = 0; i < m_items.size(); ++i) { if (!m_isVerifying) { appendLog("验证已停止"); break; } - appendLog(QString("正在验证第 %1/%2 组数据...").arg(i + 1).arg(totalCount)); + appendLog(QString("正在检测第 %1/%2 组数据...").arg(i + 1).arg(totalCount)); if (detectImagePair(m_items[i])) { - calculateError(m_items[i]); successCount++; } - // 更新进度 - m_progressBar->setValue((i + 1) * 100 / totalCount); + // 更新进度（检测阶段占50%） + m_progressBar->setValue((i + 1) * 50 / totalCount); updateTable(); // 处理事件，保持界面响应 QApplication::processEvents(); } + // 阶段2：误差计算 + if (calibType == 0) { + // Eye-In-Hand：先标定再计算一致性误差 + appendLog(QString("检测完成: %1/%2 成功，正在执行 Eye-In-Hand 标定...").arg(successCount).arg(totalCount)); + calculateEyeInHandErrors(); + } else { + // Eye-To-Hand：直接比较（相机坐标系已标定到基座坐标系的情况） + for (size_t i = 0; i < m_items.size(); ++i) { + if (m_items[i].detected) { + calculateError(m_items[i]); + } + } + } + + m_progressBar->setValue(100); + updateTable(); + m_isVerifying = false; m_btnStartVerify->setEnabled(true); m_btnStopVerify->setEnabled(false); m_btnExport->setEnabled(true); - appendLog(QString("验证完成: 成功 %1/%2").arg(successCount).arg(totalCount)); + appendLog(QString("验证完成: 检测成功 %1/%2").arg(successCount).arg(totalCount)); } void BatchVerifyDialog::onStopVerify() @@ -457,7 +491,7 @@ void BatchVerifyDialog::calculateError(BatchVerifyItem& item) return; } - // 计算位置误差 + // 直接比较模式（仅适用于相机坐标系已对齐到机器人基座坐标系的情况） item.errorX = item.camX - item.robotX; item.errorY = item.camY - item.robotY; item.errorZ = item.camZ - item.robotZ; @@ -466,6 +500,128 @@ void BatchVerifyDialog::calculateError(BatchVerifyItem& item) item.errorZ * item.errorZ); } +void BatchVerifyDialog::quatToRotationMatrix(double qw, double qx, double qy, double qz, + HECRotationMatrix& R) +{ + R.at(0, 0) = 1 - 2*(qy*qy + qz*qz); + R.at(0, 1) = 2*(qx*qy - qz*qw); + R.at(0, 2) = 2*(qx*qz + qy*qw); + R.at(1, 0) = 2*(qx*qy + qz*qw); + R.at(1, 1) = 1 - 2*(qx*qx + qz*qz); + R.at(1, 2) = 2*(qy*qz - qx*qw); + R.at(2, 0) = 2*(qx*qz - qy*qw); + R.at(2, 1) = 2*(qy*qz + qx*qw); + R.at(2, 2) = 1 - 2*(qx*qx + qy*qy); +} + +void BatchVerifyDialog::calculateEyeInHandErrors() +{ + // 收集所有成功检测的数据 + std::vector calibData; + std::vector detectedIndices; + + for (size_t i = 0; i < m_items.size(); ++i) { + if (!m_items[i].detected) continue; + + // 从四元数构建末端位姿矩阵 + HECRotationMatrix R_end; + quatToRotationMatrix(m_items[i].robotQw, m_items[i].robotQx, + m_items[i].robotQy, m_items[i].robotQz, R_end); + HECTranslationVector T_end(m_items[i].robotX, m_items[i].robotY, m_items[i].robotZ); + HECHomogeneousMatrix endPose(R_end, T_end); + + HECPoint3D target(m_items[i].camX, m_items[i].camY, m_items[i].camZ); + + calibData.push_back(HECEyeInHandData(endPose, target)); + detectedIndices.push_back(static_cast(i)); + } + + if (calibData.size() < 3) { + appendLog("有效检测数据不足3组，无法进行 Eye-In-Hand 标定验证"); + return; + } + + appendLog(QString("使用 %1 组数据进行 Eye-In-Hand 标定...").arg(calibData.size())); + + // 执行手眼标定 + int ret = m_calib->CalculateEyeInHand(calibData, m_calibResult); + if (ret != 0) { + appendLog(QString("Eye-In-Hand 标定失败，错误码: %1").arg(ret)); + return; + } + + m_calibSuccess = true; + + // 输出标定结果 + appendLog("=== Eye-In-Hand 标定结果 ==="); + appendLog(QString("旋转矩阵 R:")); + appendLog(QString(" [%1, %2, %3]") + .arg(m_calibResult.R.at(0,0), 0, 'f', 6) + .arg(m_calibResult.R.at(0,1), 0, 'f', 6) + .arg(m_calibResult.R.at(0,2), 0, 'f', 6)); + appendLog(QString(" [%1, %2, %3]") + .arg(m_calibResult.R.at(1,0), 0, 'f', 6) + .arg(m_calibResult.R.at(1,1), 0, 'f', 6) + .arg(m_calibResult.R.at(1,2), 0, 'f', 6)); + appendLog(QString(" [%1, %2, %3]") + .arg(m_calibResult.R.at(2,0), 0, 'f', 6) + .arg(m_calibResult.R.at(2,1), 0, 'f', 6) + .arg(m_calibResult.R.at(2,2), 0, 'f', 6)); + appendLog(QString("平移向量 T: [%1, %2, %3] mm") + .arg(m_calibResult.T.at(0), 0, 'f', 3) + .arg(m_calibResult.T.at(1), 0, 'f', 3) + .arg(m_calibResult.T.at(2), 0, 'f', 3)); + appendLog(QString("标定平均误差: %1 mm").arg(m_calibResult.error, 0, 'f', 4)); + appendLog(QString("标定最大误差: %1 mm").arg(m_calibResult.maxError, 0, 'f', 4)); + appendLog(QString("标定最小误差: %1 mm").arg(m_calibResult.minError, 0, 'f', 4)); + + // 使用标定结果计算每个观测映射到基座坐标系的点 + // P_base_i = T_end_i * X * P_cam_i（标定板固定，所有映射点应趋于同一位置） + HECHomogeneousMatrix X(m_calibResult.R, m_calibResult.T); + int N = static_cast(calibData.size()); + + std::vector mappedPositions(N); + for (int i = 0; i < N; ++i) { + // P_base = T_end * X * P_cam + HECPoint3D p_in_end = X.transformPoint(calibData[i].targetInCamera); + mappedPositions[i] = calibData[i].endPose.transformPoint(p_in_end); + } + + // 计算映射点的均值 + HECPoint3D meanPos(0, 0, 0); + for (const auto& p : mappedPositions) { + meanPos += p; + } + meanPos = meanPos / static_cast(N); + + appendLog(QString("标定板在基座坐标系的映射位置: (%1, %2, %3)") + .arg(meanPos.x, 0, 'f', 3) + .arg(meanPos.y, 0, 'f', 3) + .arg(meanPos.z, 0, 'f', 3)); + + // 计算每个样本的一致性误差 + for (int i = 0; i < N; ++i) { + int idx = detectedIndices[i]; + m_items[idx].errorX = mappedPositions[i].x - meanPos.x; + m_items[idx].errorY = mappedPositions[i].y - meanPos.y; + m_items[idx].errorZ = mappedPositions[i].z - meanPos.z; + m_items[idx].errorTotal = (mappedPositions[i] - meanPos).norm(); + } + + // 计算并显示一致性误差统计 + double totalErr = 0, maxErr = 0, minErr = 1e9; + for (int i = 0; i < N; ++i) { + double e = m_items[detectedIndices[i]].errorTotal; + totalErr += e; + if (e > maxErr) maxErr = e; + if (e < minErr) minErr = e; + } + appendLog(QString("一致性误差 - 平均: %1 mm, 最大: %2 mm, 最小: %3 mm") + .arg(totalErr / N, 0, 'f', 4) + .arg(maxErr, 0, 'f', 4) + .arg(minErr, 0, 'f', 4)); +} + void BatchVerifyDialog::updateTable() { m_tableResults->setRowCount(m_items.size()); @@ -523,6 +679,30 @@ void BatchVerifyDialog::onExportResults() QTextStream out(&file); out.setCodec("UTF-8"); + // Eye-In-Hand 模式下先输出标定结果 + if (m_cbCalibType->currentIndex() == 0 && m_calibSuccess) { + out << "# Eye-In-Hand 标定结果\n"; + out << "# 旋转矩阵 R:\n"; + out << QString("# [%1, %2, %3]\n") + .arg(m_calibResult.R.at(0,0), 0, 'f', 6) + .arg(m_calibResult.R.at(0,1), 0, 'f', 6) + .arg(m_calibResult.R.at(0,2), 0, 'f', 6); + out << QString("# [%1, %2, %3]\n") + .arg(m_calibResult.R.at(1,0), 0, 'f', 6) + .arg(m_calibResult.R.at(1,1), 0, 'f', 6) + .arg(m_calibResult.R.at(1,2), 0, 'f', 6); + out << QString("# [%1, %2, %3]\n") + .arg(m_calibResult.R.at(2,0), 0, 'f', 6) + .arg(m_calibResult.R.at(2,1), 0, 'f', 6) + .arg(m_calibResult.R.at(2,2), 0, 'f', 6); + out << QString("# 平移向量 T: [%1, %2, %3] mm\n") + .arg(m_calibResult.T.at(0), 0, 'f', 3) + .arg(m_calibResult.T.at(1), 0, 'f', 3) + .arg(m_calibResult.T.at(2), 0, 'f', 3); + out << QString("# 标定平均误差: %1 mm\n").arg(m_calibResult.error, 0, 'f', 4); + out << "#\n"; + } + // 写入表头 out << "序号,左目图像,右目图像,机械臂X,机械臂Y,机械臂Z,检测X,检测Y,检测Z,误差X,误差Y,误差Z,总误差\n"; diff --git a/Tools/CalibView/Src/CalibDataWidget.cpp b/Tools/CalibView/Src/CalibDataWidget.cpp index 2b31224..9d341f6 100644 --- a/Tools/CalibView/Src/CalibDataWidget.cpp +++ b/Tools/CalibView/Src/CalibDataWidget.cpp @@ -146,12 +146,12 @@ void CalibDataWidget::setupUI() modeLayout->addSpacing(20); QLabel* lblEuler = new QLabel("欧拉角顺序:", this); m_cbEulerOrder = new QComboBox(this); - m_cbEulerOrder->addItem("ZYX (默认)"); - m_cbEulerOrder->addItem("XYZ"); - m_cbEulerOrder->addItem("XZY"); - m_cbEulerOrder->addItem("YXZ"); - m_cbEulerOrder->addItem("YZX"); - m_cbEulerOrder->addItem("ZXY"); + m_cbEulerOrder->addItem("ZYX (Rz-Ry-Rx)"); + m_cbEulerOrder->addItem("XYZ (Rx-Ry-Rz)"); + m_cbEulerOrder->addItem("XZY (Rx-Rz-Ry)"); + m_cbEulerOrder->addItem("YXZ (Ry-Rx-Rz)"); + m_cbEulerOrder->addItem("YZX (Ry-Rz-Rx)"); + m_cbEulerOrder->addItem("ZXY (Rz-Rx-Ry)"); modeLayout->addWidget(lblEuler); modeLayout->addWidget(m_cbEulerOrder); @@ -494,6 +494,206 @@ void CalibDataWidget::getEyeInHandData(std::vector& calibData) DestroyHandEyeCalibInstance(calib); } +void CalibDataWidget::getEyeInHandPoseData(std::vector& calibData) const +{ + calibData.clear(); + + IHandEyeCalib* calib = CreateHandEyeCalibInstance(); + if (!calib) { + return; + } + + const HECEulerOrder eulerOrder = getEulerOrder(); + const int rowCount = m_tableHandEye->rowCount(); + + for (int row = 0; row < rowCount; ++row) { + HECEyeInHandPoseData data; + + const double endX = m_tableHandEye->item(row, 0) ? + m_tableHandEye->item(row, 0)->text().toDouble() : 0; + const double endY = m_tableHandEye->item(row, 1) ? + m_tableHandEye->item(row, 1)->text().toDouble() : 0; + const double endZ = m_tableHandEye->item(row, 2) ? + m_tableHandEye->item(row, 2)->text().toDouble() : 0; + const double endRoll = m_tableHandEye->item(row, 3) ? + m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0; + const double endPitch = m_tableHandEye->item(row, 4) ? + m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0; + const double endYaw = m_tableHandEye->item(row, 5) ? + m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0; + + HECEulerAngles endAngles(endRoll, endPitch, endYaw); + HECRotationMatrix R_end; + calib->EulerToRotationMatrix(endAngles, eulerOrder, R_end); + data.endPose = HECHomogeneousMatrix(R_end, HECTranslationVector(endX, endY, endZ)); + + const double boardX = m_tableHandEye->item(row, 6) ? + m_tableHandEye->item(row, 6)->text().toDouble() : 0; + const double boardY = m_tableHandEye->item(row, 7) ? + m_tableHandEye->item(row, 7)->text().toDouble() : 0; + const double boardZ = m_tableHandEye->item(row, 8) ? + m_tableHandEye->item(row, 8)->text().toDouble() : 0; + + double boardRoll = m_tableHandEye->item(row, 9) ? + m_tableHandEye->item(row, 9)->text().toDouble() : 0; + double boardPitch = m_tableHandEye->item(row, 10) ? + m_tableHandEye->item(row, 10)->text().toDouble() : 0; + double boardYaw = m_tableHandEye->item(row, 11) ? + m_tableHandEye->item(row, 11)->text().toDouble() : 0; + + if (m_cbCamAngleUnit->currentIndex() == 1) { + boardRoll *= M_PI / 180.0; + boardPitch *= M_PI / 180.0; + boardYaw *= M_PI / 180.0; + } + + // ChessboardDetector::DetectChessboardWithPose 固定按 ZYX 顺序输出相机观测姿态。 + // 这里不能复用机器人姿态的欧拉角顺序，否则会把旧文件中的 dExtrinsic_3..5 解释错。 + HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw); + HECEulerAngles boardAnglesFixed(boardRoll, boardPitch, boardYaw); + HECRotationMatrix R_board; + calib->EulerToRotationMatrix(boardAnglesFixed, HECEulerOrder::XYZ, R_board); + data.boardInCamera = HECHomogeneousMatrix(R_board, HECTranslationVector(boardX, boardY, boardZ)); + + calibData.push_back(data); + } + + DestroyHandEyeCalibInstance(calib); +} + +void CalibDataWidget::getEyeInHandPoseData(std::vector& calibData, + HECEulerOrder robotEulerOrder) const +{ + calibData.clear(); + + IHandEyeCalib* calib = CreateHandEyeCalibInstance(); + if (!calib) { + return; + } + + const int rowCount = m_tableHandEye->rowCount(); + + for (int row = 0; row < rowCount; ++row) { + HECEyeInHandPoseData data; + + const double endX = m_tableHandEye->item(row, 0) ? + m_tableHandEye->item(row, 0)->text().toDouble() : 0; + const double endY = m_tableHandEye->item(row, 1) ? + m_tableHandEye->item(row, 1)->text().toDouble() : 0; + const double endZ = m_tableHandEye->item(row, 2) ? + m_tableHandEye->item(row, 2)->text().toDouble() : 0; + const double endRoll = m_tableHandEye->item(row, 3) ? + m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0; + const double endPitch = m_tableHandEye->item(row, 4) ? + m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0; + const double endYaw = m_tableHandEye->item(row, 5) ? + m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0; + + HECEulerAngles endAngles(endRoll, endPitch, endYaw); + HECRotationMatrix R_end; + calib->EulerToRotationMatrix(endAngles, robotEulerOrder, R_end); + data.endPose = HECHomogeneousMatrix(R_end, HECTranslationVector(endX, endY, endZ)); + + const double boardX = m_tableHandEye->item(row, 6) ? + m_tableHandEye->item(row, 6)->text().toDouble() : 0; + const double boardY = m_tableHandEye->item(row, 7) ? + m_tableHandEye->item(row, 7)->text().toDouble() : 0; + const double boardZ = m_tableHandEye->item(row, 8) ? + m_tableHandEye->item(row, 8)->text().toDouble() : 0; + + double boardRoll = m_tableHandEye->item(row, 9) ? + m_tableHandEye->item(row, 9)->text().toDouble() : 0; + double boardPitch = m_tableHandEye->item(row, 10) ? + m_tableHandEye->item(row, 10)->text().toDouble() : 0; + double boardYaw = m_tableHandEye->item(row, 11) ? + m_tableHandEye->item(row, 11)->text().toDouble() : 0; + + if (m_cbCamAngleUnit->currentIndex() == 1) { + boardRoll *= M_PI / 180.0; + boardPitch *= M_PI / 180.0; + boardYaw *= M_PI / 180.0; + } + + HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw); + HECRotationMatrix R_board; + calib->EulerToRotationMatrix(boardAngles, HECEulerOrder::XYZ, R_board); + data.boardInCamera = HECHomogeneousMatrix(R_board, HECTranslationVector(boardX, boardY, boardZ)); + + calibData.push_back(data); + } + + DestroyHandEyeCalibInstance(calib); +} + +void CalibDataWidget::getEyeInHandPoseData(std::vector& calibData, + HECEulerOrder robotEulerOrder, + HECEulerOrder boardEulerOrder, + bool invertBoardPose) const +{ + calibData.clear(); + + IHandEyeCalib* calib = CreateHandEyeCalibInstance(); + if (!calib) { + return; + } + + const int rowCount = m_tableHandEye->rowCount(); + + for (int row = 0; row < rowCount; ++row) { + HECEyeInHandPoseData data; + + const double endX = m_tableHandEye->item(row, 0) ? + m_tableHandEye->item(row, 0)->text().toDouble() : 0; + const double endY = m_tableHandEye->item(row, 1) ? + m_tableHandEye->item(row, 1)->text().toDouble() : 0; + const double endZ = m_tableHandEye->item(row, 2) ? + m_tableHandEye->item(row, 2)->text().toDouble() : 0; + const double endRoll = m_tableHandEye->item(row, 3) ? + m_tableHandEye->item(row, 3)->text().toDouble() * M_PI / 180.0 : 0; + const double endPitch = m_tableHandEye->item(row, 4) ? + m_tableHandEye->item(row, 4)->text().toDouble() * M_PI / 180.0 : 0; + const double endYaw = m_tableHandEye->item(row, 5) ? + m_tableHandEye->item(row, 5)->text().toDouble() * M_PI / 180.0 : 0; + + HECEulerAngles endAngles(endRoll, endPitch, endYaw); + HECRotationMatrix R_end; + calib->EulerToRotationMatrix(endAngles, robotEulerOrder, R_end); + data.endPose = HECHomogeneousMatrix(R_end, HECTranslationVector(endX, endY, endZ)); + + const double boardX = m_tableHandEye->item(row, 6) ? + m_tableHandEye->item(row, 6)->text().toDouble() : 0; + const double boardY = m_tableHandEye->item(row, 7) ? + m_tableHandEye->item(row, 7)->text().toDouble() : 0; + const double boardZ = m_tableHandEye->item(row, 8) ? + m_tableHandEye->item(row, 8)->text().toDouble() : 0; + + double boardRoll = m_tableHandEye->item(row, 9) ? + m_tableHandEye->item(row, 9)->text().toDouble() : 0; + double boardPitch = m_tableHandEye->item(row, 10) ? + m_tableHandEye->item(row, 10)->text().toDouble() : 0; + double boardYaw = m_tableHandEye->item(row, 11) ? + m_tableHandEye->item(row, 11)->text().toDouble() : 0; + + if (m_cbCamAngleUnit->currentIndex() == 1) { + boardRoll *= M_PI / 180.0; + boardPitch *= M_PI / 180.0; + boardYaw *= M_PI / 180.0; + } + + HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw); + HECRotationMatrix R_board; + calib->EulerToRotationMatrix(boardAngles, boardEulerOrder, R_board); + data.boardInCamera = HECHomogeneousMatrix(R_board, HECTranslationVector(boardX, boardY, boardZ)); + if (invertBoardPose) { + data.boardInCamera = data.boardInCamera.inverse(); + } + + calibData.push_back(data); + } + + DestroyHandEyeCalibInstance(calib); +} + void CalibDataWidget::getEyeToHandPoseData(std::vector& calibData) const { calibData.clear(); @@ -560,9 +760,11 @@ void CalibDataWidget::getEyeToHandPoseData(std::vector& calibD } // 标定板姿态与机器人姿态统一使用当前选中的欧拉角顺序 + // ChessboardDetector::DetectChessboardWithPose 固定按 ZYX 顺序输出相机观测姿态。 HECEulerAngles boardAngles(boardRoll, boardPitch, boardYaw); + HECEulerAngles boardAnglesFixed(boardRoll, boardPitch, boardYaw); HECRotationMatrix R_board; - calib->EulerToRotationMatrix(boardAngles, eulerOrder, R_board); + calib->EulerToRotationMatrix(boardAnglesFixed, HECEulerOrder::XYZ, R_board); HECTranslationVector T_board(boardX, boardY, boardZ); data.boardInCamera = HECHomogeneousMatrix(R_board, T_board); @@ -863,17 +1065,17 @@ void CalibDataWidget::saveCalibData(const QString& filePath) const int mode = m_cbCalibType->currentIndex(); const int eulerOrderIndex = m_cbEulerOrder->currentIndex(); - QString axisOrder = "RzRyRx"; + // 兼容旧格式：轴序字符串直接表示姿态角的应用顺序。 + // 例如 RxRyRz 表示 RX=roll, RY=pitch, RZ=yaw。 + QString axisOrder; switch (eulerOrderIndex) { + case 0: axisOrder = "RzRyRx"; break; case 1: axisOrder = "RxRyRz"; break; case 2: axisOrder = "RxRzRy"; break; case 3: axisOrder = "RyRxRz"; break; case 4: axisOrder = "RyRzRx"; break; case 5: axisOrder = "RzRxRy"; break; - case 0: - default: - axisOrder = "RzRyRx"; - break; + default: axisOrder = "RzRyRx"; break; } ini.beginGroup("CommInfo"); @@ -1008,8 +1210,11 @@ void CalibDataWidget::loadCalibData(const QString& filePath) } else { QString axisOrder = ini.value("eRobotPosAxisOrder", "RzRyRx").toString(); - // 映射到UI的欧拉角顺序选择 - if (axisOrder == "RxRyRz") { + // 旧文件里保存的是轴顺序标签，不是矩阵乘法顺序。 + // RxRyRz 表示 RX-roll / RY-pitch / RZ-yaw。 + if (axisOrder == "RzRyRx") { + m_cbEulerOrder->setCurrentIndex(0); // ZYX + } else if (axisOrder == "RxRyRz") { m_cbEulerOrder->setCurrentIndex(1); // XYZ } else if (axisOrder == "RxRzRy") { m_cbEulerOrder->setCurrentIndex(2); // XZY @@ -1020,7 +1225,7 @@ void CalibDataWidget::loadCalibData(const QString& filePath) } else if (axisOrder == "RzRxRy") { m_cbEulerOrder->setCurrentIndex(5); // ZXY } else { - m_cbEulerOrder->setCurrentIndex(0); // ZYX / RzRyRx + m_cbEulerOrder->setCurrentIndex(0); // 默认 ZYX / RzRyRx } } diff --git a/Tools/CalibView/Src/CalibViewMainWindow.cpp b/Tools/CalibView/Src/CalibViewMainWindow.cpp index 0733522..a8eee23 100644 --- a/Tools/CalibView/Src/CalibViewMainWindow.cpp +++ b/Tools/CalibView/Src/CalibViewMainWindow.cpp @@ -23,6 +23,9 @@ #include #include +#include +#include + namespace { int parseCalibType(const QVariant& value) @@ -621,6 +624,7 @@ void CalibViewMainWindow::onEyeToHandCalibWithPose() } } +#if 0 void CalibViewMainWindow::onEyeInHandCalib() { if (!m_calib) { @@ -628,8 +632,151 @@ void CalibViewMainWindow::onEyeInHandCalib() return; } - std::vector calibData; - m_dataWidget->getEyeInHandData(calibData); + std::vector poseData; + m_dataWidget->getEyeInHandPoseData(poseData); + + bool hasBoardPose = false; + for (const auto& sample : poseData) { + for (int r = 0; r < 3 && !hasBoardPose; ++r) { + for (int c = 0; c < 3; ++c) { + const double expected = (r == c) ? 1.0 : 0.0; + if (std::abs(sample.boardInCamera.at(r, c) - expected) > 1e-6) { + hasBoardPose = true; + break; + } + } + } + if (hasBoardPose) { + break; + } + } + + std::vector pointData; + if (!hasBoardPose) { + m_dataWidget->getEyeInHandData(pointData); + } + + /* legacy pose-calibration block kept commented out while rewriting around + if (hasBoardPose) { + appendLog("开始 Eye-In-Hand 标定..."); + appendLog(QString("输入数据组数: %1").arg(poseData.size())); + + const HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder(); + QString eulerFormula; + switch (eulerOrder) { + case HECEulerOrder::XYZ: eulerFormula = "Rz*Ry*Rx"; break; + case HECEulerOrder::XZY: eulerFormula = "Ry*Rz*Rx"; break; + case HECEulerOrder::YXZ: eulerFormula = "Rz*Rx*Ry"; break; + case HECEulerOrder::YZX: eulerFormula = "Rx*Rz*Ry"; break; + case HECEulerOrder::ZXY: eulerFormula = "Ry*Rx*Rz"; break; + case HECEulerOrder::ZYX: eulerFormula = "Rx*Ry*Rz"; break; + } + + appendLog(QString("欧拉角顺序: %1 (R=%2)") + .arg(eulerOrderToString(eulerOrder)) + .arg(eulerFormula)); + appendLog("使用 Park/Tsai-Lenz 算法，利用完整位姿信息（位置+姿态）"); + + const int ret = m_calib->CalculateEyeInHandWithPose(poseData, m_currentResult); + if (ret == 0) { + m_hasResult = true; + m_resultWidget->showCalibResult(m_currentResult); +#if 0 + appendLog(QString("标定成功，误差: %1 mm") + .arg(m_currentResult.error, 0, 'f', 4)); + updateStatusBar("Eye-In-Hand 标定完成（Park 方法）"); +#endif + appendLog(QString("标定成功，误差: %1 mm") + .arg(m_currentResult.error, 0, 'f', 4)); + updateStatusBar("Eye-In-Hand Park calibration finished"); + emit calibrationCompleted(m_currentResult); + } else { + appendLog(QString("标定失败，错误码: %1").arg(ret)); + QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret)); + } + return; + } + */ + + if (hasBoardPose) { + appendLog("开始 Eye-In-Hand 标定..."); + appendLog(QString("输入数据组数: %1").arg(poseData.size())); + + const HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder(); + QString eulerFormula; + switch (eulerOrder) { + case HECEulerOrder::XYZ: eulerFormula = "Rz*Ry*Rx"; break; + case HECEulerOrder::XZY: eulerFormula = "Ry*Rz*Rx"; break; + case HECEulerOrder::YXZ: eulerFormula = "Rz*Rx*Ry"; break; + case HECEulerOrder::YZX: eulerFormula = "Rx*Rz*Ry"; break; + case HECEulerOrder::ZXY: eulerFormula = "Ry*Rx*Rz"; break; + case HECEulerOrder::ZYX: eulerFormula = "Rx*Ry*Rz"; break; + } + + appendLog(QString("欧拉角顺序: %1 (R=%2)") + .arg(eulerOrderToString(eulerOrder)) + .arg(eulerFormula)); + appendLog("使用 Park/Tsai-Lenz 算法，利用完整位姿信息（位置+姿态）"); + + const int ret = m_calib->CalculateEyeInHandWithPose(poseData, m_currentResult); + if (ret == 0) { + m_hasResult = true; + m_resultWidget->showCalibResult(m_currentResult); + appendLog(QString("标定成功，误差: %1 mm") + .arg(m_currentResult.error, 0, 'f', 4)); + + if (m_currentResult.error > 20.0) { + appendLog("当前误差仍然偏大，开始枚举机器人欧拉角顺序做诊断..."); + const HECEulerOrder probeOrders[] = { + HECEulerOrder::XYZ, + HECEulerOrder::XZY, + HECEulerOrder::YXZ, + HECEulerOrder::YZX, + HECEulerOrder::ZXY, + HECEulerOrder::ZYX + }; + for (const HECEulerOrder probeOrder : probeOrders) { + std::vector probeData; + m_dataWidget->getEyeInHandPoseData(probeData, probeOrder); + if (probeData.size() < 3) { + continue; + } + + HECCalibResult probeResult; + const int probeRet = m_calib->CalculateEyeInHandWithPose(probeData, probeResult); + if (probeRet == 0) { + appendLog(QString("顺序试探 %1 -> %2 mm") + .arg(eulerOrderToString(probeOrder)) + .arg(probeResult.error, 0, 'f', 4)); + } else { + appendLog(QString("顺序试探 %1 -> 失败(%2)") + .arg(eulerOrderToString(probeOrder)) + .arg(probeRet)); + } + } + } + +#if 0 + updateStatusBar("Eye-In-Hand 标定完成（Park 方法）"); + emit calibrationCompleted(m_currentResult); + } else { + appendLog(QString("标定失败，错误码: %1").arg(ret)); + QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret)); + } + return; + } + +#endif + updateStatusBar("Eye-In-Hand Park calibration finished"); + emit calibrationCompleted(m_currentResult); + } else { + appendLog(QString("标定失败，错误码: %1").arg(ret)); + QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret)); + } + return; + } + + const std::vector& calibData = pointData; if (calibData.size() < 3) { QMessageBox::warning(this, "警告", "至少需要3组数据进行标定"); @@ -639,6 +786,17 @@ void CalibViewMainWindow::onEyeInHandCalib() appendLog("开始 Eye-In-Hand 标定..."); appendLog(QString("输入数据组数: %1").arg(calibData.size())); + HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder(); + const char* eulerOrderNames[] = {"XYZ", "XZY", "YXZ", "YZX", "ZXY", "ZYX"}; + int orderIdx = static_cast(eulerOrder); + appendLog(QString("欧拉角顺序: %1 (R=%2)") + .arg(eulerOrderNames[orderIdx]) + .arg(orderIdx == 0 ? "Rz*Ry*Rx" : + orderIdx == 1 ? "Ry*Rz*Rx" : + orderIdx == 2 ? "Rz*Rx*Ry" : + orderIdx == 3 ? "Rx*Rz*Ry" : + orderIdx == 4 ? "Ry*Rx*Rz" : "Rx*Ry*Rz")); + int ret = m_calib->CalculateEyeInHand(calibData, m_currentResult); if (ret == 0) { @@ -654,6 +812,203 @@ void CalibViewMainWindow::onEyeInHandCalib() } } +#endif + +void CalibViewMainWindow::onEyeInHandCalib() +{ + if (!m_calib) { + QMessageBox::critical(this, "Error", "Calibration instance is not initialized"); + return; + } + + auto orderFormula = [](HECEulerOrder order) -> const char* { + switch (order) { + case HECEulerOrder::XYZ: return "Rz*Ry*Rx"; + case HECEulerOrder::XZY: return "Ry*Rz*Rx"; + case HECEulerOrder::YXZ: return "Rz*Rx*Ry"; + case HECEulerOrder::YZX: return "Rx*Rz*Ry"; + case HECEulerOrder::ZXY: return "Ry*Rx*Rz"; + case HECEulerOrder::ZYX: return "Rx*Ry*Rz"; + } + return "unknown"; + }; + + std::vector poseData; + m_dataWidget->getEyeInHandPoseData(poseData); + + bool hasBoardPose = false; + for (const auto& sample : poseData) { + if (std::abs(sample.boardInCamera.at(0, 3)) > 1e-6 || + std::abs(sample.boardInCamera.at(1, 3)) > 1e-6 || + std::abs(sample.boardInCamera.at(2, 3)) > 1e-6) { + hasBoardPose = true; + break; + } + for (int r = 0; r < 3 && !hasBoardPose; ++r) { + for (int c = 0; c < 3; ++c) { + const double expected = (r == c) ? 1.0 : 0.0; + if (std::abs(sample.boardInCamera.at(r, c) - expected) > 1e-6) { + hasBoardPose = true; + break; + } + } + } + if (hasBoardPose) { + break; + } + } + + if (hasBoardPose) { + const HECEulerOrder robotOrder = m_dataWidget->getEulerOrder(); + + appendLog("Starting Eye-In-Hand calibration..."); + appendLog(QString("Input sample count: %1").arg(poseData.size())); + appendLog(QString("Robot Euler order: %1 (%2)") + .arg(eulerOrderToString(robotOrder)) + .arg(orderFormula(robotOrder))); + appendLog("Board pose assumption: boardInCamera, Euler order = XYZ"); + appendLog("Algorithm: Park/Tsai-Lenz with full pose data"); + + const int ret = m_calib->CalculateEyeInHandWithPose(poseData, m_currentResult); + if (ret != 0) { + appendLog(QString("Calibration failed, error code: %1").arg(ret)); + QMessageBox::critical(this, "Error", QString("Calibration failed, error code: %1").arg(ret)); + return; + } + + m_hasResult = true; + m_resultWidget->showCalibResult(m_currentResult); + appendLog(QString("Calibration succeeded, error: %1 mm") + .arg(m_currentResult.error, 0, 'f', 4)); + + struct ProbeResult { + HECEulerOrder robotOrder; + HECEulerOrder boardOrder; + bool invertBoardPose = false; + double error = 0.0; + double maxError = 0.0; + double minError = 0.0; + int ret = -1; + }; + + if (m_currentResult.error > 20.0) { + appendLog("Running exhaustive Eye-In-Hand pose diagnostics..."); + const HECEulerOrder allOrders[] = { + HECEulerOrder::XYZ, + HECEulerOrder::XZY, + HECEulerOrder::YXZ, + HECEulerOrder::YZX, + HECEulerOrder::ZXY, + HECEulerOrder::ZYX + }; + + std::vector probeResults; + probeResults.reserve(72); + + for (const HECEulerOrder probeRobotOrder : allOrders) { + for (const HECEulerOrder probeBoardOrder : allOrders) { + for (const bool invertBoardPose : { false, true }) { + std::vector probeData; + m_dataWidget->getEyeInHandPoseData( + probeData, + probeRobotOrder, + probeBoardOrder, + invertBoardPose); + if (probeData.size() < 3) { + continue; + } + + ProbeResult probe; + probe.robotOrder = probeRobotOrder; + probe.boardOrder = probeBoardOrder; + probe.invertBoardPose = invertBoardPose; + + HECCalibResult probeCalibResult; + probe.ret = m_calib->CalculateEyeInHandWithPose(probeData, probeCalibResult); + if (probe.ret == 0) { + probe.error = probeCalibResult.error; + probe.maxError = probeCalibResult.maxError; + probe.minError = probeCalibResult.minError; + } + + probeResults.push_back(probe); + } + } + } + + std::sort(probeResults.begin(), probeResults.end(), + [](const ProbeResult& lhs, const ProbeResult& rhs) { + if (lhs.ret != 0 && rhs.ret != 0) { + return false; + } + if (lhs.ret != 0) { + return false; + } + if (rhs.ret != 0) { + return true; + } + return lhs.error < rhs.error; + }); + + int loggedCount = 0; + for (const ProbeResult& probe : probeResults) { + if (probe.ret != 0) { + continue; + } + + appendLog(QString("Probe #%1: robot=%2(%3), board=%4(%5), pose=%6, err=%7 mm, max=%8, min=%9") + .arg(loggedCount + 1) + .arg(eulerOrderToString(probe.robotOrder)) + .arg(orderFormula(probe.robotOrder)) + .arg(eulerOrderToString(probe.boardOrder)) + .arg(orderFormula(probe.boardOrder)) + .arg(probe.invertBoardPose ? "cameraInBoard->inverse" : "boardInCamera") + .arg(probe.error, 0, 'f', 4) + .arg(probe.maxError, 0, 'f', 4) + .arg(probe.minError, 0, 'f', 4)); + + ++loggedCount; + if (loggedCount >= 12) { + break; + } + } + } + + updateStatusBar("Eye-In-Hand pose calibration finished"); + emit calibrationCompleted(m_currentResult); + return; + } + + std::vector calibData; + m_dataWidget->getEyeInHandData(calibData); + + if (calibData.size() < 3) { + QMessageBox::warning(this, "Warning", "At least 3 samples are required"); + return; + } + + const HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder(); + appendLog("Starting Eye-In-Hand point calibration..."); + appendLog(QString("Input sample count: %1").arg(calibData.size())); + appendLog(QString("Robot Euler order: %1 (%2)") + .arg(eulerOrderToString(eulerOrder)) + .arg(orderFormula(eulerOrder))); + appendLog("Algorithm: fixed-target point method"); + + const int ret = m_calib->CalculateEyeInHand(calibData, m_currentResult); + if (ret == 0) { + m_hasResult = true; + m_resultWidget->showCalibResult(m_currentResult); + appendLog(QString("Calibration succeeded, error: %1 mm") + .arg(m_currentResult.error, 0, 'f', 4)); + updateStatusBar("Eye-In-Hand point calibration finished"); + emit calibrationCompleted(m_currentResult); + } else { + appendLog(QString("Calibration failed, error code: %1").arg(ret)); + QMessageBox::critical(this, "Error", QString("Calibration failed, error code: %1").arg(ret)); + } +} + void CalibViewMainWindow::onTCPCalib() { if (!m_calib) {