Files
VisionGuide/source/HandEyeCalibDialog.h
T
2026-10-26 10:56:56 +08:00

280 lines
11 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#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;
};