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;
|
||
};
|