280 lines
11 KiB
C++
280 lines
11 KiB
C++
#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 <QDialog>
|
|||
|
|
#include <QList>
|
|||
|
|
#include <QImage>
|
|||
|
|
#include <QString>
|
|||
|
|
#include <QtGlobal>
|
|||
|
|
#include <memory>
|
|||
|
|
#include <opencv2/core.hpp>
|
|||
|
|
|
|||
|
|
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<int>& usedIndices);
|
|||
|
|
bool solveCalibration(const QList<int>& usedIndices,
|
|||
|
|
int algorithm,
|
|||
|
|
const QString& algorithmName,
|
|||
|
|
HandEyeResult& result,
|
|||
|
|
QString* errorMessage = nullptr) const;
|
|||
|
|
bool solveVisionCalibration(const QList<int>& 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<int>& usedIndices,
|
|||
|
|
QString& report) const;
|
|||
|
|
void invalidateResult(const QString& reason = {});
|
|||
|
|
void clearValidationDatasetContext();
|
|||
|
|
void evaluateAXXB(const QList<int>& usedIndices, HandEyeResult& result) const;
|
|||
|
|
void evaluateClosureConsistency(const QList<int>& usedIndices,
|
|||
|
|
HandEyeResult& result) const;
|
|||
|
|
void displayCalibrationResult(const HandEyeResult& result);
|
|||
|
|
bool exportComparisonCsv(const QList<HandEyeResult>& results, QString& pathOut) const;
|
|||
|
|
bool exportObservationCsv(QString& pathOut) const;
|
|||
|
|
void computePerPoseErrors(const QList<int>& 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<int>& usedIndices);
|
|||
|
|
|
|||
|
|
CameraManager* m_camMgr;
|
|||
|
|
|
|||
|
|
QString m_saveDir;
|
|||
|
|
QList<HandEyeObservation> 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<Impl> m_d;
|
|||
|
|
};
|