#pragma once // ════════════════════════════════════════════════════════════════════════════ // HandEyeCalibDialog.h ─ 在线手眼标定工具 // // 功能: // 1. 采集/加载图像 + 输入 TCP 位姿 → 组成观测对 // 2. 支持 眼在手上(eye-in-hand) / 眼在手外(eye-to-hand) // 3. 3D手眼标定 AX=XB: Tsai-Lenz / Park / Horaud / Daniilidis // 4. 标定效果评价: 旋转残差、平移残差、AX-XB一致性 // 5. 导出4×4齐次变换矩阵 // ════════════════════════════════════════════════════════════════════════════ #include #include #include #include #include #include #include class CameraManager; class QLabel; class QTreeWidget; class QTreeWidgetItem; class QLineEdit; class QPushButton; class QProgressBar; class QTextEdit; class QComboBox; class QDoubleSpinBox; class QGroupBox; class QCheckBox; // ── 单条观测数据 ───────────────────────────────────────────────────────── struct HandEyeObservation { int index = 0; QString imagePath; // 当前 TOOL 的 TCP 位姿(世界或固定用户参考坐标系下) double robotX = 0, robotY = 0, robotZ = 0; // mm // ESTUN XYZABC: A/B/C 分别绕 X/Y/Z;R = Rz(C) * Ry(B) * Rx(A)。 double robotA = 0, robotB = 0, robotC = 0; // deg // 标定板检测结果 bool boardDetected = false; // 相机→标定板位姿 (由 VisionGuide 检测得到) double camPoseData[7] = {}; // VisionGuide 7-element pose // 公开数据集或函数库可直接提供4x4矩阵,避免欧拉角约定造成二次转换误差。 cv::Mat T_reference_tcp; // ^reference T_TCP,平移单位 m cv::Mat T_target2cam; // ^camera T_target,平移单位 m bool robotMatrixValid = false; bool targetMatrixValid = false; double reprojectionErrorPx = -1.0; // 在线工程采集的时间信息。时间戳由中控机与本机使用同一时钟时, // synchronizationSkewMs 才具有绝对意义;否则仍可用于追溯采集顺序。 qint64 poseTimestampMs = 0; qint64 captureTimestampMs = 0; qint64 synchronizationSkewMs = -1; QString poseSource; bool excluded = false; // 评价 double convergenceError = -1.0; QString displayName() const { return QString("obs_%1").arg(index, 3, 10, QChar('0')); } QString statusStr() const { if (excluded) return QString::fromUtf8("已排除"); if (!boardDetected) return QString::fromUtf8("未检测"); return QString::fromUtf8("有效"); } QString errorStr() const { if (convergenceError >= 0) return QString::number(convergenceError, 'f', 4) + " mm"; if (reprojectionErrorPx >= 0) return QString::number(reprojectionErrorPx, 'f', 4) + " px"; return "-"; } }; // ── 手眼标定结果 ───────────────────────────────────────────────────────── struct HandEyeResult { bool valid = false; cv::Mat R_hand_eye; // 3×3 旋转矩阵 cv::Mat t_hand_eye; // 3×1 平移向量 cv::Mat T_4x4; // 4×4 齐次变换矩阵 double rotationError = -1.0; // 平均旋转残差 (deg) double translationError = -1.0; // 平均平移残差 (mm) double convergenceRMS = -1.0; // AX-XB 一致性 RMS double closureRotationRMS = -1.0; // 闭环固定坐标系姿态 RMS (deg) double closureTranslationRMS = -1.0; // 闭环固定坐标系位置 RMS (mm) double groundTruthRotationError = -1.0; // deg double groundTruthTranslationError = -1.0; // mm bool qualityPassed = false; QString qualityMessage; int observationCount = 0; QString transformConvention; QString algorithmName; QString handEyeType; // "eye_in_hand" or "eye_to_hand" QString dataSource; }; // ════════════════════════════════════════════════════════════════════════════ class HandEyeCalibDialog : public QDialog { Q_OBJECT public: explicit HandEyeCalibDialog(CameraManager* camMgr, QWidget* parent = nullptr); ~HandEyeCalibDialog(); bool hasResult() const { return m_result.valid; } QString resultFilePath() const { return m_resultFile; } HandEyeResult result() const { return m_result; } static QString defaultSaveDir(); public slots: // 工程接口:中控机函数库收到测量时刻 TCP 位姿后调用本槽,并在同一次调用中拍照。 // 位姿约定为 ^reference T_TCP,平移 mm,旋转为 ESTUN ABC(deg): // R = Rz(C) * Ry(B) * Rx(A)(内禀 XYZ / 外禀 ZYX)。 // 从非GUI线程调用时会自动排队到本对象线程执行。 void captureSynchronizedObservation(double xMm, double yMm, double zMm, double aDeg, double bDeg, double cDeg, qint64 poseTimestampMs = 0); signals: void calibrationDone(const QString& matrixFile); void observationCaptureFinished(bool success, int observationIndex, const QString& message); private slots: // 配置 void onBrowseCaltab(); void onBrowseSaveDir(); void onLoadCamParam(); // 采集 void onCaptureAndInput(); void onLoadImageBatch(); void onLoadRobotPosesFile(); void onLoadPublicDataset(); void onEditRobotPose(); // 列表操作 void onObsClicked(QTreeWidgetItem* item, int col); void onExcludeSelected(); void onIncludeSelected(); void onDeleteSelected(); // 检测 void onValidateAll(); // 标定 void onCalibrate(); void onCompareAlgorithms(); void onComputeErrors(); void onSave(); private: void buildUi(); void setupConnections(); void refreshList(); void displayObs(int idx); void displayImageOnLabel(QLabel* lbl, const QImage& img); void log(const QString& msg); void setProgress(int pct, const QString& msg = {}); bool validateSingleObs(int idx); bool runCalibration(const QList& usedIndices); bool solveCalibration(const QList& usedIndices, int algorithm, const QString& algorithmName, HandEyeResult& result, QString* errorMessage = nullptr) const; bool solveVisionCalibration(const QList& usedIndices, HandEyeResult& result, QString* errorMessage = nullptr); bool importRwheDataset(const QString& datasetDir); bool observationTransforms(const HandEyeObservation& obs, cv::Mat& T_reference_tcp, cv::Mat& T_target2cam) const; bool captureObservation(const cv::Mat& T_reference_tcp, qint64 poseTimestampMs, const QString& poseSource, QString* errorMessage = nullptr); bool validateObservationSet(const QList& usedIndices, QString& report) const; void invalidateResult(const QString& reason = {}); void clearValidationDatasetContext(); void evaluateAXXB(const QList& usedIndices, HandEyeResult& result) const; void evaluateClosureConsistency(const QList& usedIndices, HandEyeResult& result) const; void displayCalibrationResult(const HandEyeResult& result); bool exportComparisonCsv(const QList& results, QString& pathOut) const; bool exportObservationCsv(QString& pathOut) const; void computePerPoseErrors(const QList& usedIndices); // VisionGuide 标定板检测 → 相机位姿 bool detectBoardPose(const QImage& img, double poseOut[7], double* reprojectionErrorPx = nullptr); void updateBoardParameterUi(); void invalidateBoardDetections(const QString& reason); // 辅助: VisionGuide pose ↔ OpenCV R,t 转换 static bool VisionGuidePoseToRt(const double pose[7], cv::Mat& R, cv::Mat& t); static void estunPoseToRt(double x, double y, double z, double a, double b, double c, cv::Mat& R, cv::Mat& t); static void matrixToEstunPose(const cv::Mat& T, double& x, double& y, double& z, double& a, double& b, double& c); static bool isRigidTransform(const cv::Mat& T, QString* reason = nullptr); static QString transformConventionForType(const QString& handEyeType); // 评价指标计算 void computeAXXBConsistency(const QList& usedIndices); CameraManager* m_camMgr; QString m_saveDir; QList m_observations; HandEyeResult m_result; QString m_resultFile; // Top widgets QLineEdit* m_editCaltab = nullptr; QLineEdit* m_editSaveDir = nullptr; QLineEdit* m_editCamParam = nullptr; QComboBox* m_comboHandEyeType = nullptr; QComboBox* m_comboBoardType = nullptr; QDoubleSpinBox* m_spinFindSigma = nullptr; QDoubleSpinBox* m_spinFindAlpha = nullptr; QDoubleSpinBox* m_spinGapTolerance = nullptr; QDoubleSpinBox* m_spinMaxDiamMarks = nullptr; QCheckBox* m_checkSkipFindCaltab = nullptr; // Robot pose input QDoubleSpinBox* m_spinRX = nullptr; QDoubleSpinBox* m_spinRY = nullptr; QDoubleSpinBox* m_spinRZ = nullptr; QDoubleSpinBox* m_spinRA = nullptr; QDoubleSpinBox* m_spinRB = nullptr; QDoubleSpinBox* m_spinRC = nullptr; // Middle QTreeWidget* m_treeObs = nullptr; QLabel* m_labelObsCount = nullptr; QPushButton* m_btnCapture = nullptr; QPushButton* m_btnLoadImages = nullptr; QPushButton* m_btnLoadPoses = nullptr; QPushButton* m_btnLoadDataset = nullptr; QPushButton* m_btnEditPose = nullptr; QPushButton* m_btnValidateAll = nullptr; QPushButton* m_btnExclude = nullptr; QPushButton* m_btnInclude = nullptr; QPushButton* m_btnDelete = nullptr; QPushButton* m_btnCalibrate = nullptr; QPushButton* m_btnCompare = nullptr; QPushButton* m_btnComputeErrors= nullptr; QPushButton* m_btnSave = nullptr; QLabel* m_labelResult = nullptr; QProgressBar* m_progress = nullptr; QTextEdit* m_textLog = nullptr; // Bottom QLabel* m_labelPreview = nullptr; QLabel* m_labelDataset = nullptr; // 相机参数 (VisionGuide) struct Impl; std::unique_ptr m_d; };